diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 9caeac1043..f6dc411f05 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -403,22 +403,31 @@ void FixedwingRateControl::Run() trim.copyTo(_vehicle_torque_setpoint.xyz); } - /* throttle passed through if it is finite */ - _vehicle_thrust_setpoint.xyz[0] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? _rates_sp.thrust_body[0] : 0.0f; + float thrust_setpoint = _rates_sp.thrust_body[0]; - /* scale effort by battery status */ - if (_param_fw_bat_scale_en.get() && _vehicle_thrust_setpoint.xyz[0] > 0.1f) { + // Stop motor if its setpoint is below 2%. This value was determined empirically (RC stick inaccuracy). + // Motor is stopped by setting the output to NAN (per definition). + if (PX4_ISFINITE(thrust_setpoint) && thrust_setpoint > 0.02f) { + /* scale effort by battery status */ + if (_param_fw_bat_scale_en.get()) { - if (_battery_status_sub.updated()) { - battery_status_s battery_status{}; + if (_battery_status_sub.updated()) { + battery_status_s battery_status{}; - if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { - _battery_scale = battery_status.scale; + if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { + _battery_scale = battery_status.scale; + } } + + thrust_setpoint *= _battery_scale; } - _vehicle_thrust_setpoint.xyz[0] *= _battery_scale; + } else { + thrust_setpoint = NAN; } + + _vehicle_thrust_setpoint.xyz[0] = thrust_setpoint; + } // publish rate controller status