From bba0d0384d1aca8dec1923cadf42bb47e70b7a66 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 15 Mar 2016 18:25:02 +0000 Subject: [PATCH] drivers/modules: changes after mavlink_log change The mavlink_log API changes lead to changes in all drivers/modules using it. --- src/drivers/camera_trigger/camera_trigger.cpp | 3 +- src/drivers/md25/md25.cpp | 4 +- src/drivers/px4fmu/fmu.cpp | 6 +- src/drivers/px4io/px4io.cpp | 43 ++- src/drivers/roboclaw/RoboClaw.cpp | 2 +- src/lib/runway_takeoff/RunwayTakeoff.cpp | 10 +- src/lib/runway_takeoff/RunwayTakeoff.h | 4 +- .../attitude_estimator_q_main.cpp | 23 +- src/modules/bottle_drop/bottle_drop.cpp | 40 ++- src/modules/commander/PreflightCheck.cpp | 89 +++---- src/modules/commander/PreflightCheck.h | 6 +- .../commander/accelerometer_calibration.cpp | 64 ++--- .../commander/accelerometer_calibration.h | 5 +- .../commander/airspeed_calibration.cpp | 70 ++--- src/modules/commander/airspeed_calibration.h | 3 +- src/modules/commander/baro_calibration.cpp | 4 +- src/modules/commander/baro_calibration.h | 3 +- .../commander/calibration_routines.cpp | 146 +++++------ src/modules/commander/calibration_routines.h | 6 +- src/modules/commander/commander.cpp | 244 ++++++++---------- .../state_machine_helper_test.cpp | 2 +- src/modules/commander/esc_calibration.cpp | 80 +++--- src/modules/commander/esc_calibration.h | 4 +- src/modules/commander/gyro_calibration.cpp | 36 +-- src/modules/commander/gyro_calibration.h | 3 +- src/modules/commander/mag_calibration.cpp | 87 +++---- src/modules/commander/mag_calibration.h | 3 +- src/modules/commander/rc_calibration.cpp | 10 +- src/modules/commander/rc_calibration.h | 3 +- .../commander/state_machine_helper.cpp | 60 ++--- src/modules/commander/state_machine_helper.h | 6 +- src/modules/ekf2/ekf2_main.cpp | 2 +- .../AttitudePositionEstimatorEKF.h | 4 +- .../ekf_att_pos_estimator_main.cpp | 19 +- .../fw_pos_control_l1_main.cpp | 28 +- .../BlockLocalPositionEstimator.cpp | 87 +++---- .../BlockLocalPositionEstimator.hpp | 2 +- .../mc_pos_control/mc_pos_control_main.cpp | 10 +- .../mc_pos_control.cpp | 6 +- .../mc_pos_control.h | 2 +- src/modules/navigator/datalinkloss.cpp | 12 +- src/modules/navigator/enginefailure.cpp | 4 +- src/modules/navigator/geofence.cpp | 24 +- src/modules/navigator/geofence.h | 10 +- src/modules/navigator/gpsfailure.cpp | 6 +- src/modules/navigator/land.cpp | 2 +- src/modules/navigator/loiter.cpp | 2 +- src/modules/navigator/mission.cpp | 40 ++- src/modules/navigator/mission_block.cpp | 6 +- .../navigator/mission_feasibility_checker.cpp | 48 ++-- .../navigator/mission_feasibility_checker.h | 4 +- src/modules/navigator/navigator.h | 4 +- src/modules/navigator/navigator_main.cpp | 24 +- src/modules/navigator/rcloss.cpp | 8 +- src/modules/navigator/rtl.cpp | 18 +- src/modules/navigator/takeoff.cpp | 2 +- .../position_estimator_inav_main.cpp | 39 ++- src/modules/sdlog2/sdlog2.c | 35 +-- src/modules/systemlib/rc_check.c | 22 +- src/modules/systemlib/rc_check.h | 3 +- .../vtol_att_control_main.cpp | 18 +- .../vtol_att_control/vtol_att_control_main.h | 2 +- 62 files changed, 731 insertions(+), 831 deletions(-) diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index 1cc87c95bf..f4163ca5ee 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -51,6 +51,8 @@ #include #include #include +#include + #include #include #include @@ -59,7 +61,6 @@ #include #include #include -#include #include #define TRIGGER_PIN_DEFAULT 1 diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 041772b4bf..4a6401f4f2 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -50,7 +50,7 @@ #include #include -#include +#include #include #include @@ -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) : diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4e18fde300..244d8c4d45 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -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"); } } diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index adcb4466be..f5421b4307 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include @@ -92,7 +93,6 @@ #include -#include #include #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; diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index f9116f1d45..c1f4a01960 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -50,8 +50,8 @@ #include #include +#include #include -#include #include #include diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 7089889858..d216a1ba0e 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -45,7 +45,7 @@ #include "RunwayTakeoff.h" #include #include -#include +#include #include 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; diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 0e1c5ed14b..a0f2289cef 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include 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; }; diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 8bcea6b8d6..6a30caaee6 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -68,12 +68,12 @@ #include #include #include -#include #include #include #include #include +#include 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))); diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 8d2621ff62..97e4b178e6 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -67,10 +67,10 @@ #include #include #include +#include #include #include #include -#include /** @@ -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: diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index dcc251f4a0..49b39147d4 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include @@ -65,7 +66,6 @@ #include #include -#include #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; } } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index 058d6b8956..2d24c0359a 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -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; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 1daf77826b..289450a5f6 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -145,7 +145,7 @@ #include #include #include -#include +#include #include /* 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; } } diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 05c02e294d..2000a2cdc5 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -43,8 +43,9 @@ #define ACCELEROMETER_CALIBRATION_H_ #include +#include -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_ */ diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 4139a949e0..7f24f0c201 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -52,7 +52,7 @@ #include #include #include -#include +#include #include #include #include @@ -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; diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h index 8c6b65ff1e..3a86b0097f 100644 --- a/src/modules/commander/airspeed_calibration.h +++ b/src/modules/commander/airspeed_calibration.h @@ -40,7 +40,8 @@ #define AIRSPEED_CALIBRATION_H_ #include +#include -int do_airspeed_calibration(int mavlink_fd); +int do_airspeed_calibration(orb_advert_t *mavlink_log_pub); #endif /* AIRSPEED_CALIBRATION_H_ */ diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index da98644b40..b8236621b7 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -44,7 +44,7 @@ #include #include #include -#include +#include #include /* 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; diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h index ce78ae7005..b4c645efab 100644 --- a/src/modules/commander/baro_calibration.h +++ b/src/modules/commander/baro_calibration.h @@ -40,7 +40,8 @@ #define BARO_CALIBRATION_H_ #include +#include -int do_baro_calibration(int mavlink_fd); +int do_baro_calibration(orb_advert_t *mavlink_log_pub); #endif /* BARO_CALIBRATION_H_ */ diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 4e4ffd3c81..a03688d42d 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -47,7 +47,7 @@ #include #include #include -#include +#include #include #include @@ -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= 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; } diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index b8232730ac..d4f6a545d7 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -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 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7d2b22882f..da7c4726a9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -59,6 +59,7 @@ #include #include #include +#include //#include #include #include @@ -97,12 +98,12 @@ #include #include #include +#include #include #include #include -#include #include #include #include @@ -173,8 +174,8 @@ enum MAV_MODE_FLAG { MAV_MODE_FLAG_ENUM_END = 129, /* | */ }; -/* Mavlink file descriptors */ -static int mavlink_fd = 0; +/* Mavlink log uORB handle */ +static orb_advert_t mavlink_log_pub = 0; /* System autostart ID */ static int autostart_id; @@ -260,7 +261,7 @@ void print_status(); transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); -transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); +transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const char *armedBy); /** * @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each @@ -359,17 +360,17 @@ int commander_main(int argc, char *argv[]) if (argc > 2) { int calib_ret = OK; if (!strcmp(argv[2], "mag")) { - calib_ret = do_mag_calibration(mavlink_fd); + calib_ret = do_mag_calibration(&mavlink_log_pub); } else if (!strcmp(argv[2], "accel")) { - calib_ret = do_accel_calibration(mavlink_fd); + calib_ret = do_accel_calibration(&mavlink_log_pub); } else if (!strcmp(argv[2], "gyro")) { - calib_ret = do_gyro_calibration(mavlink_fd); + calib_ret = do_gyro_calibration(&mavlink_log_pub); } else if (!strcmp(argv[2], "level")) { - calib_ret = do_level_calibration(mavlink_fd); + calib_ret = do_level_calibration(&mavlink_log_pub); } else if (!strcmp(argv[2], "esc")) { - calib_ret = do_esc_calibration(mavlink_fd, &armed); + calib_ret = do_esc_calibration(&mavlink_log_pub, &armed); } else if (!strcmp(argv[2], "airspeed")) { - calib_ret = do_airspeed_calibration(mavlink_fd); + calib_ret = do_airspeed_calibration(&mavlink_log_pub); } else { warnx("argument %s unsupported.", argv[2]); } @@ -386,41 +387,34 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "check")) { - int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); int checkres = 0; - checkres = preflight_check(&status, mavlink_fd_local, false, true); + checkres = preflight_check(&status, &mavlink_log_pub, false, true); warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED"); - checkres = preflight_check(&status, mavlink_fd_local, true, true); + checkres = preflight_check(&status, &mavlink_log_pub, true, true); warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); - px4_close(mavlink_fd_local); return 0; } if (!strcmp(argv[1], "arm")) { - int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); - if (TRANSITION_CHANGED != arm_disarm(true, mavlink_fd_local, "command line")) { + if (TRANSITION_CHANGED != arm_disarm(true, &mavlink_log_pub, "command line")) { warnx("arming failed"); } - px4_close(mavlink_fd_local); return 0; } if (!strcmp(argv[1], "disarm")) { - int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); - if (TRANSITION_DENIED == arm_disarm(false, mavlink_fd_local, "command line")) { + if (TRANSITION_DENIED == arm_disarm(false, &mavlink_log_pub, "command line")) { warnx("rejected disarm"); } - px4_close(mavlink_fd_local); return 0; } if (!strcmp(argv[1], "takeoff")) { - int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); /* see if we got a home position */ if (status.condition_home_position_valid) { - if (TRANSITION_DENIED != arm_disarm(true, mavlink_fd_local, "command line")) { + if (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "command line")) { vehicle_command_s cmd = {}; cmd.target_system = status.system_id; @@ -445,12 +439,10 @@ int commander_main(int argc, char *argv[]) warnx("rejecting takeoff, no position lock yet. Please retry.."); } - px4_close(mavlink_fd_local); return 0; } if (!strcmp(argv[1], "land")) { - int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); vehicle_command_s cmd = {}; cmd.target_system = status.system_id; @@ -467,7 +459,6 @@ int commander_main(int argc, char *argv[]) // XXX inspect use of publication handle (void)orb_advertise(ORB_ID(vehicle_command), &cmd); - px4_close(mavlink_fd_local); return 0; } @@ -519,8 +510,6 @@ int commander_main(int argc, char *argv[]) return 1; } - int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); - vehicle_command_s cmd = {}; cmd.target_system = status.system_id; cmd.target_component = status.component_id; @@ -532,7 +521,6 @@ int commander_main(int argc, char *argv[]) // XXX inspect use of publication handle (void)orb_advertise(ORB_ID(vehicle_command), &cmd); - px4_close(mavlink_fd_local); return 0; } @@ -613,24 +601,23 @@ void print_status() static orb_advert_t status_pub; -transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char *armedBy) +transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, const char *armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; // For HIL platforms, require that simulated sensors are connected if (arm && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL && is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) { - mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming"); + mavlink_and_console_log_critical(mavlink_log_pub_local, "HIL platform: Connect to simulator before arming"); return TRANSITION_DENIED; } - // Transition the armed state. By passing mavlink_fd to arming_state_transition it will - // output appropriate error messages if the state cannot transition. + // Transition the armed state. arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed, - true /* fRunPreArmChecks */, mavlink_fd_local); + true /* fRunPreArmChecks */, mavlink_log_pub_local); - if (arming_res == TRANSITION_CHANGED && mavlink_fd) { - mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_log_pub_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); @@ -667,12 +654,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s /* set HIL state */ hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? vehicle_status_s::HIL_STATE_ON : vehicle_status_s::HIL_STATE_OFF; - transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd); + transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, &mavlink_log_pub); // Transition the arming state bool cmd_arm = base_mode & MAV_MODE_FLAG_SAFETY_ARMED; - arming_ret = arm_disarm(cmd_arm, mavlink_fd, "set mode command"); + arming_ret = arm_disarm(cmd_arm, &mavlink_log_pub, "set mode command"); /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ if (cmd_arm && (arming_ret == TRANSITION_CHANGED) && @@ -717,7 +704,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s default: main_ret = TRANSITION_DENIED; - mavlink_log_critical(mavlink_fd, "Unsupported auto mode"); + mavlink_log_critical(&mavlink_log_pub, "Unsupported auto mode"); break; } @@ -770,11 +757,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; if (arming_ret == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "Rejecting arming cmd"); + mavlink_log_critical(&mavlink_log_pub, "Rejecting arming cmd"); } if (main_ret == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "Rejecting mode switch cmd"); + mavlink_log_critical(&mavlink_log_pub, "Rejecting mode switch cmd"); } } } @@ -785,7 +772,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints // for logic state parameters if (static_cast(cmd->param1 + 0.5f) != 0 && static_cast(cmd->param1 + 0.5f) != 1) { - mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1); + mavlink_log_critical(&mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1); } else { @@ -799,17 +786,17 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s // Refuse to arm if preflight checks have failed if ((!status.hil_state) != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) { - mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed."); + mavlink_log_critical(&mavlink_log_pub, "Arming DENIED. Preflight checks have failed."); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; break; } } - transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command"); + transition_result_t arming_res = arm_disarm(cmd_arms,&mavlink_log_pub, "arm/disarm component command"); if (arming_res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "REJECTING component arm cmd"); + mavlink_log_critical(&mavlink_log_pub, "REJECTING component arm cmd"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } else { @@ -836,16 +823,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s if (mav_goto == 0) { // MAV_GOTO_DO_HOLD status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; - mavlink_log_critical(mavlink_fd, "Pause mission cmd"); + mavlink_log_critical(&mavlink_log_pub, "Pause mission cmd"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - mavlink_log_critical(mavlink_fd, "Continue mission cmd"); + mavlink_log_critical(&mavlink_log_pub, "Continue mission cmd"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f", + mavlink_log_critical(&mavlink_log_pub, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f", (double)cmd->param1, (double)cmd->param2, (double)cmd->param3, @@ -945,7 +932,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } if (cmd_result == vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED) { - mavlink_and_console_log_info(mavlink_fd, "Home position: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); + mavlink_and_console_log_info(&mavlink_log_pub, "Home position: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); /* announce new home position */ if (*home_pub != nullptr) { @@ -1198,17 +1185,16 @@ int commander_thread_main(int argc, char *argv[]) /* initialize */ if (led_init() != OK) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: LED INIT FAIL"); + mavlink_and_console_log_critical(&mavlink_log_pub, "ERROR: LED INIT FAIL"); } if (buzzer_init() != OK) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: BUZZER INIT FAIL"); + mavlink_and_console_log_critical(&mavlink_log_pub, "ERROR: BUZZER INIT FAIL"); } if (battery_init() != OK) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: BATTERY INIT FAIL"); + mavlink_and_console_log_critical(&mavlink_log_pub, "ERROR: BATTERY INIT FAIL"); } - mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); /* vehicle status topic */ memset(&status, 0, sizeof(status)); @@ -1295,14 +1281,14 @@ int commander_thread_main(int argc, char *argv[]) if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { if (mission.count > 0) { - mavlink_log_info(mavlink_fd, "[cmd] Mission #%d loaded, %u WPs, curr: %d", + mavlink_log_info(&mavlink_log_pub, "[cmd] Mission #%d loaded, %u WPs, curr: %d", mission.dataman_id, mission.count, mission.current_seq); } } else { const char *missionfail = "reading mission state failed"; warnx("%s", missionfail); - mavlink_log_critical(mavlink_fd, missionfail); + mavlink_log_critical(&mavlink_log_pub, missionfail); /* initialize mission state in dataman */ mission.dataman_id = 0; @@ -1482,7 +1468,7 @@ int commander_thread_main(int argc, char *argv[]) set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } else { // sensor diagnostics done continiously, not just at boot so don't warn about any issues just yet - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, + status.condition_system_sensors_initialized = 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, false); set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } @@ -1533,11 +1519,6 @@ int commander_thread_main(int argc, char *argv[]) while (!thread_should_exit) { - if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { - /* try to open the mavlink log device every once in a while */ - mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); - } - arming_ret = TRANSITION_NOT_CHANGED; @@ -1656,10 +1637,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(telemetry_status), telemetry_subs[i], &telemetry); /* perform system checks when new telemetry link connected */ - if ( - /* we actually have a way to talk to the user */ - mavlink_fd && - /* we first connect a link or re-connect a link after loosing it */ + if (/* we first connect a link or re-connect a link after loosing it */ (telemetry_last_heartbeat[i] == 0 || (hrt_elapsed_time(&telemetry_last_heartbeat[i]) > 3 * 1000 * 1000)) && /* and this link has a communication partner */ (telemetry.heartbeat_time > 0) && @@ -1681,11 +1659,11 @@ int commander_thread_main(int argc, char *argv[]) /* provide RC and sensor status feedback to the user */ if (is_hil_setup(autostart_id)) { /* HIL configuration: check only RC input */ - (void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false, + (void)Commander::preflightCheck(&mavlink_log_pub, false, false, false, false, false, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false, true); } else { /* check sensors also */ - (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, + (void)Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, chAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); } } @@ -1716,14 +1694,14 @@ int commander_thread_main(int argc, char *argv[]) if (status.barometer_failure) { status.barometer_failure = false; status_changed = true; - mavlink_log_critical(mavlink_fd, "baro healthy"); + mavlink_log_critical(&mavlink_log_pub, "baro healthy"); } } else { if (!status.barometer_failure) { status.barometer_failure = true; status_changed = true; - mavlink_log_critical(mavlink_fd, "baro failed"); + mavlink_log_critical(&mavlink_log_pub, "baro failed"); } } } @@ -1759,7 +1737,7 @@ int commander_thread_main(int argc, char *argv[]) * apparently the USB cable went away but we are still powered, * so lets reset to a classic non-usb state. */ - mavlink_log_critical(mavlink_fd, "USB disconnected, rebooting.") + mavlink_log_critical(&mavlink_log_pub, "USB disconnected, rebooting.") usleep(400000); px4_systemreset(false); } @@ -1784,8 +1762,8 @@ int commander_thread_main(int argc, char *argv[]) vehicle_status_s::ARMING_STATE_STANDBY_ERROR); if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, - true /* fRunPreArmChecks */, mavlink_fd)) { - mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); + true /* fRunPreArmChecks */, &mavlink_log_pub)) { + mavlink_log_info(&mavlink_log_pub, "DISARMED by safety switch"); arming_state_changed = true; } } @@ -1918,10 +1896,10 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; if (status.condition_landed) { - mavlink_and_console_log_info(mavlink_fd, "LANDING DETECTED"); + mavlink_and_console_log_info(&mavlink_log_pub, "LANDING DETECTED"); } else { - mavlink_and_console_log_info(mavlink_fd, "TAKEOFF DETECTED"); + mavlink_and_console_log_info(&mavlink_log_pub, "TAKEOFF DETECTED"); } } @@ -1933,8 +1911,8 @@ int commander_thread_main(int argc, char *argv[]) } if (_inair_last_time > 0 && ((hrt_absolute_time() - _inair_last_time) > (hrt_abstime)disarm_when_landed * 1000 * 1000)) { - mavlink_log_critical(mavlink_fd, "AUTO DISARMING AFTER LANDING"); - arm_disarm(false, mavlink_fd, "auto disarm on land"); + mavlink_log_critical(&mavlink_log_pub, "AUTO DISARMING AFTER LANDING"); + arm_disarm(false, &mavlink_log_pub, "auto disarm on land"); _inair_last_time = 0; check_for_disarming = false; } @@ -2023,9 +2001,9 @@ int commander_thread_main(int argc, char *argv[]) if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) { low_battery_voltage_actions_done = true; if (armed.armed) { - mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); + mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, RETURN TO LAND ADVISED"); } else { - mavlink_log_critical(mavlink_fd, "LOW BATTERY, TAKEOFF DISCOURAGED"); + mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, TAKEOFF DISCOURAGED"); } status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW; status_changed = true; @@ -2039,15 +2017,15 @@ int commander_thread_main(int argc, char *argv[]) if (!armed.armed) { arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY_ERROR, - &armed, true /* fRunPreArmChecks */, mavlink_fd); + &armed, true /* fRunPreArmChecks */, &mavlink_log_pub); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; - mavlink_and_console_log_critical(mavlink_fd, "LOW BATTERY, LOCKING ARMING DOWN"); + mavlink_and_console_log_critical(&mavlink_log_pub, "LOW BATTERY, LOCKING ARMING DOWN"); } } else { - mavlink_and_console_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); + mavlink_and_console_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, LAND IMMEDIATELY"); } status_changed = true; @@ -2058,7 +2036,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (!status.calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, - mavlink_fd); + &mavlink_log_pub); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -2102,7 +2080,7 @@ int commander_thread_main(int argc, char *argv[]) //Check if GPS receiver is too noisy while we are disarmed if (!armed.armed && gpsIsNoisy) { if (!status.gps_failure) { - mavlink_log_critical(mavlink_fd, "GPS signal noisy"); + mavlink_log_critical(&mavlink_log_pub, "GPS signal noisy"); set_tune_override(TONE_GPS_WARNING_TUNE); //GPS suffers from signal jamming or excessive noise, disable GPS-aided flight @@ -2116,13 +2094,13 @@ int commander_thread_main(int argc, char *argv[]) if (status.gps_failure && !gpsIsNoisy) { status.gps_failure = false; status_changed = true; - mavlink_log_critical(mavlink_fd, "gps fix regained"); + mavlink_log_critical(&mavlink_log_pub, "gps fix regained"); } } else if (!status.gps_failure) { status.gps_failure = true; status_changed = true; - mavlink_log_critical(mavlink_fd, "gps fix lost"); + mavlink_log_critical(&mavlink_log_pub, "gps fix lost"); } } @@ -2137,7 +2115,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; if (status.mission_failure) { - mavlink_log_critical(mavlink_fd, "mission cannot be completed"); + mavlink_log_critical(&mavlink_log_pub, "mission cannot be completed"); } } } @@ -2188,7 +2166,7 @@ int commander_thread_main(int argc, char *argv[]) } case (geofence_result_s::GF_ACTION_TERMINATE) : { warnx("Flight termination because of geofence"); - mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination"); + mavlink_log_critical(&mavlink_log_pub, "Geofence violation: flight termination"); armed.force_failsafe = true; status_changed = true; break; @@ -2241,12 +2219,12 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { - mavlink_and_console_log_critical(mavlink_fd, "Geofence violation: flight termination"); + mavlink_and_console_log_critical(&mavlink_log_pub, "Geofence violation: flight termination"); flight_termination_printed = true; } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - mavlink_and_console_log_critical(mavlink_fd, "Flight termination active"); + mavlink_and_console_log_critical(&mavlink_log_pub, "Flight termination active"); } } @@ -2290,7 +2268,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_info(mavlink_fd, "MANUAL CONTROL REGAINED after %llums", + mavlink_log_info(&mavlink_log_pub, "MANUAL CONTROL REGAINED after %llums", (hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000); status_changed = true; } @@ -2314,7 +2292,7 @@ int commander_thread_main(int argc, char *argv[]) arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : vehicle_status_s::ARMING_STATE_STANDBY_ERROR); arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, - mavlink_fd); + &mavlink_log_pub); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -2351,7 +2329,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, - mavlink_fd); + &mavlink_log_pub); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -2371,10 +2349,10 @@ int commander_thread_main(int argc, char *argv[]) if (arming_ret == TRANSITION_CHANGED) { if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - mavlink_log_info(mavlink_fd, "ARMED by RC"); + mavlink_log_info(&mavlink_log_pub, "ARMED by RC"); } else { - mavlink_log_info(mavlink_fd, "DISARMED by RC"); + mavlink_log_info(&mavlink_log_pub, "DISARMED by RC"); } arming_state_changed = true; @@ -2400,19 +2378,19 @@ int commander_thread_main(int argc, char *argv[]) } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "main state transition denied"); + mavlink_log_critical(&mavlink_log_pub, "main state transition denied"); } /* check throttle kill switch */ if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) { /* set lockdown flag */ if (!armed.lockdown) { - mavlink_log_emergency(mavlink_fd, "MANUAL KILL SWITCH ENGAGED"); + mavlink_log_emergency(&mavlink_log_pub, "MANUAL KILL SWITCH ENGAGED"); } armed.lockdown = true; } else if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { if (armed.lockdown) { - mavlink_log_emergency(mavlink_fd, "MANUAL KILL SWITCH OFF"); + mavlink_log_emergency(&mavlink_log_pub, "MANUAL KILL SWITCH OFF"); } armed.lockdown = false; } @@ -2420,7 +2398,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.rc_input_blocked && !status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000); + mavlink_log_critical(&mavlink_log_pub, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000); status.rc_signal_lost = true; status.rc_signal_lost_timestamp = sp_man.timestamp; status_changed = true; @@ -2441,7 +2419,7 @@ int commander_thread_main(int argc, char *argv[]) /* report a regain */ if (telemetry_last_dl_loss[i] > 0) { - mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i); + mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i regained", i); } else if (telemetry_last_dl_loss[i] == 0) { /* new link */ } @@ -2465,7 +2443,7 @@ int commander_thread_main(int argc, char *argv[]) /* only reset the timestamp to a different time on state change */ telemetry_last_dl_loss[i] = hrt_absolute_time(); - mavlink_and_console_log_info(mavlink_fd, "data link #%i lost", i); + mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i lost", i); telemetry_lost[i] = true; } } @@ -2481,7 +2459,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.data_link_lost) { if (armed.armed) { - mavlink_and_console_log_critical(mavlink_fd, "ALL DATA LINKS LOST"); + mavlink_and_console_log_critical(&mavlink_log_pub, "ALL DATA LINKS LOST"); } status.data_link_lost = true; status.data_link_lost_counter++; @@ -2506,7 +2484,7 @@ int commander_thread_main(int argc, char *argv[]) !status.engine_failure) { status.engine_failure = true; status_changed = true; - mavlink_log_critical(mavlink_fd, "Engine Failure"); + mavlink_log_critical(&mavlink_log_pub, "Engine Failure"); } } else { @@ -2566,12 +2544,12 @@ int commander_thread_main(int argc, char *argv[]) if (!flight_termination_printed) { warnx("Flight termination because of data link loss && gps failure"); - mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + mavlink_log_critical(&mavlink_log_pub, "DL and GPS lost: flight termination"); flight_termination_printed = true; } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + mavlink_log_critical(&mavlink_log_pub, "DL and GPS lost: flight termination"); } } @@ -2596,7 +2574,7 @@ int commander_thread_main(int argc, char *argv[]) } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + mavlink_log_critical(&mavlink_log_pub, "RC and GPS lost: flight termination"); } } } @@ -2633,10 +2611,10 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; if (status.failsafe) { - mavlink_log_critical(mavlink_fd, "failsafe mode on"); + mavlink_log_critical(&mavlink_log_pub, "failsafe mode on"); } else { - mavlink_log_critical(mavlink_fd, "failsafe mode off"); + mavlink_log_critical(&mavlink_log_pub, "failsafe mode off"); } failsafe_old = status.failsafe; @@ -3038,7 +3016,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } while (0); } - + return res; } @@ -3357,7 +3335,7 @@ print_reject_mode(struct vehicle_status_s *status_local, const char *msg) if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; - mavlink_log_critical(mavlink_fd, "REJECT %s", msg); + mavlink_log_critical(&mavlink_log_pub, "REJECT %s", msg); /* only buzz if armed, because else we're driving people nuts indoors they really need to look at the leds as well. */ @@ -3372,7 +3350,7 @@ print_reject_arm(const char *msg) if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; - mavlink_log_critical(mavlink_fd, msg); + mavlink_log_critical(&mavlink_log_pub, msg); tune_negative(true); } } @@ -3386,23 +3364,23 @@ void 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); tune_negative(true); 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); tune_negative(true); break; case vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: /* this needs additional hints to the user - so let other messages pass and be spoken */ - mavlink_log_critical(mavlink_fd, "command temporarily rejected: %u", cmd.command); + mavlink_log_critical(&mavlink_log_pub, "command temporarily rejected: %u", cmd.command); tune_negative(true); 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); tune_negative(true); break; @@ -3459,7 +3437,7 @@ void *commander_low_prio_loop(void *arg) int ret = param_save_default(); if (ret != OK) { - mavlink_and_console_log_critical(mavlink_fd, "settings auto save error"); + mavlink_and_console_log_critical(&mavlink_log_pub, "settings auto save error"); } else { warnx("settings saved."); } @@ -3522,7 +3500,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed, - false /* fRunPreArmChecks */, mavlink_fd)) { + false /* fRunPreArmChecks */, &mavlink_log_pub)) { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); break; } else { @@ -3532,12 +3510,12 @@ void *commander_low_prio_loop(void *arg) if ((int)(cmd.param1) == 1) { /* gyro calibration */ answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); - calib_ret = do_gyro_calibration(mavlink_fd); + calib_ret = do_gyro_calibration(&mavlink_log_pub); } else if ((int)(cmd.param2) == 1) { /* magnetometer calibration */ answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); - calib_ret = do_mag_calibration(mavlink_fd); + calib_ret = do_mag_calibration(&mavlink_log_pub); } else if ((int)(cmd.param3) == 1) { /* zero-altitude pressure calibration */ @@ -3549,30 +3527,30 @@ void *commander_low_prio_loop(void *arg) /* disable RC control input completely */ status.rc_input_blocked = true; calib_ret = OK; - mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN"); + mavlink_log_info(&mavlink_log_pub, "CAL: Disabling RC IN"); } else if ((int)(cmd.param4) == 2) { /* RC trim calibration */ answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); - calib_ret = do_trim_calibration(mavlink_fd); + calib_ret = do_trim_calibration(&mavlink_log_pub); } else if ((int)(cmd.param5) == 1) { /* accelerometer calibration */ answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); - calib_ret = do_accel_calibration(mavlink_fd); + calib_ret = do_accel_calibration(&mavlink_log_pub); } else if ((int)(cmd.param5) == 2) { // board offset calibration answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); - calib_ret = do_level_calibration(mavlink_fd); + calib_ret = do_level_calibration(&mavlink_log_pub); } else if ((int)(cmd.param6) == 1) { /* airspeed calibration */ answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); - calib_ret = do_airspeed_calibration(mavlink_fd); + calib_ret = do_airspeed_calibration(&mavlink_log_pub); } else if ((int)(cmd.param7) == 1) { /* do esc calibration */ answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); - calib_ret = do_esc_calibration(mavlink_fd, &armed); + calib_ret = do_esc_calibration(&mavlink_log_pub, &armed); } else if ((int)(cmd.param4) == 0) { /* RC calibration ended - have we been in one worth confirming? */ @@ -3580,7 +3558,7 @@ void *commander_low_prio_loop(void *arg) answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); /* enable RC control input */ status.rc_input_blocked = false; - mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); + mavlink_log_info(&mavlink_log_pub, "CAL: Re-enabling RC IN"); calib_ret = OK; } /* this always succeeds */ @@ -3605,10 +3583,10 @@ void *commander_low_prio_loop(void *arg) checkAirspeed = true; } - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, + status.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed, !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); - arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); + arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, &mavlink_log_pub); } else { tune_negative(true); @@ -3623,11 +3601,11 @@ void *commander_low_prio_loop(void *arg) int ret = param_load_default(); if (ret == OK) { - mavlink_log_info(mavlink_fd, "settings loaded"); + mavlink_log_info(&mavlink_log_pub, "settings loaded"); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); } else { - mavlink_log_critical(mavlink_fd, "settings load ERROR"); + mavlink_log_critical(&mavlink_log_pub, "settings load ERROR"); /* convenience as many parts of NuttX use negative errno */ if (ret < 0) { @@ -3635,7 +3613,7 @@ void *commander_low_prio_loop(void *arg) } if (ret < 1000) { - mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); + mavlink_log_critical(&mavlink_log_pub, "ERROR: %s", strerror(ret)); } answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack); @@ -3655,7 +3633,7 @@ void *commander_low_prio_loop(void *arg) answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); } else { - mavlink_log_critical(mavlink_fd, "settings save error"); + mavlink_log_critical(&mavlink_log_pub, "settings save error"); /* convenience as many parts of NuttX use negative errno */ if (ret < 0) { @@ -3663,7 +3641,7 @@ void *commander_low_prio_loop(void *arg) } if (ret < 1000) { - mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); + mavlink_log_critical(&mavlink_log_pub, "ERROR: %s", strerror(ret)); } answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack); @@ -3676,11 +3654,11 @@ void *commander_low_prio_loop(void *arg) if (ret == OK) { /* do not spam MAVLink, but provide the answer / green led mechanism */ - mavlink_log_critical(mavlink_fd, "onboard parameters reset"); + mavlink_log_critical(&mavlink_log_pub, "onboard parameters reset"); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); } else { - mavlink_log_critical(mavlink_fd, "param reset error"); + mavlink_log_critical(&mavlink_log_pub, "param reset error"); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack); } } diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 2611136347..d50d0a4d92 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -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); diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp index 2ae74588c6..999e98c151 100644 --- a/src/modules/commander/esc_calibration.cpp +++ b/src/modules/commander/esc_calibration.cpp @@ -57,7 +57,7 @@ #include #include #include -#include +#include /* 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; diff --git a/src/modules/commander/esc_calibration.h b/src/modules/commander/esc_calibration.h index ac37b278c6..bbf66cb1b3 100644 --- a/src/modules/commander/esc_calibration.h +++ b/src/modules/commander/esc_calibration.h @@ -44,6 +44,6 @@ #include -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 \ No newline at end of file +#endif diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index d4225ded3e..ead70925ee 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -53,7 +53,7 @@ #include #include #include -#include +#include #include #include #include @@ -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 */ diff --git a/src/modules/commander/gyro_calibration.h b/src/modules/commander/gyro_calibration.h index 59c32a15e9..1abb51da6b 100644 --- a/src/modules/commander/gyro_calibration.h +++ b/src/modules/commander/gyro_calibration.h @@ -40,7 +40,8 @@ #define GYRO_CALIBRATION_H_ #include +#include -int do_gyro_calibration(int mavlink_fd); +int do_gyro_calibration(orb_advert_t *mavlink_log_pub); #endif /* GYRO_CALIBRATION_H_ */ diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 330cc3cb0c..f4692589dc 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -56,7 +56,7 @@ #include #include #include -#include +#include #include #include @@ -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(malloc(sizeof(float) * calibration_points_maxcount)); worker_data.z[cur_mag] = reinterpret_cast(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 diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h index c14f948cc1..c37bf87639 100644 --- a/src/modules/commander/mag_calibration.h +++ b/src/modules/commander/mag_calibration.h @@ -40,7 +40,8 @@ #define MAG_CALIBRATION_H_ #include +#include -int do_mag_calibration(int mavlink_fd); +int do_mag_calibration(orb_advert_t *mavlink_log_pub); #endif /* MAG_CALIBRATION_H_ */ diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 484087a234..5423c03a82 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include @@ -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; } diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h index 45efedf55c..fef1a4318c 100644 --- a/src/modules/commander/rc_calibration.h +++ b/src/modules/commander/rc_calibration.h @@ -40,7 +40,8 @@ #define RC_CALIBRATION_H_ #include +#include -int do_trim_calibration(int mavlink_fd); +int do_trim_calibration(orb_advert_t *mavlink_log_pub); #endif /* RC_CALIBRATION_H_ */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 2581865338..b87bb778c9 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -58,10 +58,10 @@ #include #include #include +#include #include #include #include -#include #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; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 7c209cf07f..f7da9860b9 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -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_ */ diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index f89f470e2f..c05998ff3b 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -58,9 +58,9 @@ #include #include #include +#include #include #include -#include #include #include #include diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index fc11e0f505..650f054993 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -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); /** diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 326ae229af..d34bdb369c 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -63,9 +63,9 @@ #include #include #include +#include #include #include -#include #include 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; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index fc34bce57b..880b847aa1 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -86,11 +86,11 @@ #include #include #include +#include #include #include #include #include -#include #include #include #include @@ -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> ¤t_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> ¤t_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> ¤t_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> ¤t_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> ¤t_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> ¤t_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> ¤t_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); diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 55230373b6..926018b2b4 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -1,5 +1,5 @@ #include "BlockLocalPositionEstimator.hpp" -#include +#include #include #include #include @@ -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"); } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index b94757cb20..20489665b1 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -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; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 342e99abf8..39dbb75438 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -79,9 +79,9 @@ #include #include +#include #include #include -#include #include #include @@ -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; } diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index e9bc2346cc..8e1b71295f 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -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)); } } diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h index d14d8b23e9..ae27ecc84a 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -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 *_att_sp_pub; /**< attitude setpoint publication */ Publisher *_local_pos_sp_pub; /**< vehicle local position setpoint publication */ diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 9f26d05016..d4e6168478 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -42,7 +42,7 @@ #include #include -#include +#include #include #include #include @@ -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(); diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index 0c344bff92..5133d9547d 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -41,7 +41,7 @@ #include #include -#include +#include #include #include #include @@ -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: diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 2baa0a7581..73839c9ccc 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -44,14 +44,15 @@ #include #include #include +#include #include #include #include #include #include -#include #include #include +#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: diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index deaf2dadb8..f46f849b18 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -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); diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index e6097c87f3..0973d4c932 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -42,7 +42,7 @@ #include #include -#include +#include #include #include #include @@ -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: diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 2f9ae7e57c..7f11744f07 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -44,8 +44,8 @@ #include #include -#include #include +#include #include #include diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index b29bc45063..c30a95a201 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -45,7 +45,7 @@ #include #include -#include +#include #include #include diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ef2c787400..62300d0926 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -52,7 +52,7 @@ #include #include -#include +#include #include #include #include @@ -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, diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index aa38478e2a..6f399eea19 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -49,7 +49,7 @@ #include #include -#include +#include #include #include @@ -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; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 83882083a5..b5cf16f6ac 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -45,7 +45,7 @@ #include #include #include -#include +#include #include #include #include @@ -54,7 +54,7 @@ #include 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; } } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index d605f76228..b372cd3219 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -52,7 +52,7 @@ class MissionFeasibilityChecker { private: - int _mavlink_fd; + orb_advert_t *_mavlink_log_pub; int _capabilities_sub; struct navigation_capabilities_s _nav_caps; @@ -83,7 +83,7 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, + bool 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, bool condition_landed); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 23a35f3621..7e93624eb6 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -176,7 +176,7 @@ public: * @return the distance at which the next waypoint should be used */ float get_acceptance_radius(float mission_item_radius); - int get_mavlink_fd() { return _mavlink_fd; } + orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; } void increment_mission_instance_count() { _mission_instance_count++; } @@ -187,7 +187,7 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _navigator_task; /**< task handle for sensor task */ - int _mavlink_fd; /**< the file descriptor to send messages over mavlink */ + orb_advert_t _mavlink_log_pub; /**< the uORB advert to send messages over mavlink */ int _global_pos_sub; /**< global position subscription */ int _gps_pos_sub; /**< gps position subscription */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 6d55962f90..edfbbd10f4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -79,7 +79,7 @@ #include #include #include -#include +#include #include "navigator.h" @@ -102,7 +102,7 @@ Navigator::Navigator() : SuperBlock(NULL, "NAV"), _task_should_exit(false), _navigator_task(-1), - _mavlink_fd(-1), + _mavlink_log_pub(nullptr), _global_pos_sub(-1), _gps_pos_sub(-1), _home_pos_sub(-1), @@ -131,7 +131,7 @@ Navigator::Navigator() : _mission_item_valid(false), _mission_instance_count(0), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), - _geofence{}, + _geofence(this), _geofence_violation_warning_sent(false), _inside_fence(true), _can_loiter_at_sp(false), @@ -265,9 +265,6 @@ Navigator::task_main_trampoline(int argc, char *argv[]) void Navigator::task_main() { - _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); - _geofence.setMavlinkFd(_mavlink_fd); - bool have_geofence_position_data = false; /* Try to load the geofence: @@ -281,7 +278,7 @@ Navigator::task_main() } else { if (_geofence.clearDm() != OK) { - mavlink_log_critical(_mavlink_fd, "failed clearing geofence"); + mavlink_log_critical(&_mavlink_log_pub, "failed clearing geofence"); } } @@ -308,9 +305,6 @@ Navigator::task_main() navigation_capabilities_update(); params_update(); - hrt_abstime mavlink_open_time = 0; - const hrt_abstime mavlink_open_interval = 500000; - /* wakeup source(s) */ px4_pollfd_struct_t fds[1] = {}; @@ -342,12 +336,6 @@ Navigator::task_main() perf_begin(_loop_perf); - if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) { - /* try to reopen the mavlink log device with specified interval */ - mavlink_open_time = hrt_abstime() + mavlink_open_interval; - _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); - } - bool updated; /* gps updated */ @@ -435,7 +423,7 @@ Navigator::task_main() /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { - mavlink_log_critical(_mavlink_fd, "Geofence violation"); + mavlink_log_critical(&_mavlink_log_pub, "Geofence violation"); _geofence_violation_warning_sent = true; } } else { @@ -777,6 +765,6 @@ Navigator::set_mission_failure(const char* reason) if (!_mission_result.mission_failure) { _mission_result.mission_failure = true; set_mission_result_updated(); - mavlink_log_critical(_mavlink_fd, "%s", reason); + mavlink_log_critical(&_mavlink_log_pub, "%s", reason); } } diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index fcc00148f8..2572788d6e 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -42,7 +42,7 @@ #include #include -#include +#include #include #include @@ -155,11 +155,11 @@ RCLoss::advance_rcl() case RCL_STATE_NONE: if (_param_loitertime.get() > 0.0f) { warnx("RC loss, OBC mode, loiter"); - mavlink_log_critical(_navigator->get_mavlink_fd(), "rc loss, loitering"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "rc loss, loitering"); _rcl_state = RCL_STATE_LOITER; } else { warnx("RC loss, OBC mode, slip loiter, terminate"); - mavlink_log_critical(_navigator->get_mavlink_fd(), "rc loss, terminating"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "rc loss, terminating"); _rcl_state = RCL_STATE_TERMINATE; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->set_mission_result_updated(); @@ -169,7 +169,7 @@ RCLoss::advance_rcl() case RCL_STATE_LOITER: _rcl_state = RCL_STATE_TERMINATE; warnx("time is up, no RC regain, terminating"); - mavlink_log_critical(_navigator->get_mavlink_fd(), "RC not regained, terminating"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RC not regained, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->set_mission_result_updated(); reset_mission_item_reached(); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 78062cd88b..7b2c46a6c3 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -42,7 +42,7 @@ #include #include -#include +#include #include #include @@ -96,7 +96,7 @@ RTL::on_activation() /* for safety reasons don't go into RTL if landed */ if (_navigator->get_vstatus()->condition_landed) { _rtl_state = RTL_STATE_LANDED; - mavlink_log_critical(_navigator->get_mavlink_fd(), "no RTL when landed"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no RTL when landed"); /* if lower than return altitude, climb up first */ } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt @@ -157,7 +157,7 @@ RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: climb to %d m (%d m above home)", + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)", (int)(climb_alt), (int)(climb_alt - _navigator->get_home_position()->alt)); break; @@ -189,7 +189,7 @@ RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d m (%d m above home)", + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)", (int)(_mission_item.altitude), (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); @@ -212,7 +212,7 @@ RTL::set_rtl_item() _mission_item.autocontinue = false; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: descend to %d m (%d m above home)", + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)", (int)(_mission_item.altitude), (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); break; @@ -238,10 +238,10 @@ RTL::set_rtl_item() _navigator->set_can_loiter_at_sp(true); if (autoland) { - mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: loiter %.1fs", (double)_mission_item.time_inside); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)_mission_item.time_inside); } else { - mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, loiter"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter"); } break; } @@ -249,14 +249,14 @@ RTL::set_rtl_item() case RTL_STATE_LAND: { set_land_item(&_mission_item, false); - mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: land at home"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: land at home"); break; } case RTL_STATE_LANDED: { set_idle_item(&_mission_item); - mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, landed"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: completed, landed"); break; } diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 2effa1b042..95d3849f55 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -44,7 +44,7 @@ #include #include -#include +#include #include #include diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 5de0a389dd..58ffd43886 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -66,9 +66,9 @@ #include #include #include -#include #include #include +#include #include #include #include @@ -234,8 +234,7 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { - int mavlink_fd; - mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); + orb_advert_t mavlink_log_pub = nullptr; float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel @@ -421,10 +420,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ret < 0) { /* poll error */ - mavlink_log_info(mavlink_fd, "[inav] poll error on init"); + mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init"); } else if (hrt_absolute_time() - baro_wait_for_sample_time > MAX_WAIT_FOR_BARO_SAMPLE) { wait_baro = false; - mavlink_log_info(mavlink_fd, "[inav] timed out waiting for a baro sample"); + mavlink_log_info(&mavlink_log_pub, "[inav] timed out waiting for a baro sample"); } else if (ret > 0) { if (fds_init[0].revents & POLLIN) { @@ -466,7 +465,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ret < 0) { /* poll error */ - mavlink_log_info(mavlink_fd, "[inav] poll error on init"); + mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init"); continue; } else if (ret > 0) { @@ -549,9 +548,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar); lidar.current_distance += params.lidar_calibration_offset; } - + if (updated) { //check if altitude estimation for lidar is enabled and new sensor data - + if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance && lidar.current_distance < lidar.max_distance && (PX4_R(att.R, 2, 2) > 0.7f)) { @@ -567,7 +566,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (lidar_first) { lidar_first = false; lidar_offset = dist_ground + z_est[0]; - mavlink_log_info(mavlink_fd, "[inav] LIDAR: new ground offset"); + mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR: new ground offset"); warnx("[inav] LIDAR: new ground offset"); } @@ -764,7 +763,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) last_vision_z = vision.z; warnx("VISION estimate valid"); - mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid"); + mavlink_log_info(&mavlink_log_pub, "[inav] VISION estimate valid"); } /* calculate correction for position */ @@ -818,7 +817,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mocap_valid = true; warnx("MOCAP data valid"); - mavlink_log_info(mavlink_fd, "[inav] MOCAP data valid"); + mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP data valid"); } /* calculate correction for position */ @@ -842,7 +841,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps_valid) { if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) { gps_valid = false; - mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); + mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal lost"); warnx("[inav] GPS signal lost"); } @@ -850,7 +849,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) { gps_valid = true; reset_est = true; - mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); + mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal found"); warnx("[inav] GPS signal found"); } } @@ -883,7 +882,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) map_projection_init(&ref, lat, lon); // XXX replace this print warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); + mavlink_log_info(&mavlink_log_pub, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); } } @@ -945,35 +944,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout)) { flow_valid = false; warnx("FLOW timeout"); - mavlink_log_info(mavlink_fd, "[inav] FLOW timeout"); + mavlink_log_info(&mavlink_log_pub, "[inav] FLOW timeout"); } /* check for timeout on GPS topic */ if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) { gps_valid = false; warnx("GPS timeout"); - mavlink_log_info(mavlink_fd, "[inav] GPS timeout"); + mavlink_log_info(&mavlink_log_pub, "[inav] GPS timeout"); } /* check for timeout on vision topic */ if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) { vision_valid = false; warnx("VISION timeout"); - mavlink_log_info(mavlink_fd, "[inav] VISION timeout"); + mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout"); } /* check for timeout on mocap topic */ if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) { mocap_valid = false; warnx("MOCAP timeout"); - mavlink_log_info(mavlink_fd, "[inav] MOCAP timeout"); + mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP timeout"); } /* check for lidar measurement timeout */ if (lidar_valid && (t > (lidar_time + lidar_timeout))) { lidar_valid = false; warnx("LIDAR timeout"); - mavlink_log_info(mavlink_fd, "[inav] LIDAR timeout"); + mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR timeout"); } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; @@ -1380,7 +1379,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } warnx("stopped"); - mavlink_log_info(mavlink_fd, "[inav] stopped"); + mavlink_log_info(&mavlink_log_pub, "[inav] stopped"); thread_running = false; return 0; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ed885f224d..054e0e06f4 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -118,10 +118,9 @@ #include #include #include +#include #include -#include - #include "logbuffer.h" #include "sdlog2_format.h" #include "sdlog2_messages.h" @@ -159,7 +158,7 @@ static int32_t _utc_offset = 0; #endif static const char *mountpoint = MOUNTPOINT; static const char *log_root = MOUNTPOINT "/log"; -static int mavlink_fd = -1; +static orb_advert_t mavlink_log_pub = NULL; struct logbuffer_s lb; /* mutex / condition to synchronize threads */ @@ -480,7 +479,7 @@ int create_log_dir() } /* print logging path, important to find log file later */ - mavlink_and_console_log_info(mavlink_fd, "[blackbox] %s", log_dir); + mavlink_and_console_log_info(&mavlink_log_pub, "[blackbox] %s", log_dir); return 0; } @@ -517,7 +516,7 @@ int open_log_file() if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_and_console_log_critical(mavlink_fd, "[blackbox] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(&mavlink_log_pub, "[blackbox] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -529,10 +528,10 @@ int open_log_file() #endif if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[blackbox] failed: %s", log_file_name); + mavlink_and_console_log_critical(&mavlink_log_pub, "[blackbox] failed: %s", log_file_name); } else { - mavlink_and_console_log_info(mavlink_fd, "[blackbox] recording: %s", log_file_name); + mavlink_and_console_log_info(&mavlink_log_pub, "[blackbox] recording: %s", log_file_name); } return fd; @@ -569,7 +568,7 @@ int open_perf_file(const char* str) if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_and_console_log_critical(mavlink_fd, "[blackbox] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(&mavlink_log_pub, "[blackbox] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -581,7 +580,7 @@ int open_perf_file(const char* str) #endif if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[blackbox] failed: %s", log_file_name); + mavlink_and_console_log_critical(&mavlink_log_pub, "[blackbox] failed: %s", log_file_name); } @@ -710,7 +709,7 @@ void sdlog2_start_log() /* create log dir if needed */ if (create_log_dir() != 0) { - mavlink_and_console_log_critical(mavlink_fd, "[blackbox] error creating log dir"); + mavlink_and_console_log_critical(&mavlink_log_pub, "[blackbox] error creating log dir"); return; } @@ -811,7 +810,7 @@ void sdlog2_stop_log() /* free log buffer */ logbuffer_free(&lb); - mavlink_and_console_log_info(mavlink_fd, "[blackbox] stopped (%lu drops)", skipped_count); + mavlink_and_console_log_info(&mavlink_log_pub, "[blackbox] stopped (%lu drops)", skipped_count); sdlog2_status(); } @@ -925,12 +924,6 @@ bool copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void int sdlog2_thread_main(int argc, char *argv[]) { - mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); - - if (mavlink_fd < 0) { - warnx("ERR: log stream, start mavlink app first"); - } - /* default log rate: 50 Hz */ int32_t log_rate = 50; int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT; @@ -2163,7 +2156,7 @@ void sdlog2_status() float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); - mavlink_log_info(mavlink_fd, "[blackbox] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); + mavlink_log_info(&mavlink_log_pub, "[blackbox] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); } } @@ -2187,16 +2180,14 @@ int check_free_space() /* use a threshold of 50 MiB */ if (statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(50 * 1024 * 1024 / statfs_buf.f_bsize)) { - mavlink_and_console_log_critical(mavlink_fd, - "[blackbox] no space on MicroSD: %u MiB", + mavlink_and_console_log_critical(&mavlink_log_pub, "[blackbox] no space on MicroSD: %u MiB", (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); /* we do not need a flag to remember that we sent this warning because we will exit anyway */ return PX4_ERROR; /* use a threshold of 100 MiB to send a warning */ } else if (!space_warning_sent && statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(100 * 1024 * 1024 / statfs_buf.f_bsize)) { - mavlink_and_console_log_critical(mavlink_fd, - "[blackbox] space on MicroSD low: %u MiB", + mavlink_and_console_log_critical(&mavlink_log_pub, "[blackbox] space on MicroSD low: %u MiB", (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); /* we don't want to flood the user with warnings */ space_warning_sent = true; diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index ff08328ddd..74de8f3ef4 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -47,12 +47,12 @@ #include #include #include -#include +#include #include #define RC_INPUT_MAP_UNMAPPED 0 -int rc_calibration_check(int mavlink_fd, bool report_fail) +int rc_calibration_check(orb_advert_t *mavlink_log_pub, bool report_fail) { char nbuf[20]; @@ -74,7 +74,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) param_t map_parm = param_find(rc_map_mandatory[j]); if (map_parm == PARAM_INVALID) { - if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -87,7 +87,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) param_get(map_parm, &mapping); if (mapping > RC_INPUT_MAX_CHANNELS) { - if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -95,7 +95,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) } if (mapping == 0) { - if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -148,7 +148,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (param_min < RC_INPUT_LOWEST_MIN_US) { count++; - if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -157,7 +157,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (param_max > RC_INPUT_HIGHEST_MAX_US) { count++; - if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -166,7 +166,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (param_trim < param_min) { count++; - if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -175,7 +175,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (param_trim > param_max) { count++; - if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -183,7 +183,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) /* assert deadzone is sane */ if (param_dz > RC_INPUT_MAX_DEADZONE_US) { - if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -201,7 +201,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) sleep(2); if (report_fail) { - mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", + mavlink_and_console_log_critical(mavlink_log_pub, "%d config error%s for %d RC channel%s.", total_fail_count, (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); } diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index bf869bda33..11c3f70b20 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -37,6 +37,7 @@ * RC calibration check */ #include +#include #pragma once @@ -48,6 +49,6 @@ __BEGIN_DECLS * @return 0 / OK if RC calibration is ok, index + 1 of the first * channel that failed else (so 1 == first channel failed) */ -__EXPORT int rc_calibration_check(int mavlink_fd, bool report_fail); +__EXPORT int rc_calibration_check(orb_advert_t *mavlink_log_pub, bool report_fail); __END_DECLS diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 8579acaca3..26352dcd69 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -47,7 +47,7 @@ * */ #include "vtol_att_control_main.h" -#include +#include namespace VTOL_att_control { @@ -62,7 +62,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _control_task(-1), // mavlink log - _mavlink_fd(-1), + _mavlink_log_pub(nullptr), //init subscription handlers _v_att_sub(-1), @@ -491,7 +491,7 @@ void VtolAttitudeControl::abort_front_transition() { if(!_abort_front_transition) { - mavlink_log_critical(_mavlink_fd, "Front transition timeout occured, aborting"); + mavlink_log_critical(&_mavlink_log_pub, "Front transition timeout occured, aborting"); _abort_front_transition = true; _vtol_vehicle_status.vtol_transition_failsafe = true; } @@ -596,8 +596,6 @@ void VtolAttitudeControl::task_main() { fflush(stdout); - _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); - /* do subscriptions */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint)); @@ -628,9 +626,6 @@ void VtolAttitudeControl::task_main() // make sure we start with idle in mc mode _vtol_type->set_idle_mc(); - hrt_abstime mavlink_open_time = 0; - const hrt_abstime mavlink_open_interval = 500000; - /* wakeup source*/ px4_pollfd_struct_t fds[3] = {}; /*input_mc, input_fw, parameters*/ @@ -668,13 +663,6 @@ void VtolAttitudeControl::task_main() continue; } - if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) { - /* try to reopen the mavlink log device with specified interval */ - mavlink_open_time = hrt_abstime() + mavlink_open_interval; - _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); - } - - if (fds[2].revents & POLLIN) { //parameters were updated, read them now /* read from param to clear updated flag */ struct parameter_update_s update; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 0fa60fa975..5a5b5eed57 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -140,7 +140,7 @@ private: //******************flags & handlers****************************************************** bool _task_should_exit; int _control_task; //task handle for VTOL attitude controller - int _mavlink_fd; // mavlink log device + orb_advert_t _mavlink_log_pub; // mavlink log uORB handle /* handlers for subscriptions */ int _v_att_sub; //vehicle attitude subscription