From 5fb16e4395cabfb551d2c18acfb09677ef802e54 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 9 Sep 2021 22:07:14 +0200 Subject: [PATCH] fw_pos_control_l1: switch to events --- .../FixedwingPositionControl.cpp | 70 +++++++++++++++---- .../runway_takeoff/RunwayTakeoff.cpp | 11 ++- 2 files changed, 66 insertions(+), 15 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 8093fd544b..3043358fd4 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -34,6 +34,7 @@ #include "FixedwingPositionControl.hpp" #include +#include using math::constrain; using math::max; @@ -146,23 +147,57 @@ FixedwingPositionControl::parameters_update() // sanity check parameters if (_param_fw_airspd_max.get() < _param_fw_airspd_min.get()) { - mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max smaller than min"); + mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max smaller than min\t"); + /* EVENT + * @description + * - FW_AIRSPD_MAX: {1:.1} + * - FW_AIRSPD_MIN: {2:.1} + */ + events::send(events::ID("fixedwing_position_control_conf_invalid_airspeed"), events::Log::Error, + "Invalid configuration: Airspeed max smaller than min", + _param_fw_airspd_max.get(), _param_fw_airspd_min.get()); check_ret = PX4_ERROR; } if (_param_fw_airspd_max.get() < 5.0f || _param_fw_airspd_min.get() > 100.0f) { - mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max < 5 m/s or min > 100 m/s"); + mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max < 5 m/s or min > 100 m/s\t"); + /* EVENT + * @description + * - FW_AIRSPD_MAX: {1:.1} + * - FW_AIRSPD_MIN: {2:.1} + */ + events::send(events::ID("fixedwing_position_control_conf_invalid_airspeed_bounds"), events::Log::Error, + "Invalid configuration: Airspeed max \\< 5 m/s or min \\> 100 m/s", + _param_fw_airspd_max.get(), _param_fw_airspd_min.get()); check_ret = PX4_ERROR; } if (_param_fw_airspd_trim.get() < _param_fw_airspd_min.get() || _param_fw_airspd_trim.get() > _param_fw_airspd_max.get()) { - mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed cruise out of min or max bounds"); + mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed cruise out of min or max bounds\t"); + /* EVENT + * @description + * - FW_AIRSPD_MAX: {1:.1} + * - FW_AIRSPD_MIN: {2:.1} + * - FW_AIRSPD_TRIM: {3:.1} + */ + events::send(events::ID("fixedwing_position_control_conf_invalid_cruise_bounds"), + events::Log::Error, + "Invalid configuration: Airspeed cruise out of min or max bounds", + _param_fw_airspd_max.get(), _param_fw_airspd_min.get(), _param_fw_airspd_trim.get()); check_ret = PX4_ERROR; } if (_param_fw_airspd_stall.get() > _param_fw_airspd_min.get() * 0.9f) { - mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Stall airspeed higher than 0.9 of min"); + mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Stall airspeed higher than 0.9 of min\t"); + /* EVENT + * @description + * - FW_AIRSPD_MIN: {1:.1} + * - FW_AIRSPD_STALL: {2:.1} + */ + events::send(events::ID("fixedwing_position_control_conf_invalid_stall"), events::Log::Error, + "Invalid configuration: Stall airspeed higher than 90% of minimum airspeed", + _param_fw_airspd_min.get(), _param_fw_airspd_stall.get()); check_ret = PX4_ERROR; } @@ -475,7 +510,9 @@ FixedwingPositionControl::abort_landing(bool abort) { // only announce changes if (abort && !_land_abort) { - mavlink_log_critical(&_mavlink_log_pub, "Landing aborted"); + mavlink_log_critical(&_mavlink_log_pub, "Landing aborted\t"); + // TODO: add reason + events::send(events::ID("fixedwing_position_control_land_aborted"), events::Log::Critical, "Landing aborted"); } _land_abort = abort; @@ -1238,7 +1275,8 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2d * doesn't matter if it gets reset when takeoff is detected eventually */ _takeoff_ground_alt = _current_altitude; - mavlink_log_info(&_mavlink_log_pub, "Takeoff on runway"); + mavlink_log_info(&_mavlink_log_pub, "Takeoff on runway\t"); + events::send(events::ID("fixedwing_position_control_takeoff"), events::Log::Info, "Takeoff on runway"); } float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt); @@ -1287,7 +1325,8 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2d /* Inform user that launchdetection is running every 4s */ if ((now - _launch_detection_notify) > 4_s) { - mavlink_log_critical(&_mavlink_log_pub, "Launch detection running"); + mavlink_log_critical(&_mavlink_log_pub, "Launch detection running\t"); + events::send(events::ID("fixedwing_position_control_launch_detection"), events::Log::Info, "Launch detection running"); _launch_detection_notify = now; } @@ -1452,7 +1491,8 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d } _land_noreturn_horizontal = true; - mavlink_log_info(&_mavlink_log_pub, "Landing, heading hold"); + mavlink_log_info(&_mavlink_log_pub, "Landing, heading hold\t"); + events::send(events::ID("fixedwing_position_control_landing"), events::Log::Info, "Landing, heading hold"); } if (_land_noreturn_horizontal) { @@ -1541,7 +1581,9 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d if (!_land_motor_lim) { _land_motor_lim = true; - mavlink_log_info(&_mavlink_log_pub, "Landing, limiting throttle"); + mavlink_log_info(&_mavlink_log_pub, "Landing, limiting throttle\t"); + events::send(events::ID("fixedwing_position_control_landing_limit_throttle"), events::Log::Info, + "Landing, limiting throttle"); } } @@ -1572,7 +1614,8 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d // just started with the flaring phase _flare_pitch_sp = radians(_param_fw_psp_off.get()); _flare_height = _current_altitude - terrain_alt; - mavlink_log_info(&_mavlink_log_pub, "Landing, flaring"); + mavlink_log_info(&_mavlink_log_pub, "Landing, flaring\t"); + events::send(events::ID("fixedwing_position_control_landing_flaring"), events::Log::Info, "Landing, flaring"); _land_noreturn_vertical = true; } else { @@ -1609,7 +1652,8 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d altitude_desired = terrain_alt + landing_slope_alt_rel_desired; if (!_land_onslope) { - mavlink_log_info(&_mavlink_log_pub, "Landing, on slope"); + mavlink_log_info(&_mavlink_log_pub, "Landing, on slope\t"); + events::send(events::ID("fixedwing_position_control_landing_on_slope"), events::Log::Info, "Landing, on slope"); _land_onslope = true; } @@ -1747,7 +1791,9 @@ FixedwingPositionControl::Run() } } else { - mavlink_log_critical(&_mavlink_log_pub, "Invalid offboard setpoint"); + mavlink_log_critical(&_mavlink_log_pub, "Invalid offboard setpoint\t"); + events::send(events::ID("fixedwing_position_control_invalid_offboard_sp"), events::Log::Error, + "Invalid offboard setpoint"); } } diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp index c9b9bfd437..d6aa863ce0 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp @@ -45,6 +45,7 @@ #include "RunwayTakeoff.h" #include #include +#include using namespace time_literals; @@ -88,7 +89,9 @@ void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl case RunwayTakeoffState::CLAMPED_TO_RUNWAY: if (airspeed > _param_fw_airspd_min.get() * _param_rwto_airspd_scl.get()) { _state = RunwayTakeoffState::TAKEOFF; - mavlink_log_info(mavlink_log_pub, "#Takeoff airspeed reached"); + mavlink_log_info(mavlink_log_pub, "#Takeoff airspeed reached\t"); + events::send(events::ID("runway_takeoff_reached_airspeed"), events::Log::Info, + "Takeoff airspeed reached"); } break; @@ -106,7 +109,8 @@ void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl _start_wp(1) = current_lon; } - mavlink_log_info(mavlink_log_pub, "#Climbout"); + mavlink_log_info(mavlink_log_pub, "#Climbout\t"); + events::send(events::ID("runway_takeoff_climbout"), events::Log::Info, "Climbout"); } break; @@ -115,7 +119,8 @@ void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl if (alt_agl > _param_fw_clmbout_diff.get()) { _climbout = false; _state = RunwayTakeoffState::FLY; - mavlink_log_info(mavlink_log_pub, "#Navigating to waypoint"); + mavlink_log_info(mavlink_log_pub, "#Navigating to waypoint\t"); + events::send(events::ID("runway_takeoff_nav_to_wp"), events::Log::Info, "Navigating to waypoint"); } break;