fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-08-21 10:00:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor(fw_rate_control): move motor stopping logic to FW rate controller

Browse Source 
Signed-off-by: Silvan <silvan@auterion.com>
This commit is contained in:
Silvan
2026-02-16 17:46:46 +01:00
committed by Balduin
parent a0ee7b4eb9
commit fbfff689a2
1 changed files with 18 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_rate_control/FixedwingRateControl.cpp
+18 -9
View File
@@ -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