dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
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;
}