fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 01:53:34 +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-03-10 09:08:12 +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); trim.copyTo(_vehicle_torque_setpoint.xyz);
} }
/* throttle passed through if it is finite */ float thrust_setpoint = _rates_sp.thrust_body[0];
_vehicle_thrust_setpoint.xyz[0] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? _rates_sp.thrust_body[0] : 0.0f;
/* scale effort by battery status */ // Stop motor if its setpoint is below 2%. This value was determined empirically (RC stick inaccuracy).
if (_param_fw_bat_scale_en.get() && _vehicle_thrust_setpoint.xyz[0] > 0.1f) { // 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()) { if (_battery_status_sub.updated()) {
battery_status_s battery_status{}; battery_status_s battery_status{};
if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) {
_battery_scale = battery_status.scale; _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 // publish rate controller status