From 7a6aa4cd092e6561726cb1bc7170d6ad223967c5 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 7 May 2021 15:31:23 +0200 Subject: [PATCH] FW pos control: Provide detailed feedback on airspeed misconfiguration. Signed-off-by: Silvan Fuhrer --- .../FixedwingPositionControl.cpp | 27 ++++++++++++------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index d75aeb2b2e..bbe431c4b6 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -141,19 +141,26 @@ FixedwingPositionControl::parameters_update() landing_status_publish(); + int check_ret = PX4_OK; + // sanity check parameters - if ((_param_fw_airspd_max.get() < _param_fw_airspd_min.get()) || - (_param_fw_airspd_max.get() < 5.0f) || - (_param_fw_airspd_min.get() > 100.0f) || - (_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, "Airspeed parameters invalid"); - - return PX4_ERROR; + if (_param_fw_airspd_max.get() < _param_fw_airspd_min.get()) { + mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max smaller than min"); + check_ret = PX4_ERROR; } - return PX4_OK; + 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"); + 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"); + check_ret = PX4_ERROR; + } + + return check_ret; } void