Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 09:13:32 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

drivers/modules: changes after mavlink_log change

Browse Source 
The mavlink_log API changes lead to changes in all drivers/modules using
it.
This commit is contained in:
Julian Oes
2016-03-15 18:25:02 +00:00
parent b49b012d35
commit bba0d0384d
62 changed files with 731 additions and 831 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/camera_trigger/camera_trigger.cpp
+2 -1
View File
@@ -51,6 +51,8 @@
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/uORB.h>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/sensor_combined.h>
@@ -59,7 +61,6 @@
#include <poll.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
#include <mavlink/mavlink_log.h>
#include <board_config.h>
#define TRIGGER_PIN_DEFAULT 1
src/drivers/md25/md25.cpp
+2 -2
View File
@@ -50,7 +50,7 @@
#include <systemlib/err.h>
#include <arch/board/board.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/debug_key_value.h>
@@ -80,7 +80,7 @@ enum {
};
// File descriptors
static int mavlink_fd;
static orb_advert_t mavlink_log_pub;
MD25::MD25(const char *deviceName, int bus,
uint16_t address, uint32_t speed) :
src/drivers/px4fmu/fmu.cpp
+3 -3
View File
@@ -2517,18 +2517,18 @@ void
PX4FMU::dsm_bind_ioctl(int dsmMode)
{
if (!_armed.armed) {
// mavlink_log_info(_mavlink_fd, "[FMU] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8"));
// mavlink_log_info(&_mavlink_log_pub, "[FMU] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8"));
warnx("[FMU] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8"));
int ret = ioctl(nullptr, DSM_BIND_START,
(dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES));
if (ret) {
// mavlink_log_critical(_mavlink_fd, "binding failed.");
// mavlink_log_critical(&_mavlink_log_pub, "binding failed.");
warnx("binding failed.");
}
} else {
// mavlink_log_info(_mavlink_fd, "[FMU] system armed, bind request rejected");
// mavlink_log_info(&_mavlink_log_pub, "[FMU] system armed, bind request rejected");
warnx("[FMU] system armed, bind request rejected");
}
}
src/drivers/px4io/px4io.cpp
+18 -25
View File
@@ -73,6 +73,7 @@
#include <systemlib/scheduling_priorities.h>
#include <systemlib/param/param.h>
#include <systemlib/circuit_breaker.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
@@ -92,7 +93,6 @@
#include <debug.h>
#include <mavlink/mavlink_log.h>
#include <modules/px4iofirmware/protocol.h>
#include "uploader.h"
@@ -264,7 +264,7 @@ private:
volatile int _task; ///< worker task id
volatile bool _task_should_exit; ///< worker terminate flag
int _mavlink_fd; ///< mavlink file descriptor.
orb_advert_t _mavlink_log_pub; ///< mavlink log pub
perf_counter_t _perf_update; ///< local performance counter for status updates
perf_counter_t _perf_write; ///< local performance counter for PWM control writes
@@ -505,7 +505,7 @@ PX4IO::PX4IO(device::Device *interface) :
_rc_last_valid(0),
_task(-1),
_task_should_exit(false),
_mavlink_fd(-1),
_mavlink_log_pub(nullptr),
_perf_update(perf_alloc(PC_ELAPSED, "io update")),
_perf_write(perf_alloc(PC_ELAPSED, "io write")),
_perf_sample_latency(perf_alloc(PC_ELAPSED, "io latency")),
@@ -604,7 +604,7 @@ PX4IO::detect()
} else {
DEVICE_LOG("IO version error");
mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!");
mavlink_log_emergency(&_mavlink_log_pub, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!");
}
return -1;
@@ -662,12 +662,12 @@ PX4IO::init()
/* if the error still persists after timing out, we give up */
if (protocol == _io_reg_get_error) {
mavlink_and_console_log_emergency(_mavlink_fd, "Failed to communicate with IO, abort.");
mavlink_and_console_log_emergency(&_mavlink_log_pub, "Failed to communicate with IO, abort.");
return -1;
}
if (protocol != PX4IO_PROTOCOL_VERSION) {
mavlink_and_console_log_emergency(_mavlink_fd, "IO protocol/firmware mismatch, abort.");
mavlink_and_console_log_emergency(&_mavlink_log_pub, "IO protocol/firmware mismatch, abort.");
return -1;
}
@@ -684,7 +684,7 @@ PX4IO::init()
(_max_rc_input < 1) || (_max_rc_input > 255)) {
DEVICE_LOG("config read error");
mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort.");
mavlink_log_emergency(&_mavlink_log_pub, "[IO] config read fail, abort.");
return -1;
}
@@ -722,7 +722,7 @@ PX4IO::init()
/* get a status update from IO */
io_get_status();
mavlink_and_console_log_emergency(_mavlink_fd, "RECOVERING FROM FMU IN-AIR RESTART");
mavlink_and_console_log_emergency(&_mavlink_log_pub, "RECOVERING FROM FMU IN-AIR RESTART");
/* WARNING: COMMANDER app/vehicle status must be initialized.
* If this fails (or the app is not started), worst-case IO
@@ -751,7 +751,7 @@ PX4IO::init()
/* abort after 5s */
if ((hrt_absolute_time() - try_start_time) / 1000 > 3000) {
mavlink_and_console_log_emergency(_mavlink_fd, "Failed to recover from in-air restart (1), abort");
mavlink_and_console_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (1), abort");
return 1;
}
@@ -807,7 +807,7 @@ PX4IO::init()
/* abort after 5s */
if ((hrt_absolute_time() - try_start_time) / 1000 > 2000) {
mavlink_and_console_log_emergency(_mavlink_fd, "Failed to recover from in-air restart (2), abort");
mavlink_and_console_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (2), abort");
return 1;
}
@@ -854,7 +854,7 @@ PX4IO::init()
ret = io_set_rc_config();
if (ret != OK) {
mavlink_and_console_log_critical(_mavlink_fd, "IO RC config upload fail");
mavlink_and_console_log_critical(&_mavlink_log_pub, "IO RC config upload fail");
return ret;
}
}
@@ -911,8 +911,6 @@ PX4IO::task_main()
hrt_abstime poll_last = 0;
hrt_abstime orb_check_last = 0;
_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
@@ -1027,11 +1025,6 @@ PX4IO::task_main()
/* run at 5Hz */
orb_check_last = now;
/* try to claim the MAVLink log FD */
if (_mavlink_fd < 0) {
_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
}
/* check updates on uORB topics and handle it */
bool updated = false;
@@ -1087,7 +1080,7 @@ PX4IO::task_main()
int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1);
if (pret != OK) {
mavlink_and_console_log_critical(_mavlink_fd, "IO vscale upload failed");
mavlink_and_console_log_critical(&_mavlink_log_pub, "IO vscale upload failed");
}
/* send RC throttle failsafe value to IO */
@@ -1104,7 +1097,7 @@ PX4IO::task_main()
pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1);
if (pret != OK) {
mavlink_and_console_log_critical(_mavlink_fd, "failsafe upload failed, FS: %d us", (int)failsafe_thr);
mavlink_and_console_log_critical(&_mavlink_log_pub, "failsafe upload failed, FS: %d us", (int)failsafe_thr);
}
}
}
@@ -1546,7 +1539,7 @@ PX4IO::io_set_rc_config()
/* check the IO initialisation flag */
if (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
mavlink_and_console_log_critical(_mavlink_fd, "config for RC%d rejected by IO", i + 1);
mavlink_and_console_log_critical(&_mavlink_log_pub, "config for RC%d rejected by IO", i + 1);
break;
}
@@ -1619,16 +1612,16 @@ void
PX4IO::dsm_bind_ioctl(int dsmMode)
{
if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
mavlink_log_info(_mavlink_fd, "[IO] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8"));
mavlink_log_info(&_mavlink_log_pub, "[IO] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8"));
int ret = ioctl(nullptr, DSM_BIND_START,
(dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES));
if (ret) {
mavlink_log_critical(_mavlink_fd, "binding failed.");
mavlink_log_critical(&_mavlink_log_pub, "binding failed.");
}
} else {
mavlink_log_info(_mavlink_fd, "[IO] system armed, bind request rejected");
mavlink_log_info(&_mavlink_log_pub, "[IO] system armed, bind request rejected");
}
}
@@ -2148,7 +2141,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
} while (retries > 0);
if (retries == 0) {
mavlink_and_console_log_info(_mavlink_fd, "[IO] mixer upload fail");
mavlink_and_console_log_info(&_mavlink_log_pub, "[IO] mixer upload fail");
/* load must have failed for some reason */
return -EINVAL;
src/drivers/roboclaw/RoboClaw.cpp
+1 -1
View File
@@ -50,8 +50,8 @@
#include <termios.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <arch/board/board.h>
#include <mavlink/mavlink_log.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_controls_0.h>
src/lib/runway_takeoff/RunwayTakeoff.cpp
+5 -5
View File
@@ -45,7 +45,7 @@
#include "RunwayTakeoff.h"
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
namespace runwaytakeoff
@@ -92,7 +92,7 @@ void RunwayTakeoff::init(float yaw, double current_lat, double current_lon)
}
void RunwayTakeoff::update(float airspeed, float alt_agl,
double current_lat, double current_lon, int mavlink_fd)
double current_lat, double current_lon, orb_advert_t *mavlink_log_pub)
{
switch (_state) {
@@ -106,7 +106,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl,
case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
if (airspeed > _airspeed_min.get() * _min_airspeed_scaling.get()) {
_state = RunwayTakeoffState::TAKEOFF;
mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached");
mavlink_log_info(mavlink_log_pub, "#Takeoff airspeed reached");
}
break;
@@ -124,7 +124,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl,
_start_wp(1) = (float)current_lon;
}
mavlink_log_info(mavlink_fd, "#Climbout");
mavlink_log_info(mavlink_log_pub, "#Climbout");
}
break;
@@ -133,7 +133,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl,
if (alt_agl > _climbout_diff.get()) {
_climbout = false;
_state = RunwayTakeoffState::FLY;
mavlink_log_info(mavlink_fd, "#Navigating to waypoint");
mavlink_log_info(mavlink_log_pub, "#Navigating to waypoint");
}
break;
src/lib/runway_takeoff/RunwayTakeoff.h
+2 -2
View File
@@ -48,7 +48,7 @@
#include <drivers/drv_hrt.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
namespace runwaytakeoff
@@ -69,7 +69,7 @@ public:
~RunwayTakeoff();
void init(float yaw, double current_lat, double current_lon);
void update(float airspeed, float alt_agl, double current_lat, double current_lon, int mavlink_fd);
void update(float airspeed, float alt_agl, double current_lat, double current_lon, orb_advert_t *mavlink_log_pub);
RunwayTakeoffState getState() { return _state; };
bool isInitialized() { return _initialized; };
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+7 -16
View File
@@ -68,12 +68,12 @@
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/geo/geo.h>
#include <lib/ecl/validation/data_validator_group.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[]);
@@ -192,7 +192,7 @@ private:
bool _vibration_warning = false;
bool _ext_hdg_good = false;
int _mavlink_fd = -1;
orb_advert_t _mavlink_log_pub = nullptr;
perf_counter_t _update_perf;
perf_counter_t _loop_perf;
@@ -326,15 +326,6 @@ void AttitudeEstimatorQ::task_main()
while (!_task_should_exit) {
int ret = px4_poll(fds, 1, 1000);
#ifndef __PX4_QURT
if (_mavlink_fd < 0) {
/* TODO: This call currently stalls the thread on QURT */
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
#endif
if (ret < 0) {
// Poll error, sleep and try again
usleep(10000);
@@ -422,7 +413,7 @@ void AttitudeEstimatorQ::task_main()
if (_voter_gyro.failover_count() > 0) {
_failsafe = true;
flags = _voter_gyro.failover_state();
mavlink_and_console_log_emergency(_mavlink_fd, "Gyro #%i failure :%s%s%s%s%s!",
mavlink_and_console_log_emergency(&_mavlink_log_pub, "Gyro #%i failure :%s%s%s%s%s!",
_voter_gyro.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
@@ -434,7 +425,7 @@ void AttitudeEstimatorQ::task_main()
if (_voter_accel.failover_count() > 0) {
_failsafe = true;
flags = _voter_accel.failover_state();
mavlink_and_console_log_emergency(_mavlink_fd, "Accel #%i failure :%s%s%s%s%s!",
mavlink_and_console_log_emergency(&_mavlink_log_pub, "Accel #%i failure :%s%s%s%s%s!",
_voter_accel.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
@@ -446,7 +437,7 @@ void AttitudeEstimatorQ::task_main()
if (_voter_mag.failover_count() > 0) {
_failsafe = true;
flags = _voter_mag.failover_state();
mavlink_and_console_log_emergency(_mavlink_fd, "Mag #%i failure :%s%s%s%s%s!",
mavlink_and_console_log_emergency(&_mavlink_log_pub, "Mag #%i failure :%s%s%s%s%s!",
_voter_mag.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
@@ -456,7 +447,7 @@ void AttitudeEstimatorQ::task_main()
}
if (_failsafe) {
mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
mavlink_and_console_log_emergency(&_mavlink_log_pub, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
}
}
@@ -469,7 +460,7 @@ void AttitudeEstimatorQ::task_main()
} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
_vibration_warning = true;
mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d",
mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d",
(int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
(int)(100 * _voter_accel.get_vibration_factor(curr_time)),
(int)(100 * _voter_mag.get_vibration_factor(curr_time)));
src/modules/bottle_drop/bottle_drop.cpp
+19 -21
View File
@@ -67,10 +67,10 @@
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <geo/geo.h>
#include <dataman/dataman.h>
#include <mathlib/mathlib.h>
#include <mavlink/mavlink_log.h>
/**
@@ -113,7 +113,7 @@ public:
private:
bool _task_should_exit; /**< if true, task should exit */
int _main_task; /**< handle for task */
int _mavlink_fd;
orb_advert_t _mavlink_log_pub;
int _command_sub;
int _wind_estimate_sub;
@@ -174,7 +174,6 @@ BottleDrop::BottleDrop() :
_task_should_exit(false),
_main_task(-1),
_mavlink_fd(-1),
_command_sub(-1),
_wind_estimate_sub(-1),
_command {},
@@ -341,8 +340,7 @@ void
BottleDrop::task_main()
{
_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(_mavlink_fd, "[bottle_drop] started");
mavlink_log_info(&_mavlink_log_pub, "[bottle_drop] started");
_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate));
@@ -533,7 +531,7 @@ BottleDrop::task_main()
approach_error = _wrap_pi(ground_direction - approach_direction);
if (counter % 90 == 0) {
mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real,
mavlink_log_info(&_mavlink_log_pub, "drop distance %u, heading error %u", (unsigned)distance_real,
(unsigned)math::degrees(approach_error));
}
}
@@ -638,7 +636,7 @@ BottleDrop::task_main()
float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat,
flight_vector_e.lon);
mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real,
mavlink_log_critical(&_mavlink_log_pub, "position set, approach heading: %u", (unsigned)distance_real,
(unsigned)math::degrees(approach_direction + M_PI_F));
_drop_state = DROP_STATE_TARGET_SET;
@@ -662,7 +660,7 @@ BottleDrop::task_main()
fabsf(approach_error) < math::radians(20.0f)) {
open_bay();
_drop_state = DROP_STATE_BAY_OPEN;
mavlink_log_info(_mavlink_fd, "#audio: opening bay");
mavlink_log_info(&_mavlink_log_pub, "#audio: opening bay");
}
}
}
@@ -680,7 +678,7 @@ BottleDrop::task_main()
(distance_real < precision) && ((distance_real < future_distance))) {
drop();
_drop_state = DROP_STATE_DROPPED;
mavlink_log_info(_mavlink_fd, "#audio: payload dropped");
mavlink_log_info(&_mavlink_log_pub, "#audio: payload dropped");
} else {
@@ -704,7 +702,7 @@ BottleDrop::task_main()
_drop_approval = false;
lock_release();
close_bay();
mavlink_log_info(_mavlink_fd, "#audio: closing bay");
mavlink_log_info(&_mavlink_log_pub, "#audio: closing bay");
// remove onboard mission
_onboard_mission.current_seq = -1;
@@ -751,16 +749,16 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) {
open_bay();
drop();
mavlink_log_critical(_mavlink_fd, "drop bottle");
mavlink_log_critical(&_mavlink_log_pub, "drop bottle");
} else if (cmd->param1 > 0.5f) {
open_bay();
mavlink_log_critical(_mavlink_fd, "opening bay");
mavlink_log_critical(&_mavlink_log_pub, "opening bay");
} else {
lock_release();
close_bay();
mavlink_log_critical(_mavlink_fd, "closing bay");
mavlink_log_critical(&_mavlink_log_pub, "closing bay");
}
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
@@ -771,12 +769,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
switch ((int)(cmd->param1 + 0.5f)) {
case 0:
_drop_approval = false;
mavlink_log_critical(_mavlink_fd, "got drop position, no approval");
mavlink_log_critical(&_mavlink_log_pub, "got drop position, no approval");
break;
case 1:
_drop_approval = true;
mavlink_log_critical(_mavlink_fd, "got drop position and approval");
mavlink_log_critical(&_mavlink_log_pub, "got drop position and approval");
break;
default:
@@ -791,7 +789,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
_target_position.lon = cmd->param6;
_target_position.alt = cmd->param7;
_drop_state = DROP_STATE_TARGET_VALID;
mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat,
mavlink_log_info(&_mavlink_log_pub, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat,
(double)_target_position.lon, (double)_target_position.alt);
map_projection_init(&ref, _target_position.lat, _target_position.lon);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
@@ -820,7 +818,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
case 1:
_drop_approval = true;
mavlink_log_info(_mavlink_fd, "#audio: got drop approval");
mavlink_log_info(&_mavlink_log_pub, "#audio: got drop approval");
break;
default:
@@ -846,19 +844,19 @@ BottleDrop::answer_command(struct vehicle_command_s *cmd, unsigned result)
break;
case vehicle_command_s::VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(_mavlink_fd, "command denied: %u", cmd->command);
mavlink_log_critical(&_mavlink_log_pub, "command denied: %u", cmd->command);
break;
case vehicle_command_s::VEHICLE_CMD_RESULT_FAILED:
mavlink_log_critical(_mavlink_fd, "command failed: %u", cmd->command);
mavlink_log_critical(&_mavlink_log_pub, "command failed: %u", cmd->command);
break;
case vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(_mavlink_fd, "command temporarily rejected: %u", cmd->command);
mavlink_log_critical(&_mavlink_log_pub, "command temporarily rejected: %u", cmd->command);
break;
case vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED:
mavlink_log_critical(_mavlink_fd, "command unsupported: %u", cmd->command);
mavlink_log_critical(&_mavlink_log_pub, "command unsupported: %u", cmd->command);
break;
default:
src/modules/commander/PreflightCheck.cpp
+39 -50
View File
@@ -54,6 +54,7 @@
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/rc_check.h>
#include <systemlib/mavlink_log.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
@@ -65,7 +66,6 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <mavlink/mavlink_log.h>
#include "PreflightCheck.h"
@@ -84,7 +84,7 @@ static int check_calibration(DevHandle &h, const char* param_template, int &devi
int ret = h.ioctl(SENSORIOCCALTEST, 0);
calibration_found = (ret == OK);
devid = h.ioctl(DEVIOCGDEVICEID, 0);
char s[20];
@@ -116,7 +116,7 @@ static int check_calibration(DevHandle &h, const char* param_template, int &devi
return !calibration_found;
}
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -128,8 +128,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
if (!h.isValid()) {
if (!optional) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
}
}
@@ -140,8 +139,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
if (ret) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
}
success = false;
goto out;
@@ -151,8 +149,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
if (ret != OK) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
}
success = false;
goto out;
@@ -163,7 +160,7 @@ out:
return success;
}
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
{
bool success = true;
@@ -175,8 +172,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (!h.isValid()) {
if (!optional) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
}
}
@@ -187,8 +183,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (ret) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
}
success = false;
goto out;
@@ -198,8 +193,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (ret != OK) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u TEST FAILED: %d", instance, ret);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u TEST FAILED: %d", instance, ret);
}
success = false;
goto out;
@@ -217,7 +211,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
}
/* this is frickin' fatal */
success = false;
@@ -225,7 +219,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
}
} else {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL READ");
}
/* this is frickin' fatal */
success = false;
@@ -239,7 +233,7 @@ out:
return success;
}
static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -251,8 +245,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (!h.isValid()) {
if (!optional) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
}
}
@@ -263,8 +256,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (ret) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
}
success = false;
goto out;
@@ -274,8 +266,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (ret != OK) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
}
success = false;
goto out;
@@ -286,7 +277,7 @@ out:
return success;
}
static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -298,8 +289,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (!h.isValid()) {
if (!optional) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
}
}
@@ -312,8 +302,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
// int ret = check_calibration(fd, "CAL_BARO%u_ID");
// if (ret) {
// mavlink_and_console_log_critical(mavlink_fd,
// "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance);
// mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance);
// success = false;
// goto out;
// }
@@ -324,7 +313,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
return success;
}
static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail)
{
bool success = true;
int ret;
@@ -335,7 +324,7 @@ static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
(hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
}
success = false;
goto out;
@@ -343,7 +332,7 @@ static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
if (fabsf(airspeed.confidence) < 0.99f) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR COMM ERROR");
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR COMM ERROR");
}
success = false;
goto out;
@@ -351,7 +340,7 @@ static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
mavlink_and_console_log_critical(mavlink_log_pub, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
}
// XXX do not make this fatal yet
}
@@ -361,7 +350,7 @@ out:
return success;
}
static bool gnssCheck(int mavlink_fd, bool report_fail)
static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail)
{
bool success = true;
@@ -385,7 +374,7 @@ static bool gnssCheck(int mavlink_fd, bool report_fail)
//Report failure to detect module
if (!success) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
}
}
@@ -393,7 +382,7 @@ static bool gnssCheck(int mavlink_fd, bool report_fail)
return success;
}
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, bool checkGyro,
bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures)
{
bool failed = false;
@@ -409,7 +398,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_mag_count);
int device_id = -1;
if (!magnometerCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
if (!magnometerCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -421,7 +410,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary compass not found");
mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary compass not found");
}
failed = true;
}
@@ -438,7 +427,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_accel_count);
int device_id = -1;
if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id, reportFailures) && required) {
if (!accelerometerCheck(mavlink_log_pub, i, !required, checkDynamic, device_id, reportFailures) && required) {
failed = true;
}
@@ -450,7 +439,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary accelerometer not found");
mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary accelerometer not found");
}
failed = true;
}
@@ -467,7 +456,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_gyro_count);
int device_id = -1;
if (!gyroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
if (!gyroCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -479,7 +468,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary gyro not found");
mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary gyro not found");
}
failed = true;
}
@@ -496,7 +485,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_baro_count);
int device_id = -1;
if (!baroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
if (!baroCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -506,10 +495,10 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
}
// TODO there is no logic in place to calibrate the primary baro yet
// // check if the primary device is present
// // check if the primary device is present
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "warning: primary barometer not operational");
mavlink_and_console_log_critical(mavlink_log_pub, "warning: primary barometer not operational");
}
failed = true;
}
@@ -517,16 +506,16 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* ---- AIRSPEED ---- */
if (checkAirspeed) {
if (!airspeedCheck(mavlink_fd, true, reportFailures)) {
if (!airspeedCheck(mavlink_log_pub, true, reportFailures)) {
failed = true;
}
}
/* ---- RC CALIBRATION ---- */
if (checkRC) {
if (rc_calibration_check(mavlink_fd, reportFailures) != OK) {
if (rc_calibration_check(mavlink_log_pub, reportFailures) != OK) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "RC calibration check failed");
mavlink_and_console_log_critical(mavlink_log_pub, "RC calibration check failed");
}
failed = true;
}
@@ -534,7 +523,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* ---- Global Navigation Satellite System receiver ---- */
if (checkGNSS) {
if (!gnssCheck(mavlink_fd, reportFailures)) {
if (!gnssCheck(mavlink_log_pub, reportFailures)) {
failed = true;
}
}
src/modules/commander/PreflightCheck.h
+3 -3
View File
@@ -49,8 +49,8 @@ namespace Commander
* The function won't fail the test if optional sensors are not found, however,
* it will fail the test if optional sensors are found but not in working condition.
*
* @param mavlink_fd
* Mavlink output file descriptor for feedback when a sensor fails
* @param mavlink_log_pub
* Mavlink output orb handle reference for feedback when a sensor fails
* @param checkMag
* true if the magneteometer should be checked
* @param checkAcc
@@ -66,7 +66,7 @@ namespace Commander
* @param checkGNSS
* true if the GNSS receiver should be checked
**/
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc,
bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc,
bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures = false);
const unsigned max_mandatory_gyro_count = 1;
src/modules/commander/accelerometer_calibration.cpp
+32 -32
View File
@@ -145,7 +145,7 @@
#include <conversion/rotation.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/vehicle_attitude.h>
/* oddly, ERROR is not defined for c++ */
@@ -160,26 +160,26 @@ static int32_t device_id[max_accel_sens];
static int device_prio_max = 0;
static int32_t device_id_primary = 0;
calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
int mat_invert3(float src[3][3], float dst[3][3]);
calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g);
/// Data passed to calibration worker routine
typedef struct {
int mavlink_fd;
orb_advert_t *mavlink_log_pub;
unsigned done_count;
int subs[max_accel_sens];
float accel_ref[max_accel_sens][detect_orientation_side_count][3];
} accel_worker_data_t;
int do_accel_calibration(int mavlink_fd)
int do_accel_calibration(orb_advert_t *mavlink_log_pub)
{
#ifndef __PX4_QURT
int fd;
#endif
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
struct accel_calibration_s accel_scale;
accel_scale.x_offset = 0.0f;
@@ -210,7 +210,7 @@ int do_accel_calibration(int mavlink_fd)
px4_close(fd);
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
}
#else
(void)sprintf(str, "CAL_ACC%u_XOFF", s);
@@ -252,7 +252,7 @@ int do_accel_calibration(int mavlink_fd)
/* measure and calculate offsets & scales */
if (res == OK) {
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors);
if (cal_return == calibrate_return_cancelled) {
// Cancel message already displayed, nothing left to do
return ERROR;
@@ -265,7 +265,7 @@ int do_accel_calibration(int mavlink_fd)
if (res != OK) {
if (active_sensors == 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
}
return ERROR;
}
@@ -325,7 +325,7 @@ int do_accel_calibration(int mavlink_fd)
failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i])));
if (failed) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, i);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, i);
return ERROR;
}
@@ -334,7 +334,7 @@ int do_accel_calibration(int mavlink_fd)
fd = px4_open(str, 0);
if (fd < 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "sensor does not exist");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist");
res = ERROR;
} else {
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
@@ -342,7 +342,7 @@ int do_accel_calibration(int mavlink_fd)
}
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, i);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, i);
}
#endif
}
@@ -352,16 +352,16 @@ int do_accel_calibration(int mavlink_fd)
res = param_save_default();
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
}
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
@@ -375,22 +375,22 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien
const unsigned samples_num = 750;
accel_worker_data_t* worker_data = (accel_worker_data_t*)(data);
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation));
mavlink_and_console_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation));
read_accelerometer_avg(worker_data->subs, worker_data->accel_ref, orientation, samples_num);
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side result: [%8.4f %8.4f %8.4f]", detect_orientation_str(orientation),
mavlink_and_console_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%8.4f %8.4f %8.4f]", detect_orientation_str(orientation),
(double)worker_data->accel_ref[0][orientation][0],
(double)worker_data->accel_ref[0][orientation][1],
(double)worker_data->accel_ref[0][orientation][2]);
worker_data->done_count++;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
mavlink_and_console_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
return calibrate_return_ok;
}
calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
{
calibrate_return result = calibrate_return_ok;
@@ -398,7 +398,7 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
accel_worker_data_t worker_data;
worker_data.mavlink_fd = mavlink_fd;
worker_data.mavlink_log_pub = mavlink_log_pub;
worker_data.done_count = 0;
bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false };
@@ -448,7 +448,7 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_fd, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */);
result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */);
calibrate_cancel_unsubscribe(cancel_sub);
}
@@ -471,7 +471,7 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
if (result != calibrate_return_ok) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: calibration calculation error");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: calibration calculation error");
break;
}
}
@@ -631,7 +631,7 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref
return calibrate_return_ok;
}
int do_level_calibration(int mavlink_fd) {
int do_level_calibration(orb_advert_t *mavlink_log_pub) {
const unsigned cal_time = 5;
const unsigned cal_hz = 100;
unsigned settle_time = 30;
@@ -641,7 +641,7 @@ int do_level_calibration(int mavlink_fd) {
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "level");
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "level");
param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF");
param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF");
@@ -676,7 +676,7 @@ int do_level_calibration(int mavlink_fd) {
// sleep for some time
hrt_abstime start = hrt_absolute_time();
while(hrt_elapsed_time(&start) < settle_time * 1000000) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (int)(90*hrt_elapsed_time(&start)/1e6f/(float)settle_time));
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (int)(90*hrt_elapsed_time(&start)/1e6f/(float)settle_time));
sleep(settle_time / 10);
}
@@ -687,8 +687,8 @@ int do_level_calibration(int mavlink_fd) {
if (pollret <= 0) {
// attitude estimator is not running
mavlink_and_console_log_critical(mavlink_fd, "attitude estimator not running - check system boot");
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level");
mavlink_and_console_log_critical(mavlink_log_pub, "attitude estimator not running - check system boot");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
goto out;
}
@@ -698,21 +698,21 @@ int do_level_calibration(int mavlink_fd) {
counter++;
}
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
if (counter > (cal_time * cal_hz / 2 )) {
roll_mean /= counter;
pitch_mean /= counter;
} else {
mavlink_and_console_log_info(mavlink_fd, "not enough measurements taken");
mavlink_and_console_log_info(mavlink_log_pub, "not enough measurements taken");
success = false;
goto out;
}
if (fabsf(roll_mean) > 0.8f ) {
mavlink_and_console_log_critical(mavlink_fd, "excess roll angle");
mavlink_and_console_log_critical(mavlink_log_pub, "excess roll angle");
} else if (fabsf(pitch_mean) > 0.8f ) {
mavlink_and_console_log_critical(mavlink_fd, "excess pitch angle");
mavlink_and_console_log_critical(mavlink_log_pub, "excess pitch angle");
} else {
roll_mean *= (float)M_RAD_TO_DEG;
pitch_mean *= (float)M_RAD_TO_DEG;
@@ -723,13 +723,13 @@ int do_level_calibration(int mavlink_fd) {
out:
if (success) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level");
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level");
return 0;
} else {
// set old parameters
param_set(roll_offset_handle, &roll_offset_current);
param_set(pitch_offset_handle, &pitch_offset_current);
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
return 1;
}
}
src/modules/commander/accelerometer_calibration.h
+3 -2
View File
@@ -43,8 +43,9 @@
#define ACCELEROMETER_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_accel_calibration(int mavlink_fd);
int do_level_calibration(int mavlink_fd);
int do_accel_calibration(orb_advert_t *mavlink_log_pub);
int do_level_calibration(orb_advert_t *mavlink_log_pub);
#endif /* ACCELEROMETER_CALIBRATION_H_ */
src/modules/commander/airspeed_calibration.cpp
+35 -35
View File
@@ -52,7 +52,7 @@
#include <drivers/drv_airspeed.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/differential_pressure.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/airspeed.h>
@@ -65,20 +65,20 @@ static const int ERROR = -1;
static const char *sensor_name = "dpress";
static void feedback_calibration_failed(int mavlink_fd)
static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub)
{
sleep(5);
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
}
int do_airspeed_calibration(int mavlink_fd)
int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
{
int result = OK;
unsigned calibration_counter = 0;
const unsigned maxcount = 2400;
/* give directions */
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
const unsigned calibration_count = (maxcount * 2) / 3;
@@ -101,12 +101,12 @@ int do_airspeed_calibration(int mavlink_fd)
paramreset_successful = true;
} else {
mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] airspeed offset zero failed");
}
px4_close(fd);
}
int cancel_sub = calibrate_cancel_subscribe();
if (!paramreset_successful) {
@@ -115,26 +115,26 @@ int do_airspeed_calibration(int mavlink_fd)
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
goto error_return;
}
/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
}
mavlink_and_console_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
usleep(500 * 1000);
while (calibration_counter < calibration_count) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
@@ -149,12 +149,12 @@ int do_airspeed_calibration(int mavlink_fd)
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_fd);
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
}
@@ -167,14 +167,14 @@ int do_airspeed_calibration(int mavlink_fd)
airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) {
if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed");
}
px4_close(fd_scale);
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
@@ -183,28 +183,28 @@ int do_airspeed_calibration(int mavlink_fd)
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
goto error_return;
}
} else {
feedback_calibration_failed(mavlink_fd);
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
mavlink_and_console_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
mavlink_and_console_log_critical(mavlink_fd, "[cal] Create airflow now");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Create airflow now");
calibration_counter = 0;
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) {
goto error_return;
}
@@ -222,7 +222,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 500 == 0) {
mavlink_and_console_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
@@ -230,56 +230,56 @@ int do_airspeed_calibration(int mavlink_fd)
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_and_console_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_and_console_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Swap static and dynamic ports!");
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
/* save */
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0);
(void)param_save_default();
feedback_calibration_failed(mavlink_fd);
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
} else {
mavlink_and_console_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_fd);
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
}
if (calibration_counter == maxcount) {
feedback_calibration_failed(mavlink_fd);
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
tune_neutral(true);
normal_return:
calibrate_cancel_unsubscribe(cancel_sub);
px4_close(diff_pres_sub);
// This give a chance for the log messages to go out of the queue before someone else stomps on then
sleep(1);
return result;
error_return:
result = ERROR;
goto normal_return;
src/modules/commander/airspeed_calibration.h
+2 -1
View File
@@ -40,7 +40,8 @@
#define AIRSPEED_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_airspeed_calibration(int mavlink_fd);
int do_airspeed_calibration(orb_advert_t *mavlink_log_pub);
#endif /* AIRSPEED_CALIBRATION_H_ */
src/modules/commander/baro_calibration.cpp
+2 -2
View File
@@ -44,7 +44,7 @@
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_baro.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/param/param.h>
/* oddly, ERROR is not defined for c++ */
@@ -53,7 +53,7 @@
#endif
static const int ERROR = -1;
int do_baro_calibration(int mavlink_fd)
int do_baro_calibration(orb_advert_t *mavlink_log_pub)
{
// TODO implement this
return ERROR;
src/modules/commander/baro_calibration.h
+2 -1
View File
@@ -40,7 +40,8 @@
#define BARO_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_baro_calibration(int mavlink_fd);
int do_baro_calibration(orb_advert_t *mavlink_log_pub);
#endif /* BARO_CALIBRATION_H_ */
src/modules/commander/calibration_routines.cpp
+73 -73
View File
@@ -47,7 +47,7 @@
#include <float.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <geo/geo.h>
#include <string.h>
@@ -233,10 +233,10 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
return 0;
}
enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub, int accel_sub, bool lenient_still_position)
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, int cancel_sub, int accel_sub, bool lenient_still_position)
{
const unsigned ndim = 3;
struct sensor_combined_s sensor;
float accel_ema[ndim] = { 0.0f }; // exponential moving average of accel
float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; // max-hold dispersion of accel
@@ -245,11 +245,11 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2
hrt_abstime still_time = lenient_still_position ? 500000 : 1300000; // still time required in us
px4_pollfd_struct_t fds[1];
fds[0].fd = accel_sub;
fds[0].events = POLLIN;
hrt_abstime t_start = hrt_absolute_time();
/* set timeout to 30s */
hrt_abstime timeout = 30000000;
@@ -257,22 +257,22 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
hrt_abstime t = t_start;
hrt_abstime t_prev = t_start;
hrt_abstime t_still = 0;
unsigned poll_errcount = 0;
while (true) {
/* wait blocking for new data */
int poll_ret = px4_poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(sensor_combined), accel_sub, &sensor);
t = hrt_absolute_time();
float dt = (t - t_prev) / 1000000.0f;
t_prev = t;
float w = dt / ema_len;
for (unsigned i = 0; i < ndim; i++) {
float di = 0.0f;
switch (i) {
case 0:
@@ -285,21 +285,21 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
di = sensor.accelerometer_m_s2[2];
break;
}
float d = di - accel_ema[i];
accel_ema[i] += d * w;
d = d * d;
accel_disp[i] = accel_disp[i] * (1.0f - w);
if (d > still_thr2 * 8.0f) {
d = still_thr2 * 8.0f;
}
if (d > accel_disp[i]) {
accel_disp[i] = d;
}
}
/* still detector with hysteresis */
if (accel_disp[0] < still_thr2 &&
accel_disp[1] < still_thr2 &&
@@ -307,10 +307,10 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
/* is still now */
if (t_still == 0) {
/* first time */
mavlink_and_console_log_info(mavlink_fd, "[cal] detected rest position, hold still...");
mavlink_and_console_log_info(mavlink_log_pub, "[cal] detected rest position, hold still...");
t_still = t;
t_timeout = t + timeout;
} else {
/* still since t_still */
if (t > t_still + still_time) {
@@ -318,28 +318,28 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
break;
}
}
} else if (accel_disp[0] > still_thr2 * 4.0f ||
accel_disp[1] > still_thr2 * 4.0f ||
accel_disp[2] > still_thr2 * 4.0f) {
/* not still, reset still start time */
if (t_still != 0) {
mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
mavlink_and_console_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
usleep(200000);
t_still = 0;
}
}
} else if (poll_ret == 0) {
poll_errcount++;
}
if (t > t_timeout) {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
return DETECT_ORIENTATION_ERROR;
}
}
@@ -349,39 +349,39 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
fabsf(accel_ema[2]) < accel_err_thr) {
return DETECT_ORIENTATION_TAIL_DOWN; // [ g, 0, 0 ]
}
if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return DETECT_ORIENTATION_NOSE_DOWN; // [ -g, 0, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return DETECT_ORIENTATION_LEFT; // [ 0, g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return DETECT_ORIENTATION_RIGHT; // [ 0, -g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
return DETECT_ORIENTATION_UPSIDE_DOWN; // [ 0, 0, g ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
return DETECT_ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ]
}
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: invalid orientation");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: invalid orientation");
return DETECT_ORIENTATION_ERROR; // Can't detect orientation
}
@@ -396,11 +396,11 @@ const char* detect_orientation_str(enum detect_orientation_return orientation)
"down", // right-side up
"error"
};
return rgOrientationStrs[orientation];
}
calibrate_return calibrate_from_orientation(int mavlink_fd,
calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
int cancel_sub,
bool side_data_collected[detect_orientation_side_count],
calibration_from_orientation_worker_t calibration_worker,
@@ -408,94 +408,94 @@ calibrate_return calibrate_from_orientation(int mavlink_fd,
bool lenient_still_position)
{
calibrate_return result = calibrate_return_ok;
// Setup subscriptions to onboard accel sensor
int sub_accel = orb_subscribe(ORB_ID(sensor_combined));
if (sub_accel < 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "No onboard accel");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "No onboard accel");
return calibrate_return_error;
}
unsigned orientation_failures = 0;
// Rotate through all requested orientation
while (true) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) {
result = calibrate_return_cancelled;
break;
}
if (orientation_failures > 4) {
result = calibrate_return_error;
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "timeout: no motion");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "timeout: no motion");
break;
}
unsigned int side_complete_count = 0;
// Update the number of completed sides
for (unsigned i = 0; i < detect_orientation_side_count; i++) {
if (side_data_collected[i]) {
side_complete_count++;
}
}
if (side_complete_count == detect_orientation_side_count) {
// We have completed all sides, move on
break;
}
/* inform user which orientations are still needed */
char pendingStr[256];
pendingStr[0] = 0;
for (unsigned int cur_orientation=0; cur_orientation<detect_orientation_side_count; cur_orientation++) {
if (!side_data_collected[cur_orientation]) {
strcat(pendingStr, " ");
strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation));
}
}
mavlink_and_console_log_info(mavlink_fd, "[cal] pending:%s", pendingStr);
mavlink_and_console_log_info(mavlink_fd, "[cal] hold vehicle still on a pending side");
enum detect_orientation_return orient = detect_orientation(mavlink_fd, cancel_sub, sub_accel, lenient_still_position);
mavlink_and_console_log_info(mavlink_log_pub, "[cal] pending:%s", pendingStr);
mavlink_and_console_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side");
enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, cancel_sub, sub_accel, lenient_still_position);
if (orient == DETECT_ORIENTATION_ERROR) {
orientation_failures++;
mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
mavlink_and_console_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
continue;
}
/* inform user about already handled side */
if (side_data_collected[orient]) {
orientation_failures++;
mavlink_and_console_log_critical(mavlink_fd, "%s side already completed", detect_orientation_str(orient));
mavlink_and_console_log_critical(mavlink_fd, "rotate to a pending side");
mavlink_and_console_log_critical(mavlink_log_pub, "%s side already completed", detect_orientation_str(orient));
mavlink_and_console_log_critical(mavlink_log_pub, "rotate to a pending side");
continue;
}
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
orientation_failures = 0;
// Call worker routine
result = calibration_worker(orient, cancel_sub, worker_data);
if (result != calibrate_return_ok ) {
break;
}
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
// Note that this side is complete
side_data_collected[orient] = true;
tune_neutral(true);
usleep(200000);
}
if (sub_accel >= 0) {
px4_close(sub_accel);
}
return result;
}
@@ -509,24 +509,24 @@ void calibrate_cancel_unsubscribe(int cmd_sub)
orb_unsubscribe(cmd_sub);
}
static void calibrate_answer_command(int mavlink_fd, struct vehicle_command_s &cmd, unsigned result)
static void calibrate_answer_command(orb_advert_t *mavlink_log_pub, struct vehicle_command_s &cmd, unsigned result)
{
switch (result) {
case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED:
tune_positive(true);
break;
case vehicle_command_s::VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(mavlink_fd, "command denied during calibration: %u", cmd.command);
mavlink_log_critical(mavlink_log_pub, "command denied during calibration: %u", cmd.command);
tune_negative(true);
break;
default:
break;
}
}
bool calibrate_cancel_check(int mavlink_fd, int cancel_sub)
bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, int cancel_sub)
{
px4_pollfd_struct_t fds[1];
fds[0].fd = cancel_sub;
@@ -535,9 +535,9 @@ bool calibrate_cancel_check(int mavlink_fd, int cancel_sub)
if (px4_poll(&fds[0], 1, 0) > 0) {
struct vehicle_command_s cmd;
memset(&cmd, 0, sizeof(cmd));
orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION &&
(int)cmd.param1 == 0 &&
(int)cmd.param2 == 0 &&
@@ -545,13 +545,13 @@ bool calibrate_cancel_check(int mavlink_fd, int cancel_sub)
(int)cmd.param4 == 0 &&
(int)cmd.param5 == 0 &&
(int)cmd.param6 == 0) {
calibrate_answer_command(mavlink_fd, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
mavlink_log_critical(mavlink_fd, CAL_QGC_CANCELLED_MSG);
calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG);
return true;
} else {
calibrate_answer_command(mavlink_fd, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
}
}
return false;
}
src/modules/commander/calibration_routines.h
+3 -3
View File
@@ -74,7 +74,7 @@ static const unsigned detect_orientation_side_count = 6;
/// Wait for vehicle to become still and detect it's orientation
/// @return Returns detect_orientation_return according to orientation when vehicle
/// and ready for measurements
enum detect_orientation_return detect_orientation(int mavlink_fd, ///< Mavlink fd to write output to
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to
int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe
int accel_sub, ///< Orb subcription to accel sensor
bool lenient_still_detection); ///< true: Use more lenient still position detection
@@ -95,7 +95,7 @@ typedef calibrate_return (*calibration_from_orientation_worker_t)(detect_orienta
/// Perform calibration sequence which require a rest orientation detection prior to calibration.
/// @return OK: Calibration succeeded, ERROR: Calibration failed
calibrate_return calibrate_from_orientation(int mavlink_fd, ///< Mavlink fd to write output to
calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to
int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe
bool side_data_collected[detect_orientation_side_count], ///< Sides for which data still needs calibration
calibration_from_orientation_worker_t calibration_worker, ///< Worker routine which performs the actual calibration
@@ -111,5 +111,5 @@ int calibrate_cancel_subscribe(void);
void calibrate_cancel_unsubscribe(int cancel_sub);
/// Used to periodically check for a cancel command
bool calibrate_cancel_check(int mavlink_fd, ///< Mavlink fd for output
bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to
int cancel_sub); ///< Cancel subcription fromcalibration_cancel_subscribe
src/modules/commander/commander.cpp
+111 -133
View File
File diff suppressed because it is too large Load Diff
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+1 -1
View File
@@ -288,7 +288,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, nullptr /* no mavlink_log_pub */);
// Validate result of transition
ut_assert(test->assertMsg, test->expected_transition_result == result);
src/modules/commander/esc_calibration.cpp
+40 -40
View File
@@ -57,7 +57,7 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/uORB.h>
#include <drivers/drv_hrt.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -65,82 +65,82 @@
#endif
static const int ERROR = -1;
int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed)
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* armed)
{
int return_code = OK;
int fd = -1;
struct battery_status_s battery;
int batt_sub = -1;
bool batt_updated = false;
bool batt_connected = false;
hrt_abstime battery_connect_wait_timeout = 30000000;
hrt_abstime pwm_high_timeout = 10000000;
hrt_abstime timeout_start;
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "esc");
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
batt_sub = orb_subscribe(ORB_ID(battery_status));
if (batt_sub < 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Subscribe to battery");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Subscribe to battery");
goto Error;
}
// Make sure battery is disconnected
orb_copy(ORB_ID(battery_status), batt_sub, &battery);
if (battery.voltage_filtered_v > 3.0f) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
goto Error;
}
armed->in_esc_calibration_mode = true;
fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
if (fd < 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Can't open PWM device");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Can't open PWM device");
goto Error;
}
/* tell IO/FMU that its ok to disable its safety with the switch */
if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to disable safety switch");
goto Error;
}
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to arm system");
goto Error;
}
/* tell IO to switch off safety without using the safety switch */
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to force safety off");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to disable safety switch");
goto Error;
}
mavlink_and_console_log_info(mavlink_fd, "[cal] Connect battery now");
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) {
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to arm system");
goto Error;
}
/* tell IO to switch off safety without using the safety switch */
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) {
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to force safety off");
goto Error;
}
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Connect battery now");
timeout_start = hrt_absolute_time();
while (true) {
// We are either waiting for the user to connect the battery. Or we are waiting to let the PWM
// sit high.
hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout;
if (hrt_absolute_time() - timeout_start > timeout_wait) {
if (!batt_connected) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
goto Error;
}
// PWM was high long enough
break;
}
if (!batt_connected) {
orb_check(batt_sub, &batt_updated);
if (batt_updated) {
@@ -149,7 +149,7 @@ int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed)
// Battery is connected, signal to user and start waiting again
batt_connected = true;
timeout_start = hrt_absolute_time();
mavlink_and_console_log_info(mavlink_fd, "[cal] Battery connected");
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Battery connected");
}
}
}
@@ -162,24 +162,24 @@ Out:
}
if (fd != -1) {
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Safety switch still off");
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Safety switch still off");
}
if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Servos still armed");
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Servos still armed");
}
if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Safety switch still deactivated");
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Safety switch still deactivated");
}
px4_close(fd);
}
armed->in_esc_calibration_mode = false;
if (return_code == OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "esc");
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
}
return return_code;
Error:
return_code = ERROR;
goto Out;
src/modules/commander/esc_calibration.h
+2 -2
View File
@@ -44,6 +44,6 @@
#include <uORB/topics/actuator_armed.h>
int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed);
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* armed);
#endif
#endif
src/modules/commander/gyro_calibration.cpp
+18 -18
View File
@@ -53,7 +53,7 @@
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_gyro.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/mcu_version.h>
@@ -70,7 +70,7 @@ static const unsigned max_gyros = 3;
/// Data passed to calibration worker routine
typedef struct {
int mavlink_fd;
orb_advert_t *mavlink_log_pub;
int32_t device_id[max_gyros];
int gyro_sensor_sub[max_gyros];
struct gyro_calibration_s gyro_scale[max_gyros];
@@ -96,7 +96,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
/* use first gyro to pace, but count correctly per-gyro for statistics */
while (calibration_counter[0] < calibration_count) {
if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) {
return calibrate_return_cancelled;
}
@@ -122,7 +122,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
}
if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) {
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count);
mavlink_and_console_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count);
}
}
@@ -131,14 +131,14 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
}
if (poll_errcount > 1000) {
mavlink_and_console_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
mavlink_and_console_log_critical(worker_data->mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
return calibrate_return_error;
}
}
for (unsigned s = 0; s < max_gyros; s++) {
if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) {
mavlink_and_console_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s)
mavlink_and_console_log_critical(worker_data->mavlink_log_pub, "[cal] ERROR: missing data, sensor %d", s)
return calibrate_return_error;
}
@@ -150,14 +150,14 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
return calibrate_return_ok;
}
int do_gyro_calibration(int mavlink_fd)
int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
{
int res = OK;
gyro_worker_data_t worker_data = {};
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
worker_data.mavlink_fd = mavlink_fd;
worker_data.mavlink_log_pub = mavlink_log_pub;
struct gyro_calibration_s gyro_scale_zero;
gyro_scale_zero.x_offset = 0.0f;
@@ -180,7 +180,7 @@ int do_gyro_calibration(int mavlink_fd)
(void)sprintf(str, "CAL_GYRO%u_ID", s);
res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s);
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Unable to reset CAL_GYRO%u_ID", s);
return ERROR;
}
@@ -195,7 +195,7 @@ int do_gyro_calibration(int mavlink_fd)
px4_close(fd);
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
return ERROR;
}
}
@@ -294,7 +294,7 @@ int do_gyro_calibration(int mavlink_fd)
fabsf(ydiff) > maxoff ||
fabsf(zdiff) > maxoff) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] motion, retrying..");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] motion, retrying..");
res = ERROR;
} else {
@@ -306,7 +306,7 @@ int do_gyro_calibration(int mavlink_fd)
} while (res == ERROR && try_count <= max_tries);
if (try_count >= max_tries) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: Motion during calibration");
res = ERROR;
}
@@ -351,14 +351,14 @@ int do_gyro_calibration(int mavlink_fd)
px4_close(fd);
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, 1);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, 1);
}
#endif
}
}
if (failed) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to set offset params");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: failed to set offset params");
res = ERROR;
}
}
@@ -375,7 +375,7 @@ int do_gyro_calibration(int mavlink_fd)
res = param_save_default();
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
}
}
@@ -383,9 +383,9 @@ int do_gyro_calibration(int mavlink_fd)
usleep(200000);
if (res == OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
} else {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
src/modules/commander/gyro_calibration.h
+2 -1
View File
@@ -40,7 +40,8 @@
#define GYRO_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_gyro_calibration(int mavlink_fd);
int do_gyro_calibration(orb_advert_t *mavlink_log_pub);
#endif /* GYRO_CALIBRATION_H_ */
src/modules/commander/mag_calibration.cpp
+39 -48
View File
@@ -56,7 +56,7 @@
#include <drivers/drv_gyro.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_mag.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@@ -80,11 +80,11 @@ bool internal[max_mags];
int device_prio_max = 0;
int32_t device_id_primary = 0;
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t (&device_ids)[max_mags]);
/// Data passed to calibration worker routine
typedef struct {
int mavlink_fd;
orb_advert_t *mavlink_log_pub;
unsigned done_count;
int sub_mag[max_mags];
unsigned int calibration_points_perside;
@@ -92,15 +92,15 @@ typedef struct {
uint64_t calibration_interval_perside_useconds;
unsigned int calibration_counter_total[max_mags];
bool side_data_collected[detect_orientation_side_count];
float* x[max_mags];
float* y[max_mags];
float* z[max_mags];
float *x[max_mags];
float *y[max_mags];
float *z[max_mags];
} mag_worker_data_t;
int do_mag_calibration(int mavlink_fd)
int do_mag_calibration(orb_advert_t *mavlink_log_pub)
{
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
struct mag_calibration_s mscale_null;
mscale_null.x_offset = 0.0f;
@@ -126,7 +126,7 @@ int do_mag_calibration(int mavlink_fd)
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
break;
}
#else
@@ -179,7 +179,7 @@ int do_mag_calibration(int mavlink_fd)
result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
if (result != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, cur_mag);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, cur_mag);
}
/* calibrate range */
@@ -187,7 +187,7 @@ int do_mag_calibration(int mavlink_fd)
result = px4_ioctl(fd, MAGIOCCALIBRATE, fd);
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, "[cal] Skipped scale calibration, sensor %u", cur_mag);
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Skipped scale calibration, sensor %u", cur_mag);
/* this is non-fatal - mark it accordingly */
result = OK;
}
@@ -199,7 +199,7 @@ int do_mag_calibration(int mavlink_fd)
// Calibrate all mags at the same time
if (result == OK) {
switch (mag_calibrate_all(mavlink_fd, device_ids)) {
switch (mag_calibrate_all(mavlink_log_pub, device_ids)) {
case calibrate_return_cancelled:
// Cancel message already displayed, we're done here
result = ERROR;
@@ -213,16 +213,16 @@ int do_mag_calibration(int mavlink_fd)
usleep(200000);
if (result == OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
break;
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
}
// Fall through
default:
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
break;
}
}
@@ -263,8 +263,8 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
mag_worker_data_t* worker_data = (mag_worker_data_t*)(data);
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation");
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
mavlink_and_console_log_info(worker_data->mavlink_log_pub, "[cal] Rotate vehicle around the detected orientation");
mavlink_and_console_log_info(worker_data->mavlink_log_pub, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
/*
* Detect if the system is rotating.
@@ -289,7 +289,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
fabsf(gyro_z_integral) < gyro_int_thresh_rad) {
/* abort on request */
if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) {
result = calibrate_return_cancelled;
px4_close(sub_gyro);
return result;
@@ -299,7 +299,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
if (hrt_absolute_time() > detection_deadline) {
result = calibrate_return_error;
warnx("int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral);
mavlink_and_console_log_critical(worker_data->mavlink_fd, "Failed: This calibration requires rotation.");
mavlink_and_console_log_critical(worker_data->mavlink_log_pub, "Failed: This calibration requires rotation.");
break;
}
@@ -339,7 +339,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter_side < worker_data->calibration_points_perside) {
if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) {
result = calibrate_return_cancelled;
break;
}
@@ -394,7 +394,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
calibration_counter_side++;
// Progress indicator for side
mavlink_and_console_log_info(worker_data->mavlink_fd,
mavlink_and_console_log_info(worker_data->mavlink_log_pub,
"[cal] %s side calibration: progress <%u>",
detect_orientation_str(orientation), progress_percentage(worker_data) +
(unsigned)((100 / calibration_sides) * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
@@ -405,28 +405,28 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
if (poll_errcount > worker_data->calibration_points_perside * 3) {
result = calibrate_return_error;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
mavlink_and_console_log_info(worker_data->mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
break;
}
}
if (result == calibrate_return_ok) {
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation));
mavlink_and_console_log_info(worker_data->mavlink_log_pub, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation));
worker_data->done_count++;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, progress_percentage(worker_data));
mavlink_and_console_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, progress_percentage(worker_data));
}
return result;
}
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t (&device_ids)[max_mags])
{
calibrate_return result = calibrate_return_ok;
mag_worker_data_t worker_data;
worker_data.mavlink_fd = mavlink_fd;
worker_data.mavlink_log_pub = mavlink_log_pub;
worker_data.done_count = 0;
worker_data.calibration_points_perside = calibration_total_points / calibration_sides;
worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides;
@@ -460,7 +460,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: out of memory");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: out of memory");
result = calibrate_return_error;
}
}
@@ -512,7 +512,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
// Mag in this slot is available
unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside;
//mavlink_and_console_log_info(mavlink_fd, "Orb interval %u msecs", orb_interval_msecs);
//mavlink_and_console_log_info(mavlink_log_pub, "Orb interval %u msecs", orb_interval_msecs);
orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs);
}
}
@@ -522,7 +522,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_fd, // Mavlink fd to write output
result = calibrate_from_orientation(mavlink_log_pub, // uORB handle to write output
cancel_sub, // Subscription to vehicle_command for cancel support
worker_data.side_data_collected, // Sides to calibrate
mag_calibration_worker, // Calibration worker
@@ -559,15 +559,15 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
&sphere_radius[cur_mag]);
if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
mavlink_and_console_log_emergency(mavlink_fd, "ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
mavlink_and_console_log_emergency(mavlink_log_pub, "ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
result = calibrate_return_error;
}
if (fabsf(sphere_x[cur_mag]) > MAG_MAX_OFFSET_LEN ||
fabsf(sphere_y[cur_mag]) > MAG_MAX_OFFSET_LEN ||
fabsf(sphere_z[cur_mag]) > MAG_MAX_OFFSET_LEN) {
mavlink_and_console_log_info(mavlink_fd, "WARNING: Large offsets %s mag", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
mavlink_and_console_log_info(mavlink_fd, "offsets: %8.4f, %8.4f, %8.4f, #%u", (double)sphere_x[cur_mag],
mavlink_and_console_log_emergency(mavlink_log_pub, "ERROR: Replace %s mag fault", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
mavlink_and_console_log_info(mavlink_log_pub, "Excessive offsets: %8.4f, %8.4f, %8.4f, #%u", (double)sphere_x[cur_mag],
(double)sphere_y[cur_mag], (double)sphere_z[cur_mag], cur_mag);
result = calibrate_return_ok;
}
@@ -639,13 +639,13 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
fd_mag = px4_open(str, 0);
if (fd_mag < 0) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: unable to open mag device #%u", cur_mag);
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: unable to open mag device #%u", cur_mag);
result = calibrate_return_error;
}
if (result == calibrate_return_ok) {
if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
result = calibrate_return_error;
}
}
@@ -658,7 +658,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
#ifndef __PX4_QURT
if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
result = calibrate_return_error;
}
#endif
@@ -674,15 +674,6 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
if (result == calibrate_return_ok) {
bool failed = false;
PX4_DEBUG("found offset %d: x: %.6f, y: %.6f, z: %.6f", cur_mag,
(double)mscale.x_offset,
(double)mscale.y_offset,
(double)mscale.z_offset);
PX4_DEBUG("found scale %d: x: %.6f, y: %.6f, z: %.6f", cur_mag,
(double)mscale.x_scale,
(double)mscale.y_scale,
(double)mscale.z_scale);
/* set parameters */
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
@@ -705,14 +696,14 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
#endif
if (failed) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
result = calibrate_return_error;
} else {
mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
mavlink_and_console_log_info(mavlink_log_pub, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
cur_mag,
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
#ifndef __PX4_QURT
mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
mavlink_and_console_log_info(mavlink_log_pub, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
cur_mag,
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
#endif
src/modules/commander/mag_calibration.h
+2 -1
View File
@@ -40,7 +40,8 @@
#define MAG_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_mag_calibration(int mavlink_fd);
int do_mag_calibration(orb_advert_t *mavlink_log_pub);
#endif /* MAG_CALIBRATION_H_ */
src/modules/commander/rc_calibration.cpp
+5 -5
View File
@@ -46,7 +46,7 @@
#include <unistd.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@@ -56,7 +56,7 @@
#endif
static const int ERROR = -1;
int do_trim_calibration(int mavlink_fd)
int do_trim_calibration(orb_advert_t *mavlink_log_pub)
{
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
usleep(400000);
@@ -65,7 +65,7 @@ int do_trim_calibration(int mavlink_fd)
orb_check(sub_man, &changed);
if (!changed) {
mavlink_log_critical(mavlink_fd, "no inputs, aborting");
mavlink_log_critical(mavlink_log_pub, "no inputs, aborting");
return ERROR;
}
@@ -98,12 +98,12 @@ int do_trim_calibration(int mavlink_fd)
int save_ret = param_save_default();
if (save_ret != 0 || p1r != 0 || p2r != 0 || p3r != 0) {
mavlink_log_critical(mavlink_fd, "TRIM: PARAM SET FAIL");
mavlink_log_critical(mavlink_log_pub, "TRIM: PARAM SET FAIL");
px4_close(sub_man);
return ERROR;
}
mavlink_log_info(mavlink_fd, "trim cal done");
mavlink_log_info(mavlink_log_pub, "trim cal done");
px4_close(sub_man);
return OK;
}
src/modules/commander/rc_calibration.h
+2 -1
View File
@@ -40,7 +40,8 @@
#define RC_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_trim_calibration(int mavlink_fd);
int do_trim_calibration(orb_advert_t *mavlink_log_pub);
#endif /* RC_CALIBRATION_H_ */
src/modules/commander/state_machine_helper.cpp
+30 -30
View File
@@ -58,10 +58,10 @@
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
#include <mavlink/mavlink_log.h>
#include "state_machine_helper.h"
#include "commander_helper.h"
@@ -107,7 +107,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
arming_state_t new_arming_state, ///< arming state requested
struct actuator_armed_s *armed, ///< current armed status
bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none
orb_advert_t *mavlink_log_pub) ///< uORB handle for mavlink log
{
// Double check that our static arrays are still valid
ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
@@ -132,16 +132,16 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
prearm_ret = preflight_check(status, mavlink_fd, true /* pre-arm */ );
prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */ );
}
/* re-run the pre-flight check as long as sensors are failing */
if (!status->condition_system_sensors_initialized
if (!status->condition_system_sensors_initialized
&& (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
|| new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) {
prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */);
prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */);
status->condition_system_sensors_initialized = !prearm_ret;
last_preflight_check = hrt_absolute_time();
last_prearm_ret = prearm_ret;
@@ -193,7 +193,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// Fail transition if we need safety switch press
} else if (safety->safety_switch_available && !safety->safety_off) {
mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Press safety switch first!");
feedback_provided = true;
valid_transition = false;
}
@@ -204,7 +204,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// Fail transition if power is not good
if (!status->condition_power_input_valid) {
mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Connect power module.");
feedback_provided = true;
valid_transition = false;
}
@@ -214,14 +214,14 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
// Check avionics rail voltages
if (status->avionics_power_rail_voltage < 4.5f) {
mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
} else if (status->avionics_power_rail_voltage < 4.9f) {
mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
} else if (status->avionics_power_rail_voltage > 5.4f) {
mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
}
}
@@ -245,15 +245,15 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
if (status->condition_system_sensors_initialized) {
mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot before arming");
mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot before arming");
} else {
mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check failed, refusing to arm");
}
feedback_provided = true;
} else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
status->condition_system_sensors_initialized) {
mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot to complete");
mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot to complete");
feedback_provided = true;
} else {
// Silent ignore
@@ -265,10 +265,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) &&
(!status->condition_system_sensors_initialized)) {
if ((!status->condition_system_prearm_error_reported &&
status->condition_system_hotplug_timeout) ||
if ((!status->condition_system_prearm_error_reported &&
status->condition_system_hotplug_timeout) ||
(new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
mavlink_and_console_log_critical(mavlink_log_pub, "Not ready to fly: Sensors need inspection");
status->condition_system_prearm_error_reported = true;
}
feedback_provided = true;
@@ -299,7 +299,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
if (ret == TRANSITION_DENIED) {
/* print to MAVLink and console if we didn't provide any feedback yet */
if (!feedback_provided) {
mavlink_and_console_log_critical(mavlink_fd, "INVAL: %s - %s", state_names[status->arming_state], state_names[new_arming_state]);
mavlink_and_console_log_critical(mavlink_log_pub, "INVAL: %s - %s", state_names[status->arming_state], state_names[new_arming_state]);
}
}
@@ -397,7 +397,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
/**
* Transition from one hil state to another
*/
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, orb_advert_t *mavlink_log_pub)
{
transition_result_t ret = TRANSITION_DENIED;
@@ -408,7 +408,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
switch (new_state) {
case vehicle_status_s::HIL_STATE_OFF:
/* we're in HIL and unexpected things can happen if we disable HIL now */
mavlink_and_console_log_critical(mavlink_fd, "Not switching off HIL (safety)");
mavlink_and_console_log_critical(mavlink_log_pub, "Not switching off HIL (safety)");
ret = TRANSITION_DENIED;
break;
@@ -481,11 +481,11 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
closedir(d);
ret = TRANSITION_CHANGED;
mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state");
mavlink_and_console_log_critical(mavlink_log_pub, "Switched to ON hil state");
} else {
/* failed opening dir */
mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
mavlink_log_info(mavlink_log_pub, "FAILED LISTING DEVICE ROOT DIRECTORY");
ret = TRANSITION_DENIED;
}
@@ -542,11 +542,11 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
}
ret = TRANSITION_CHANGED;
mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state");
mavlink_and_console_log_critical(mavlink_log_pub, "Switched to ON hil state");
#endif
} else {
mavlink_and_console_log_critical(mavlink_fd, "Not switching to HIL when armed");
mavlink_and_console_log_critical(mavlink_log_pub, "Not switching to HIL when armed");
ret = TRANSITION_DENIED;
}
break;
@@ -830,13 +830,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
return status->nav_state != nav_state_old;
}
int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report)
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report)
{
/*
/*
*/
bool reportFailures = force_report || (!status->condition_system_prearm_error_reported &&
status->condition_system_hotplug_timeout);
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
@@ -844,19 +844,19 @@ int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool
checkAirspeed = true;
}
bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true,
bool preflight_ok = Commander::preflightCheck(mavlink_log_pub, true, true, true, true,
checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
!status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures);
if (!status->cb_usb && status->usb_connected && prearm) {
preflight_ok = false;
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited");
mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Flying with USB connected prohibited");
}
}
/* report once, then set the flag */
if (mavlink_fd >= 0 && reportFailures && !preflight_ok) {
if (reportFailures && !preflight_ok) {
status->condition_system_prearm_error_reported = true;
}
src/modules/commander/state_machine_helper.h
+3 -3
View File
@@ -57,14 +57,14 @@ typedef enum {
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd);
arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_state, orb_advert_t *mavlink_log_pub);
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe);
int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report=false);
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report=false);
#endif /* STATE_MACHINE_HELPER_H_ */
src/modules/ekf2/ekf2_main.cpp
+1 -1
View File
@@ -58,9 +58,9 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <mavlink/mavlink_log.h>
#include <platforms/px4_defines.h>
#include <drivers/drv_hrt.h>
#include <controllib/uorb/blocks.hpp>
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+2 -2
View File
@@ -229,7 +229,7 @@ private:
bool _newDataMag;
bool _newRangeData;
int _mavlink_fd;
orb_advert_t _mavlink_log_pub;
control::BlockParamFloat _mag_offset_x;
control::BlockParamFloat _mag_offset_y;
@@ -356,7 +356,7 @@ private:
* Runs the sensor fusion step of the filter. The parameters determine which of the sensors
* are fused with each other
**/
void updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor,
void updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor,
const bool fuseBaro, const bool fuseAirSpeed);
/**
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+8 -11
View File
@@ -63,9 +63,9 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <mavlink/mavlink_log.h>
#include <platforms/px4_defines.h>
static uint64_t IMUusec = 0;
@@ -213,7 +213,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_newDataMag(false),
_newRangeData(false),
_mavlink_fd(-1),
_mag_offset_x(this, "MAGB_X"),
_mag_offset_y(this, "MAGB_Y"),
_mag_offset_z(this, "MAGB_Z"),
@@ -405,7 +404,7 @@ int AttitudePositionEstimatorEKF::check_filter_state()
// Do not warn about accel offset if we have no position updates
if (!(warn_index == 5 && _ekf->staticMode)) {
PX4_WARN("reset: %s", feedback[warn_index]);
mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
mavlink_log_critical(&_mavlink_log_pub, "[ekf check] %s", feedback[warn_index]);
}
}
@@ -511,8 +510,6 @@ void AttitudePositionEstimatorEKF::task_main_trampoline(int argc, char *argv[])
void AttitudePositionEstimatorEKF::task_main()
{
_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
_ekf = new AttPosEKF();
if (!_ekf) {
@@ -769,7 +766,7 @@ void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp,
_local_pos.ref_timestamp = timestamp;
map_projection_init(&_pos_ref, lat, lon);
mavlink_and_console_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
mavlink_and_console_log_info(&_mavlink_log_pub, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
}
}
@@ -1442,7 +1439,7 @@ void AttitudePositionEstimatorEKF::pollData()
_voter_mag.failover_count() > 0)) {
_failsafe = true;
mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
mavlink_and_console_log_emergency(&_mavlink_log_pub, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
}
if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
@@ -1454,7 +1451,7 @@ void AttitudePositionEstimatorEKF::pollData()
} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
_vibration_warning = true;
mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d",
mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d",
(int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
(int)(100 * _voter_accel.get_vibration_factor(curr_time)),
(int)(100 * _voter_mag.get_vibration_factor(curr_time)));
@@ -1570,7 +1567,7 @@ void AttitudePositionEstimatorEKF::pollData()
//Stop dead-reckoning mode
if (_global_pos.dead_reckoning) {
mavlink_log_info(_mavlink_fd, "[ekf] stop dead-reckoning");
mavlink_log_info(&_mavlink_log_pub, "[ekf] stop dead-reckoning");
}
_global_pos.dead_reckoning = false;
@@ -1637,7 +1634,7 @@ void AttitudePositionEstimatorEKF::pollData()
if (dtLastGoodGPS >= POS_RESET_THRESHOLD) {
if (_global_pos.dead_reckoning) {
mavlink_log_info(_mavlink_fd, "[ekf] gave up dead-reckoning after long timeout");
mavlink_log_info(&_mavlink_log_pub, "[ekf] gave up dead-reckoning after long timeout");
}
_gpsIsGood = false;
@@ -1648,7 +1645,7 @@ void AttitudePositionEstimatorEKF::pollData()
else if (dtLastGoodGPS >= 0.5f) {
if (_armed.armed) {
if (!_global_pos.dead_reckoning) {
mavlink_log_info(_mavlink_fd, "[ekf] dead-reckoning enabled");
mavlink_log_info(&_mavlink_log_pub, "[ekf] dead-reckoning enabled");
}
_global_pos.dead_reckoning = true;
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+11 -17
View File
@@ -86,11 +86,11 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
#include <systemlib/mavlink_log.h>
#include <geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <mavlink/mavlink_log.h>
#include <launchdetection/LaunchDetector.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <external_lgpl/tecs/tecs.h>
@@ -151,7 +151,7 @@ public:
bool task_running() { return _task_running; }
private:
int _mavlink_fd;
orb_advert_t _mavlink_log_pub;
bool _task_should_exit; /**< if true, sensor task should exit */
bool _task_running; /**< if true, task is running in its mainloop */
@@ -506,7 +506,7 @@ FixedwingPositionControl *g_control = nullptr;
FixedwingPositionControl::FixedwingPositionControl() :
_mavlink_fd(-1),
_mavlink_log_pub(nullptr),
_task_should_exit(false),
_task_running(false),
@@ -1063,7 +1063,7 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
* for the whole landing */
if (_parameters.land_use_terrain_estimate && global_pos.terrain_alt_valid) {
if (!land_useterrain) {
mavlink_log_info(_mavlink_fd, "Landing, using terrain estimate");
mavlink_log_info(&_mavlink_log_pub, "Landing, using terrain estimate");
land_useterrain = true;
}
@@ -1400,7 +1400,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
target_bearing = _yaw;
}
mavlink_log_info(_mavlink_fd, "#Landing, heading hold");
mavlink_log_info(&_mavlink_log_pub, "#Landing, heading hold");
}
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw));
@@ -1506,7 +1506,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (!land_motor_lim) {
land_motor_lim = true;
mavlink_log_info(_mavlink_fd, "#Landing, limiting throttle");
mavlink_log_info(&_mavlink_log_pub, "#Landing, limiting throttle");
}
}
@@ -1534,7 +1534,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// just started with the flaring phase
_att_sp.pitch_body = 0.0f;
height_flare = _global_pos.alt - terrain_alt;
mavlink_log_info(_mavlink_fd, "#Landing, flaring");
mavlink_log_info(&_mavlink_log_pub, "#Landing, flaring");
land_noreturn_vertical = true;
} else {
@@ -1567,7 +1567,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
altitude_desired_rel = landing_slope_alt_rel_desired;
if (!land_onslope) {
mavlink_log_info(_mavlink_fd, "#Landing, on slope");
mavlink_log_info(&_mavlink_log_pub, "#Landing, on slope");
land_onslope = true;
}
@@ -1602,7 +1602,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
* doesn't matter if it gets reset when takeoff is detected eventually */
_takeoff_ground_alt = _global_pos.alt;
mavlink_log_info(_mavlink_fd, "#Takeoff on runway");
mavlink_log_info(&_mavlink_log_pub, "#Takeoff on runway");
}
float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos);
@@ -1613,7 +1613,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_global_pos.alt - terrain_alt,
_global_pos.lat,
_global_pos.lon,
_mavlink_fd);
&_mavlink_log_pub);
/*
* Update navigation: _runway_takeoff returns the start WP according to mode and phase.
@@ -1665,7 +1665,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
static hrt_abstime last_sent = 0;
if (hrt_absolute_time() - last_sent > 4e6) {
mavlink_log_critical(_mavlink_fd, "#Launchdetection running");
mavlink_log_critical(&_mavlink_log_pub, "#Launchdetection running");
last_sent = hrt_absolute_time();
}
@@ -2126,12 +2126,6 @@ FixedwingPositionControl::task_main()
if (fds[1].revents & POLLIN) {
perf_begin(_loop_perf);
/* XXX Hack to get mavlink output going */
if (_mavlink_fd < 0) {
/* try to open the mavlink log device every once in a while */
_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
}
/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+42 -45
View File
@@ -1,5 +1,5 @@
#include "BlockLocalPositionEstimator.hpp"
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <fcntl.h>
#include <systemlib/err.h>
#include <matrix/math.hpp>
@@ -135,9 +135,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_time_last_vision_p(0),
_time_last_mocap(0),
// mavlink log
_mavlink_fd(open(MAVLINK_LOG_DEVICE, 0)),
// initialization flags
_baroInitialized(false),
_gpsInitialized(false),
@@ -342,7 +339,7 @@ void BlockLocalPositionEstimator::update()
_x(i) = 0;
}
mavlink_log_info(_mavlink_fd, "[lpe] reinit x");
mavlink_log_info(&_mavlink_log_pub, "[lpe] reinit x");
warnx("[lpe] reinit x");
}
@@ -361,7 +358,7 @@ void BlockLocalPositionEstimator::update()
}
if (reinit_P) {
mavlink_log_info(_mavlink_fd, "[lpe] reinit P");
mavlink_log_info(&_mavlink_log_pub, "[lpe] reinit P");
warnx("[lpe] reinit P");
initP();
}
@@ -471,7 +468,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_xy > EST_SRC_TIMEOUT) {
if (!_xyTimeout) {
_xyTimeout = true;
mavlink_log_info(_mavlink_fd, "[lpe] xy timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] xy timeout ");
warnx("[lpe] xy timeout ");
}
@@ -484,7 +481,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_z > EST_SRC_TIMEOUT) {
if (!_zTimeout) {
_zTimeout = true;
mavlink_log_info(_mavlink_fd, "[lpe] z timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] z timeout ");
warnx("[lpe] z timeout ");
}
@@ -497,7 +494,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_tz > EST_SRC_TIMEOUT) {
if (!_tzTimeout) {
_tzTimeout = true;
mavlink_log_info(_mavlink_fd, "[lpe] tz timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] tz timeout ");
warnx("[lpe] tz timeout ");
}
@@ -510,7 +507,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_baro > BARO_TIMEOUT) {
if (_baroInitialized) {
_baroInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] baro timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] baro timeout ");
warnx("[lpe] baro timeout ");
}
}
@@ -518,7 +515,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_gps > GPS_TIMEOUT) {
if (_gpsInitialized) {
_gpsInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] GPS timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] GPS timeout ");
warnx("[lpe] GPS timeout ");
}
}
@@ -526,7 +523,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) {
if (_flowInitialized) {
_flowInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] flow timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] flow timeout ");
warnx("[lpe] flow timeout ");
}
}
@@ -534,7 +531,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_sonar > RANGER_TIMEOUT) {
if (_sonarInitialized) {
_sonarInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] sonar timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar timeout ");
warnx("[lpe] sonar timeout ");
}
}
@@ -542,7 +539,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_lidar > RANGER_TIMEOUT) {
if (_lidarInitialized) {
_lidarInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] lidar timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar timeout ");
warnx("[lpe] lidar timeout ");
}
}
@@ -550,7 +547,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_vision_p > VISION_TIMEOUT) {
if (_visionInitialized) {
_visionInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] vision position timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] vision position timeout ");
warnx("[lpe] vision position timeout ");
}
}
@@ -558,7 +555,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_mocap > MOCAP_TIMEOUT) {
if (_mocapInitialized) {
_mocapInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] mocap timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] mocap timeout ");
warnx("[lpe] mocap timeout ");
}
}
@@ -574,7 +571,7 @@ void BlockLocalPositionEstimator::updateHome()
// like gps and baro to be off, need to allow it
// to reset by resetting covariance
initP();
mavlink_log_info(_mavlink_fd, "[lpe] home "
mavlink_log_info(&_mavlink_log_pub, "[lpe] home "
"lat %6.2f lon %6.2f alt %5.1f m",
lat, lon, double(alt));
warnx("[lpe] home "
@@ -598,7 +595,7 @@ void BlockLocalPositionEstimator::initBaro()
if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) {
_baroAltHome = _baroStats.getMean()(0);
mavlink_log_info(_mavlink_fd,
mavlink_log_info(&_mavlink_log_pub,
"[lpe] baro init %d m std %d cm",
(int)_baroStats.getMean()(0),
(int)(100 * _baroStats.getStdDev()(0)));
@@ -643,7 +640,7 @@ void BlockLocalPositionEstimator::initGps()
_gpsAltHome = _gpsStats.getMean()(2);
map_projection_init(&_map_ref,
_gpsLatHome, _gpsLonHome);
mavlink_log_info(_mavlink_fd, "[lpe] gps init "
mavlink_log_info(&_mavlink_log_pub, "[lpe] gps init "
"lat %6.2f lon %6.2f alt %5.1f m",
_gpsLatHome,
_gpsLonHome,
@@ -691,7 +688,7 @@ void BlockLocalPositionEstimator::initLidar()
}
// not, might want to hard code this to zero
mavlink_log_info(_mavlink_fd, "[lpe] lidar init: "
mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar init: "
"mean %d cm stddev %d cm",
int(100 * _lidarStats.getMean()(0)),
int(100 * _lidarStats.getStdDev()(0)));
@@ -731,7 +728,7 @@ void BlockLocalPositionEstimator::initSonar()
}
// not, might want to hard code this to zero
mavlink_log_info(_mavlink_fd, "[lpe] sonar init "
mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar init "
"mean %d cm std %d cm",
int(100 * _sonarStats.getMean()(0)),
int(100 * _sonarStats.getStdDev()(0)));
@@ -758,7 +755,7 @@ void BlockLocalPositionEstimator::initFlow()
_time_last_flow = _timeStamp;
if (_flowQStats.getCount() > REQ_FLOW_INIT_COUNT) {
mavlink_log_info(_mavlink_fd, "[lpe] flow init: "
mavlink_log_info(&_mavlink_log_pub, "[lpe] flow init: "
"quality %d std %d",
int(_flowQStats.getMean()(0)),
int(_flowQStats.getStdDev()(0)));
@@ -785,7 +782,7 @@ void BlockLocalPositionEstimator::initVision()
if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) {
_visionHome = _visionStats.getMean();
mavlink_log_info(_mavlink_fd, "[lpe] vision position init: "
mavlink_log_info(&_mavlink_log_pub, "[lpe] vision position init: "
"%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m",
double(_visionStats.getMean()(0)),
double(_visionStats.getMean()(1)),
@@ -825,7 +822,7 @@ void BlockLocalPositionEstimator::initMocap()
if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) {
_mocapHome = _mocapStats.getMean();
mavlink_log_info(_mavlink_fd, "[lpe] mocap position init: "
mavlink_log_info(&_mavlink_log_pub, "[lpe] mocap position init: "
"%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m",
double(_mocapStats.getMean()(0)),
double(_mocapStats.getMean()(1)),
@@ -1071,7 +1068,7 @@ void BlockLocalPositionEstimator::correctFlow()
if (qual < _flow_min_q.get()) {
if (_flowFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] low flow quality %d", int(qual));
mavlink_log_info(&_mavlink_log_pub, "[lpe] low flow quality %d", int(qual));
warnx("[lpe] low flow quality %d", int(qual));
_flowFault = FAULT_SEVERE;
}
@@ -1096,7 +1093,7 @@ void BlockLocalPositionEstimator::correctFlow()
} else {
// no valid distance sensor, so return
if (_flowFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] no distance for flow");
mavlink_log_info(&_mavlink_log_pub, "[lpe] no distance for flow");
warnx("[lpe] no distance for flow");
_flowFault = FAULT_SEVERE;
}
@@ -1167,14 +1164,14 @@ void BlockLocalPositionEstimator::correctFlow()
if (beta > BETA_TABLE[n_y_flow]) {
if (_flowFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] flow fault, beta %5.2f", double(beta));
mavlink_log_info(&_mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta));
warnx("[lpe] flow fault, beta %5.2f", double(beta));
_flowFault = FAULT_MINOR;
}
} else if (_flowFault) {
_flowFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] flow OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] flow OK");
warnx("[lpe] flow OK");
}
@@ -1207,7 +1204,7 @@ void BlockLocalPositionEstimator::correctSonar()
} else if (d > max_dist) {
if (_sonarFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] sonar max distance");
mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar max distance");
warnx("[lpe] sonar max distance");
_sonarFault = FAULT_SEVERE;
}
@@ -1262,7 +1259,7 @@ void BlockLocalPositionEstimator::correctSonar()
if (_sonarFault < FAULT_MINOR) {
// avoid printing messages near ground
if (_x(X_tz) > 1.0f) {
mavlink_log_info(_mavlink_fd, "[lpe] sonar fault, beta %5.2f", double(beta));
mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar fault, beta %5.2f", double(beta));
warnx("[lpe] sonar fault, beta %5.2f", double(beta));
}
@@ -1274,7 +1271,7 @@ void BlockLocalPositionEstimator::correctSonar()
// avoid printing messages near ground
if (_x(X_tz) > 1.0f) {
mavlink_log_info(_mavlink_fd, "[lpe] sonar OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar OK");
warnx("[lpe] sonar OK");
}
}
@@ -1324,7 +1321,7 @@ void BlockLocalPositionEstimator::correctBaro()
if (beta > BETA_TABLE[n_y_baro]) {
if (_baroFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] baro fault, r %5.2f m, beta %5.2f",
mavlink_log_info(&_mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
double(r(0)), double(beta));
warnx("[lpe] baro fault, r %5.2f m, beta %5.2f",
double(r(0)), double(beta));
@@ -1333,7 +1330,7 @@ void BlockLocalPositionEstimator::correctBaro()
} else if (_baroFault) {
_baroFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] baro OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] baro OK");
warnx("[lpe] baro OK");
}
@@ -1369,7 +1366,7 @@ void BlockLocalPositionEstimator::correctLidar()
} else if (d > max_dist) {
if (_lidarFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] lidar out of range");
mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar out of range");
warnx("[lpe] lidar out of range");
_lidarFault = FAULT_SEVERE;
}
@@ -1416,7 +1413,7 @@ void BlockLocalPositionEstimator::correctLidar()
// only print message if above 1 meter, avoids
// message clutter when on ground
if (_x(X_tz) > 1.0f) {
mavlink_log_info(_mavlink_fd, "[lpe] lidar fault, r %5.2f m, beta %5.2f",
mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar fault, r %5.2f m, beta %5.2f",
double(r(0)), double(beta));
warnx("[lpe] lidar fault, r %5.2f m, beta %5.2f",
double(r(0)), double(beta));
@@ -1431,7 +1428,7 @@ void BlockLocalPositionEstimator::correctLidar()
// only print message if above 1 meter, avoids
// message clutter when on ground
if (_x(X_tz) > 1.0f) {
mavlink_log_info(_mavlink_fd, "[lpe] lidar OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar OK");
warnx("[lpe] lidar OK");
}
}
@@ -1461,7 +1458,7 @@ void BlockLocalPositionEstimator::correctGps()
if (nSat < 6 || eph > _gps_eph_max.get()) {
if (_gpsFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph));
mavlink_log_info(&_mavlink_log_pub, "[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph));
warnx("[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph));
_gpsFault = FAULT_SEVERE;
}
@@ -1554,12 +1551,12 @@ void BlockLocalPositionEstimator::correctGps()
if (beta > BETA_TABLE[n_y_gps]) {
if (_gpsFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] gps fault, beta: %5.2f", double(beta));
mavlink_log_info(&_mavlink_log_pub, "[lpe] gps fault, beta: %5.2f", double(beta));
warnx("[lpe] gps fault, beta: %5.2f", double(beta));
mavlink_log_info(_mavlink_fd, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
mavlink_log_info(&_mavlink_log_pub, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
double(r(0)), double(r(1)), double(r(2)),
double(r(3)), double(r(4)), double(r(5)));
mavlink_log_info(_mavlink_fd, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
mavlink_log_info(&_mavlink_log_pub, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
double(S_I(0, 0)), double(S_I(1, 1)), double(S_I(2, 2)),
double(S_I(3, 3)), double(S_I(4, 4)), double(S_I(5, 5)));
warnx("[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
@@ -1573,7 +1570,7 @@ void BlockLocalPositionEstimator::correctGps()
} else if (_gpsFault) {
_gpsFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] GPS OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] GPS OK");
warnx("[lpe] GPS OK");
}
@@ -1618,14 +1615,14 @@ void BlockLocalPositionEstimator::correctVision()
if (beta > BETA_TABLE[n_y_vision]) {
if (_visionFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] vision position fault, beta %5.2f", double(beta));
mavlink_log_info(&_mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta));
warnx("[lpe] vision position fault, beta %5.2f", double(beta));
_visionFault = FAULT_MINOR;
}
} else if (_visionFault) {
_visionFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] vision position OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] vision position OK");
warnx("[lpe] vision position OK");
}
@@ -1672,14 +1669,14 @@ void BlockLocalPositionEstimator::correctMocap()
if (beta > BETA_TABLE[n_y_mocap]) {
if (_mocapFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] mocap fault, beta %5.2f", double(beta));
mavlink_log_info(&_mavlink_log_pub, "[lpe] mocap fault, beta %5.2f", double(beta));
warnx("[lpe] mocap fault, beta %5.2f", double(beta));
_mocapFault = FAULT_MINOR;
}
} else if (_mocapFault) {
_mocapFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] mocap OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] mocap OK");
warnx("[lpe] mocap OK");
}
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+1 -1
View File
@@ -275,7 +275,7 @@ private:
uint64_t _time_last_sonar;
uint64_t _time_last_vision_p;
uint64_t _time_last_mocap;
int _mavlink_fd;
orb_advert_t _mavlink_log_pub;
// initialization flags
bool _baroInitialized;
src/modules/mc_pos_control/mc_pos_control_main.cpp
+4 -6
View File
@@ -79,9 +79,9 @@
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <systemlib/systemlib.h>
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <mavlink/mavlink_log.h>
#include <platforms/px4_defines.h>
#include <controllib/blocks.hpp>
@@ -123,7 +123,7 @@ public:
private:
bool _task_should_exit; /**< if true, task should exit */
int _control_task; /**< task handle for task */
int _mavlink_fd; /**< mavlink fd */
orb_advert_t _mavlink_log_pub; /**< mavlink log advert */
int _vehicle_status_sub; /**< vehicle status subscription */
int _ctrl_state_sub; /**< control state subscription */
@@ -348,7 +348,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
SuperBlock(NULL, "MPC"),
_task_should_exit(false),
_control_task(-1),
_mavlink_fd(-1),
_mavlink_log_pub(nullptr),
/* subscriptions */
_ctrl_state_sub(-1),
@@ -1146,8 +1146,6 @@ void
MulticopterPositionControl::task_main()
{
_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
/*
* do subscriptions
*/
@@ -1960,7 +1958,7 @@ MulticopterPositionControl::task_main()
&& !_control_mode.flag_control_climb_rate_enabled;
}
mavlink_log_info(_mavlink_fd, "[mpc] stopped");
mavlink_log_info(&_mavlink_log_pub, "[mpc] stopped");
_control_task = -1;
}
src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
+3 -3
View File
@@ -53,7 +53,7 @@ MulticopterPositionControlMultiplatform::MulticopterPositionControlMultiplatform
_task_should_exit(false),
_control_task(-1),
_mavlink_fd(-1),
_mavlink_log_pub(nullptr),
/* publications */
_att_sp_pub(nullptr),
@@ -232,7 +232,7 @@ MulticopterPositionControlMultiplatform::reset_pos_sp()
// - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
//XXX: port this once a mavlink like interface is available
// mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
// mavlink_log_info(&_mavlink_log_pub, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
PX4_INFO("[mpc] reset pos sp: %2.3f, %2.3f", (double)_pos_sp(0), (double)_pos_sp(1));
}
}
@@ -249,7 +249,7 @@ MulticopterPositionControlMultiplatform::reset_alt_sp()
_att_sp_msg.data().yaw_body = _att->data().yaw;
//XXX: port this once a mavlink like interface is available
// mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
// mavlink_log_info(&_mavlink_log_pub, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
PX4_INFO("[mpc] reset alt sp: %2.3f", -(double)_pos_sp(2));
}
}
src/modules/mc_pos_control_multiplatform/mc_pos_control.h
+1 -1
View File
@@ -84,7 +84,7 @@ protected:
bool _task_should_exit; /**< if true, task should exit */
int _control_task; /**< task handle for task */
int _mavlink_fd; /**< mavlink fd */
orb_advert_t _mavlink_log_pub; /**< mavlink log advert */
Publisher<px4_vehicle_attitude_setpoint> *_att_sp_pub; /**< attitude setpoint publication */
Publisher<px4_vehicle_local_position_setpoint> *_local_pos_sp_pub; /**< vehicle local position setpoint publication */
src/modules/navigator/datalinkloss.cpp
+6 -6
View File
@@ -42,7 +42,7 @@
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <navigator/navigation.h>
@@ -187,7 +187,7 @@ DataLinkLoss::advance_dll()
if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
warnx("%d data link losses, limit is %d, fly to airfield home",
_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
mavlink_log_critical(_navigator->get_mavlink_fd(), "too many DL losses, fly to airfield home");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "too many DL losses, fly to airfield home");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->set_mission_result_updated();
reset_mission_item_reached();
@@ -195,19 +195,19 @@ DataLinkLoss::advance_dll()
} else {
if (!_param_skipcommshold.get()) {
warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to comms hold");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "fly to comms hold");
_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
} else {
/* comms hold wp not active, fly to airfield home directly */
warnx("Skipping comms hold wp. Flying directly to airfield home");
mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to airfield home, comms hold skipped");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "fly to airfield home, comms hold skipped");
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
}
}
break;
case DLL_STATE_FLYTOCOMMSHOLDWP:
warnx("fly to airfield home");
mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to airfield home");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "fly to airfield home");
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->set_mission_result_updated();
@@ -216,7 +216,7 @@ DataLinkLoss::advance_dll()
case DLL_STATE_FLYTOAIRFIELDHOMEWP:
_dll_state = DLL_STATE_TERMINATE;
warnx("time is up, state should have been changed manually by now");
mavlink_log_critical(_navigator->get_mavlink_fd(), "no manual control, terminating");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no manual control, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->set_mission_result_updated();
reset_mission_item_reached();
src/modules/navigator/enginefailure.cpp
+2 -2
View File
@@ -41,7 +41,7 @@
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <navigator/navigation.h>
@@ -141,7 +141,7 @@ EngineFailure::advance_ef()
{
switch (_ef_state) {
case EF_STATE_NONE:
mavlink_log_emergency(_navigator->get_mavlink_fd(), "Engine failure. Loitering down");
mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "Engine failure. Loitering down");
_ef_state = EF_STATE_LOITERDOWN;
break;
default:
src/modules/navigator/geofence.cpp
+14 -10
View File
@@ -44,14 +44,15 @@
#include <string.h>
#include <dataman/dataman.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <stdlib.h>
#include <stdio.h>
#include <ctype.h>
#include <px4_config.h>
#include <unistd.h>
#include <mavlink/mavlink_log.h>
#include <geo/geo.h>
#include <drivers/drv_hrt.h>
#include "navigator.h"
#define GEOFENCE_RANGE_WARNING_LIMIT 3000000
@@ -61,8 +62,10 @@
#endif
static const int ERROR = -1;
Geofence::Geofence() :
Geofence::Geofence(Navigator *navigator) :
SuperBlock(NULL, "GF"),
_navigator(navigator),
_fence_pub(nullptr),
_home_pos{},
_home_pos_set(false),
@@ -77,8 +80,7 @@ Geofence::Geofence() :
_param_counter_threshold(this, "COUNT"),
_param_max_hor_distance(this, "MAX_HOR_DIST"),
_param_max_ver_distance(this, "MAX_VER_DIST"),
_outside_counter(0),
_mavlinkFd(-1)
_outside_counter(0)
{
/* Load initial params */
updateParams();
@@ -145,8 +147,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) {
if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m",
(double)(dist_z - max_vertical_distance));
mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(),
"Geofence exceeded max vertical distance by %.1f m",
(double)(dist_z - max_vertical_distance));
_last_vertical_range_warning = hrt_absolute_time();
}
@@ -155,8 +158,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) {
if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m",
(double)(dist_xy - max_horizontal_distance));
mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(),
"Geofence exceeded max horizontal distance by %.1f m",
(double)(dist_xy - max_horizontal_distance));
_last_horizontal_range_warning = hrt_absolute_time();
}
@@ -403,12 +407,12 @@ Geofence::loadFromFile(const char *filename)
if (gotVertical && pointCounter > 0) {
_vertices_count = pointCounter;
warnx("Geofence: imported successfully");
mavlink_log_info(_mavlinkFd, "Geofence imported");
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported");
rc = OK;
} else {
warnx("Geofence: import error");
mavlink_log_critical(_mavlinkFd, "Geofence import error");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence import error");
}
error:
src/modules/navigator/geofence.h
+5 -5
View File
@@ -53,10 +53,12 @@
#define GEOFENCE_FILENAME PX4_ROOTFSDIR"/fs/microsd/etc/geofence.txt"
class Navigator;
class Geofence : public control::SuperBlock
{
public:
Geofence();
Geofence(Navigator *navigator);
~Geofence();
/* Altitude mode, corresponding to the param GF_ALTMODE */
@@ -105,9 +107,9 @@ public:
int getGeofenceAction() { return _param_action.get(); }
void setMavlinkFd(int value) { _mavlinkFd = value; }
private:
Navigator *_navigator;
orb_advert_t _fence_pub; /**< publish fence topic */
home_position_s _home_pos;
@@ -131,8 +133,6 @@ private:
unsigned _outside_counter;
int _mavlinkFd;
bool inside(double lat, double lon, float altitude);
bool inside(const struct vehicle_global_position_s &global_position);
bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl);
src/modules/navigator/gpsfailure.cpp
+3 -3
View File
@@ -42,7 +42,7 @@
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <navigator/navigation.h>
@@ -162,12 +162,12 @@ GpsFailure::advance_gpsf()
case GPSF_STATE_NONE:
_gpsf_state = GPSF_STATE_LOITER;
warnx("gpsf loiter");
mavlink_log_critical(_navigator->get_mavlink_fd(), "GPS failed: open loop loiter");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "GPS failed: open loop loiter");
break;
case GPSF_STATE_LOITER:
_gpsf_state = GPSF_STATE_TERMINATE;
warnx("gpsf terminate");
mavlink_log_emergency(_navigator->get_mavlink_fd(), "no gps recovery, termination");
mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no gps recovery, termination");
warnx("mavlink sent");
break;
case GPSF_STATE_TERMINATE:
src/modules/navigator/land.cpp
+1 -1
View File
@@ -44,8 +44,8 @@
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <uORB/uORB.h>
#include <uORB/topics/position_setpoint_triplet.h>
src/modules/navigator/loiter.cpp
+1 -1
View File
@@ -45,7 +45,7 @@
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
src/modules/navigator/mission.cpp
+18 -22
View File
@@ -52,7 +52,7 @@
#include <drivers/drv_hrt.h>
#include <dataman/dataman.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <lib/mathlib/mathlib.h>
@@ -257,7 +257,7 @@ Mission::update_offboard_mission()
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
failed = !_missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
failed = !_missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt, _navigator->home_position_valid(),
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
@@ -351,7 +351,7 @@ Mission::set_mission_items()
if (_param_onboard_enabled.get() && prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_ONBOARD) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "onboard mission now running");
user_feedback_done = true;
}
_mission_type = MISSION_TYPE_ONBOARD;
@@ -360,7 +360,7 @@ Mission::set_mission_items()
} else if (prepare_mission_items(false, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
mavlink_log_info(_navigator->get_mavlink_log_pub(), "offboard mission now running");
user_feedback_done = true;
}
_mission_type = MISSION_TYPE_OFFBOARD;
@@ -368,7 +368,7 @@ Mission::set_mission_items()
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "mission finished, loitering");
user_feedback_done = true;
/* use last setpoint for loiter */
@@ -400,9 +400,9 @@ Mission::set_mission_items()
if (_navigator->get_vstatus()->condition_landed) {
/* landed, refusing to take off without a mission */
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, refusing takeoff");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, refusing takeoff");
} else {
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, loitering");
}
user_feedback_done = true;
@@ -432,7 +432,7 @@ Mission::set_mission_items()
float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
mavlink_log_info(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
mavlink_log_info(_navigator->get_mavlink_log_pub(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
_mission_item.lat = _navigator->get_global_position()->lat;
@@ -821,7 +821,7 @@ Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item
int offset = 1;
if (read_mission_item(onboard, 0, mission_item)) {
first_res = true;
/* trying to find next position mission item */
@@ -873,7 +873,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
/* mission item index out of bounds - if they are equal, we just reached the end */
if (*mission_index_ptr != (int)mission->count) {
mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
}
return false;
}
@@ -886,8 +886,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
/* read mission item from datamanager */
if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
mavlink_and_console_log_critical(_navigator->get_mavlink_fd(),
"ERROR waypoint could not be read");
mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(), "ERROR waypoint could not be read");
return false;
}
@@ -906,8 +905,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
&mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the
* dataman */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"ERROR DO JUMP waypoint could not be written");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR DO JUMP waypoint could not be written");
return false;
}
report_do_jump_mission_changed(*mission_index_ptr,
@@ -919,8 +917,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
} else {
if (offset == 0) {
mavlink_log_info(_navigator->get_mavlink_fd(),
"DO JUMP repetitions completed");
mavlink_log_info(_navigator->get_mavlink_log_pub(), "DO JUMP repetitions completed");
}
/* no more DO_JUMPS, therefore just try to continue with next mission item */
(*mission_index_ptr)++;
@@ -934,8 +931,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
}
/* we have given up, we don't want to cycle forever */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"ERROR DO JUMP is cycling, giving up");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR DO JUMP is cycling, giving up");
return false;
}
@@ -957,7 +953,7 @@ Mission::save_offboard_mission_state()
if (mission_state.current_seq != _current_offboard_mission_index) {
if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
warnx("ERROR: can't save mission state");
mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: can't save mission state");
}
}
}
@@ -969,12 +965,12 @@ Mission::save_offboard_mission_state()
mission_state.current_seq = _current_offboard_mission_index;
warnx("ERROR: invalid mission state");
mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: invalid mission state");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: invalid mission state");
/* write modified state only if changed */
if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
warnx("ERROR: can't save mission state");
mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: can't save mission state");
}
}
@@ -1027,7 +1023,7 @@ Mission::check_mission_valid()
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
_navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
_navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt, _navigator->home_position_valid(),
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
src/modules/navigator/mission_block.cpp
+3 -3
View File
@@ -49,7 +49,7 @@
#include <systemlib/err.h>
#include <geo/geo.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
#include <uORB/uORB.h>
@@ -233,7 +233,7 @@ MissionBlock::is_mission_item_reached()
_time_first_inside_orbit = now;
// if (_mission_item.time_inside > 0.01f) {
// mavlink_log_critical(_mavlink_fd, "waypoint reached, wait for %.1fs",
// mavlink_log_critical(_mavlink_log_pub, "waypoint reached, wait for %.1fs",
// (double)_mission_item.time_inside);
// }
}
@@ -486,7 +486,7 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
if (at_current_location) {
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
/* use home position */
} else {
item->lat = _navigator->get_home_position()->lat;
src/modules/navigator/mission_feasibility_checker.cpp
+24 -24
View File
@@ -45,7 +45,7 @@
#include <geo/geo.h>
#include <math.h>
#include <mathlib/mathlib.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <fw_pos_control_l1/landingslope.h>
#include <systemlib/err.h>
#include <stdio.h>
@@ -54,7 +54,7 @@
#include <uORB/topics/fence.h>
MissionFeasibilityChecker::MissionFeasibilityChecker() :
_mavlink_fd(-1),
_mavlink_log_pub(nullptr),
_capabilities_sub(-1),
_initDone(false),
_dist_1wp_ok(false)
@@ -63,7 +63,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() :
}
bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing,
bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_pub, bool isRotarywing,
dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued,
float default_acceptance_rad,
@@ -74,13 +74,13 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota
/* Init if not done yet */
init();
_mavlink_fd = mavlink_fd;
_mavlink_log_pub = mavlink_log_pub;
// first check if we have a valid position
if (!home_valid /* can later use global / local pos for finer granularity */) {
failed = true;
warned = true;
mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock.");
mavlink_log_info(_mavlink_log_pub, "Not yet ready for mission, no position lock.");
} else {
failed = failed || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued);
}
@@ -127,7 +127,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_curr
}
if (takeoff_alt - 1.0f < acceptance_radius) {
mavlink_log_critical(_mavlink_fd, "Mission rejected: Takeoff altitude too low!");
mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Takeoff altitude too low!");
return false;
}
}
@@ -165,7 +165,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
if (MissionBlock::item_contains_position(&missionitem) &&
!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) {
mavlink_log_critical(_mavlink_fd, "Geofence violation for waypoint %d", i);
mavlink_log_critical(_mavlink_log_pub, "Geofence violation for waypoint %d", i);
return false;
}
}
@@ -194,10 +194,10 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
warning_issued = true;
if (throw_error) {
mavlink_log_critical(_mavlink_fd, "Rejecting mission: No home pos, WP %d uses rel alt", i+1);
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: No home pos, WP %d uses rel alt", i+1);
return false;
} else {
mavlink_log_critical(_mavlink_fd, "Warning: No home pos, WP %d uses rel alt", i+1);
mavlink_log_critical(_mavlink_log_pub, "Warning: No home pos, WP %d uses rel alt", i+1);
return true;
}
}
@@ -210,10 +210,10 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
warning_issued = true;
if (throw_error) {
mavlink_log_critical(_mavlink_fd, "Rejecting mission: Waypoint %d below home", i+1);
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: Waypoint %d below home", i+1);
return false;
} else {
mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i+1);
mavlink_log_critical(_mavlink_log_pub, "Warning: Waypoint %d below home", i+1);
return true;
}
}
@@ -230,7 +230,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
if (dm_read(dm_current, i, &missionitem, len) != len) {
// not supposed to happen unless the datamanager can't access the SD card, etc.
mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Cannot access SD card");
mavlink_log_critical(_mavlink_log_pub, "Rejecting Mission: Cannot access SD card");
return false;
}
@@ -250,7 +250,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported cmd: %d", (int)(i+1), (int)missionitem.nav_cmd);
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission item %i: unsupported cmd: %d", (int)(i+1), (int)missionitem.nav_cmd);
return false;
}
@@ -259,7 +259,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
i == 0 &&
condition_landed) {
mavlink_log_critical(_mavlink_fd, "Rejecting mission that starts with LAND command while vehicle is landed.");
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission that starts with LAND command while vehicle is landed.");
return false;
}
@@ -308,25 +308,25 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
return true;
} else {
/* Landing waypoint is above altitude of slope at the given waypoint distance */
mavlink_log_critical(_mavlink_fd, "Landing: last waypoint too high/too close");
mavlink_log_critical(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm",
mavlink_log_critical(_mavlink_log_pub, "Landing: last waypoint too high/too close");
mavlink_log_critical(_mavlink_log_pub, "Move down to %.1fm or move further away by %.1fm",
(double)(slope_alt_req),
(double)(wp_distance_req - wp_distance));
return false;
}
} else {
/* Landing waypoint is above last waypoint */
mavlink_log_critical(_mavlink_fd, "Landing waypoint above last nav waypoint");
mavlink_log_critical(_mavlink_log_pub, "Landing waypoint above last nav waypoint");
return false;
}
} else {
/* Last wp is in flare region */
//xxx give recommendations
mavlink_log_critical(_mavlink_fd, "Warning: Landing: last waypoint in flare region");
mavlink_log_critical(_mavlink_log_pub, "Warning: Landing: last waypoint in flare region");
return false;
}
} else {
mavlink_log_critical(_mavlink_fd, "Warning: starting with land waypoint");
mavlink_log_critical(_mavlink_log_pub, "Warning: starting with land waypoint");
return false;
}
}
@@ -353,13 +353,13 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
/* check actuator number */
if (mission_item.params[0] < 0 || mission_item.params[0] > 5) {
mavlink_log_critical(_mavlink_fd, "Actuator number %d is out of bounds 0..5", (int)mission_item.params[0]);
mavlink_log_critical(_mavlink_log_pub, "Actuator number %d is out of bounds 0..5", (int)mission_item.params[0]);
warning_issued = true;
return false;
}
/* check actuator value */
if (mission_item.params[1] < -2000 || mission_item.params[1] > 2000) {
mavlink_log_critical(_mavlink_fd, "Actuator value %d is out of bounds -2000..2000", (int)mission_item.params[1]);
mavlink_log_critical(_mavlink_log_pub, "Actuator value %d is out of bounds -2000..2000", (int)mission_item.params[1]);
warning_issued = true;
return false;
}
@@ -375,14 +375,14 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
_dist_1wp_ok = true;
if (dist_to_1wp > ((dist_first_wp * 3) / 2)) {
/* allow at 2/3 distance, but warn */
mavlink_log_critical(_mavlink_fd, "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
mavlink_log_critical(_mavlink_log_pub, "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
warning_issued = true;
}
return true;
} else {
/* item is too far from home */
mavlink_log_critical(_mavlink_fd, "First waypoint too far: %d m,refusing mission", (int)dist_to_1wp, (int)dist_first_wp);
mavlink_log_critical(_mavlink_log_pub, "First waypoint too far: %d m,refusing mission", (int)dist_to_1wp, (int)dist_first_wp);
warning_issued = true;
return false;
}
@@ -390,7 +390,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
} else {
/* error reading, mission is invalid */
mavlink_log_info(_mavlink_fd, "error reading offboard mission");
mavlink_log_info(_mavlink_log_pub, "error reading offboard mission");
return false;
}
}

Some files were not shown because too many files have changed in this diff Show More