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

FW Position Control: fix entering of no-position-estimate failsafes

Browse Source 
Affects the states AUTO_ALTITUDE and AUTO_CLIMBRATE. Those modes should only be entered
if armed (as they are pure failsafe modes). Also allow though to enter them even if
the position setpoint(s) are invalid, as they are not needed.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-10-20 18:22:40 +02:00
parent 67b2c835e0
commit c267cf71c3
1 changed files with 7 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+7 -3
View File
@@ -789,12 +789,16 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
_control_mode_current = FW_POSCTRL_MODE_AUTO;
}
} else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_climb_rate_enabled
&& pos_sp_curr_valid) {
} else if (_control_mode.flag_control_auto_enabled
&& _control_mode.flag_control_climb_rate_enabled
&& _control_mode.flag_armed // only enter this modes if armed, as pure failsafe modes
&& !_control_mode.flag_control_position_enabled) {
// failsafe modes engaged if position estimate is invalidated
// reset timer the first time we switch into this mode
if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE
&& commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE) {
// reset timer the first time we switch into this mode
_time_in_fixed_bank_loiter = now;
}