forked from Archive/PX4-Autopilot
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:
parent
67b2c835e0
commit
c267cf71c3
|
@ -789,12 +789,16 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
|
||||||
_control_mode_current = FW_POSCTRL_MODE_AUTO;
|
_control_mode_current = FW_POSCTRL_MODE_AUTO;
|
||||||
}
|
}
|
||||||
|
|
||||||
} else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_climb_rate_enabled
|
} else if (_control_mode.flag_control_auto_enabled
|
||||||
&& pos_sp_curr_valid) {
|
&& _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
|
if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE
|
||||||
&& commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE) {
|
&& 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;
|
_time_in_fixed_bank_loiter = now;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue