diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index d75aeb2b2e..355a32f0fb 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -141,19 +141,31 @@ 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 min smaller than max"); + 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; + } + + 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"); + check_ret = PX4_ERROR; + } + + return check_ret; } void diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index d6080394cd..b41181a8e6 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -353,6 +353,7 @@ private: (ParamFloat) _param_fw_airspd_max, (ParamFloat) _param_fw_airspd_min, (ParamFloat) _param_fw_airspd_trim, + (ParamFloat) _param_fw_airspd_stall, (ParamFloat) _param_fw_clmbout_diff,