mirror of https://github.com/ArduPilot/ardupilot
Plane: Prevent FS action overiding VTOL land
This commit is contained in:
parent
1e0dfaa1fe
commit
ad9e6f02cd
|
@ -171,7 +171,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
|
|||
}
|
||||
FALLTHROUGH;
|
||||
case Failsafe_Action_RTL:
|
||||
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland ) {
|
||||
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland && !quadplane.in_vtol_land_sequence()) {
|
||||
// never stop a landing if we were already committed
|
||||
set_mode(mode_rtl, MODE_REASON_BATTERY_FAILSAFE);
|
||||
aparm.throttle_cruise.load();
|
||||
|
|
|
@ -3163,3 +3163,11 @@ bool QuadPlane::in_vtol_land_final(void) const
|
|||
{
|
||||
return in_vtol_land_descent() && poscontrol.state == QPOS_LAND_FINAL;
|
||||
}
|
||||
|
||||
/*
|
||||
see if we are in any of the phases of a VTOL landing
|
||||
*/
|
||||
bool QuadPlane::in_vtol_land_sequence(void) const
|
||||
{
|
||||
return in_vtol_land_approach() || in_vtol_land_descent() || in_vtol_land_final();
|
||||
}
|
||||
|
|
|
@ -550,6 +550,11 @@ private:
|
|||
are we in the final landing phase of a VTOL landing?
|
||||
*/
|
||||
bool in_vtol_land_final(void) const;
|
||||
|
||||
/*
|
||||
are we in any of the phases of a VTOL landing?
|
||||
*/
|
||||
bool in_vtol_land_sequence(void) const;
|
||||
|
||||
public:
|
||||
void motor_test_output();
|
||||
|
|
Loading…
Reference in New Issue