From 79c55614d860d2f3e8ce775741dd96ef062c998f Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 24 Nov 2022 09:52:01 +0100 Subject: [PATCH] FW PositionController: remove mavlink_log_pub, and only use events Signed-off-by: Silvan Fuhrer --- .../FixedwingPositionControl.cpp | 18 ++---------------- .../FixedwingPositionControl.hpp | 2 -- .../launchdetection/LaunchDetector.cpp | 8 +------- .../launchdetection/LaunchDetector.h | 3 +-- .../runway_takeoff/RunwayTakeoff.cpp | 5 +---- .../runway_takeoff/RunwayTakeoff.h | 4 +--- 6 files changed, 6 insertions(+), 34 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index adf0d8c6c8..1e7ec5fb68 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -149,7 +149,6 @@ 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\t"); /* EVENT * @description * - FW_AIRSPD_MAX: {1:.1} @@ -162,7 +161,6 @@ FixedwingPositionControl::parameters_update() } 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\t"); /* EVENT * @description * - FW_AIRSPD_MAX: {1:.1} @@ -176,7 +174,6 @@ FixedwingPositionControl::parameters_update() 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 trim out of min or max bounds\t"); /* EVENT * @description * - FW_AIRSPD_MAX: {1:.1} @@ -191,7 +188,6 @@ FixedwingPositionControl::parameters_update() } if (_param_fw_airspd_stall.get() > _param_fw_airspd_min.get()) { - mavlink_log_critical(&_mavlink_log_pub, "Config invalid: FW_AIRSPD_STALL airspeed higher than FW_AIRSPD_MIN\t"); /* EVENT * @description * - FW_AIRSPD_MIN: {1:.1} @@ -626,28 +622,24 @@ FixedwingPositionControl::updateLandingAbortStatus(const uint8_t new_abort_statu switch (new_abort_status) { case (position_controller_landing_status_s::ABORTED_BY_OPERATOR): { - mavlink_log_critical(&_mavlink_log_pub, "Landing aborted by operator\t"); events::send(events::ID("fixedwing_position_control_landing_abort_status_operator_abort"), events::Log::Critical, "Landing aborted by operator"); break; } case (position_controller_landing_status_s::TERRAIN_NOT_FOUND): { - mavlink_log_critical(&_mavlink_log_pub, "Landing aborted: terrain estimate not found\t"); events::send(events::ID("fixedwing_position_control_landing_abort_status_terrain_not_found"), events::Log::Critical, "Landing aborted: terrain measurement not found"); break; } case (position_controller_landing_status_s::TERRAIN_TIMEOUT): { - mavlink_log_critical(&_mavlink_log_pub, "Landing aborted: terrain estimate timed out\t"); events::send(events::ID("fixedwing_position_control_landing_abort_status_terrain_timeout"), events::Log::Critical, "Landing aborted: terrain estimate timed out"); break; } default: { - mavlink_log_critical(&_mavlink_log_pub, "Landing aborted: unknown criterion\t"); events::send(events::ID("fixedwing_position_control_landing_abort_status_unknown_criterion"), events::Log::Critical, "Landing aborted: unknown criterion"); } @@ -813,7 +805,6 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) && !_vehicle_status.in_transition_mode) { if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE) { // Need to init because last loop iteration was in a different mode - mavlink_log_critical(&_mavlink_log_pub, "Start loiter with fixed bank angle.\t"); events::send(events::ID("fixedwing_position_control_fb_loiter"), events::Log::Critical, "Start loiter with fixed bank angle"); } @@ -822,7 +813,6 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) } else { if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE && !_vehicle_status.in_transition_mode) { - mavlink_log_critical(&_mavlink_log_pub, "Start descending.\t"); events::send(events::ID("fixedwing_position_control_descend"), events::Log::Critical, "Start descending"); } @@ -1430,7 +1420,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _launch_current_yaw = _yaw; - mavlink_log_info(&_mavlink_log_pub, "Takeoff on runway\t"); events::send(events::ID("fixedwing_position_control_takeoff"), events::Log::Info, "Takeoff on runway"); } @@ -1439,8 +1428,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo } _runway_takeoff.update(now, takeoff_airspeed, _airspeed, _current_altitude - _takeoff_ground_alt, - clearance_altitude_amsl - _takeoff_ground_alt, - &_mavlink_log_pub); + clearance_altitude_amsl - _takeoff_ground_alt); // yaw control is disabled once in "taking off" state _att_sp.fw_control_yaw_wheel = _runway_takeoff.controlYaw(); @@ -1544,7 +1532,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo /* Perform launch detection */ /* Detect launch using body X (forward) acceleration */ - _launchDetector.update(control_interval, _body_acceleration(0), &_mavlink_log_pub); + _launchDetector.update(control_interval, _body_acceleration(0)); } } else { @@ -1714,7 +1702,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo _flare_states.start_time = now; _flare_states.initial_height_rate_setpoint = _tecs.hgt_rate_setpoint(); _flare_states.initial_throttle_setpoint = _att_sp.thrust_body[0]; - mavlink_log_info(&_mavlink_log_pub, "Landing, flaring\t"); events::send(events::ID("fixedwing_position_control_landing_flaring"), events::Log::Info, "Landing, flaring"); } @@ -2222,7 +2209,6 @@ FixedwingPositionControl::Run() } if (!valid_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/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 3b9f144289..e74652031c 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -191,8 +191,6 @@ public: private: void Run() override; - orb_advert_t _mavlink_log_pub{nullptr}; - uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; diff --git a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp index d84178c371..e841fccf1c 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp +++ b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp @@ -40,13 +40,12 @@ #include "LaunchDetector.h" #include -#include #include namespace launchdetection { -void LaunchDetector::update(const float dt, const float accel_x, orb_advert_t *mavlink_log_pub) +void LaunchDetector::update(const float dt, const float accel_x) { switch (state_) { case launch_detection_status_s::STATE_WAITING_FOR_LAUNCH: @@ -55,7 +54,6 @@ void LaunchDetector::update(const float dt, const float accel_x, orb_advert_t * /* Inform user that launchdetection is running every kInfoDelay seconds */ if (info_delay_counter_s_ >= kInfoDelay) { - mavlink_log_info(mavlink_log_pub, "Launch detection running\t"); events::send(events::ID("launch_detection_running_info"), events::Log::Info, "Launch detection running"); info_delay_counter_s_ = 0.f; // reset counter } @@ -67,15 +65,12 @@ void LaunchDetector::update(const float dt, const float accel_x, orb_advert_t * if (acceleration_detected_counter_ > param_fw_laun_ac_t_.get()) { if (param_fw_laun_mot_del_.get() > 0.f) { state_ = launch_detection_status_s::STATE_LAUNCH_DETECTED_DISABLED_MOTOR; - mavlink_log_info(mavlink_log_pub, "Launch detected: enable control, waiting %8.1fs until throttling up\t", - (double)param_fw_laun_mot_del_.get()); events::send(events::ID("launch_detection_wait_for_throttle"), {events::Log::Info, events::LogInternal::Info}, "Launch detected: enablecontrol, waiting {1:.1}s until full throttle", (double)param_fw_laun_mot_del_.get()); } else { /* No motor delay set: go directly to enablemotors state */ state_ = launch_detection_status_s::STATE_FLYING; - mavlink_log_info(mavlink_log_pub, "Launch detected: enable motors (no motor delay)\t"); events::send(events::ID("launch_detection_no_motor_delay"), {events::Log::Info, events::LogInternal::Info}, "Launch detected: enable motors (no motor delay)"); } @@ -93,7 +88,6 @@ void LaunchDetector::update(const float dt, const float accel_x, orb_advert_t * motor_delay_counter_ += dt; if (motor_delay_counter_ > param_fw_laun_mot_del_.get()) { - mavlink_log_info(mavlink_log_pub, "Launch detected: enable motors\t"); events::send(events::ID("launch_detection_enable_motors"), {events::Log::Info, events::LogInternal::Info}, "Launch detected: enable motors"); state_ = launch_detection_status_s::STATE_FLYING; diff --git a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h index 53e568006f..698b56a0f1 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h +++ b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h @@ -69,9 +69,8 @@ public: * * @param dt Time step [us] * @param accel_x Measured acceleration in body x [m/s/s] - * @param mavlink_log_pub */ - void update(const float dt, const float accel_x, orb_advert_t *mavlink_log_pub); + void update(const float dt, const float accel_x); /** * @brief Get the Launch Detected state 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 455b78dd30..9b67f3a3c3 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp @@ -43,7 +43,6 @@ #include #include "RunwayTakeoff.h" -#include #include #include @@ -63,7 +62,7 @@ void RunwayTakeoff::init(const hrt_abstime &time_now, const float initial_yaw, c } void RunwayTakeoff::update(const hrt_abstime &time_now, const float takeoff_airspeed, const float calibrated_airspeed, - const float vehicle_altitude, const float clearance_altitude, orb_advert_t *mavlink_log_pub) + const float vehicle_altitude, const float clearance_altitude) { switch (takeoff_state_) { case RunwayTakeoffState::THROTTLE_RAMP: @@ -80,7 +79,6 @@ void RunwayTakeoff::update(const hrt_abstime &time_now, const float takeoff_airs if (calibrated_airspeed > rotation_airspeed) { takeoff_time_ = time_now; takeoff_state_ = RunwayTakeoffState::CLIMBOUT; - mavlink_log_info(mavlink_log_pub, "Takeoff airspeed reached, climbout\t"); events::send(events::ID("runway_takeoff_reached_airspeed"), events::Log::Info, "Takeoff airspeed reached, climbout"); } @@ -91,7 +89,6 @@ void RunwayTakeoff::update(const hrt_abstime &time_now, const float takeoff_airs case RunwayTakeoffState::CLIMBOUT: if (vehicle_altitude > clearance_altitude) { takeoff_state_ = RunwayTakeoffState::FLY; - mavlink_log_info(mavlink_log_pub, "Reached clearance altitude\t"); events::send(events::ID("runway_takeoff_reached_clearance_altitude"), events::Log::Info, "Reached clearance altitude"); } diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h index 532a1dd33b..94fc02735e 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h @@ -47,7 +47,6 @@ #include #include -#include #include #include @@ -84,10 +83,9 @@ public: * @param calibrated_airspeed Vehicle calibrated airspeed [m/s] * @param vehicle_altitude Vehicle altitude (AGL) [m] * @param clearance_altitude Altitude (AGL) above which we have cleared all occlusions in the runway path [m] - * @param mavlink_log_pub */ void update(const hrt_abstime &time_now, const float takeoff_airspeed, const float calibrated_airspeed, - const float vehicle_altitude, const float clearance_altitude, orb_advert_t *mavlink_log_pub); + const float vehicle_altitude, const float clearance_altitude); /** * @return Current takeoff state