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

FW/CA: move motor stop logic for FW 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
parent 0da369d66b
commit 5115a66a84
2 changed files with 19 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocator.cpp
+1
View File
@@ -395,6 +395,7 @@ ControlAllocator::Run()
if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)) {
_thrust_sp = matrix::Vector3f(vehicle_thrust_setpoint.xyz);
// TODO: stop the motors on axes that have NAN
}
if (do_update) {
src/modules/fw_rate_control/FixedwingRateControl.cpp
+18 -9
View File
@@ -391,22 +391,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