mirror of https://github.com/ArduPilot/ardupilot
Plane: Quadplane: continue in mode loiter to Qland on failsafe, as with Qland
This commit is contained in:
parent
085c5c815b
commit
bfbd0d93c1
|
@ -70,9 +70,6 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
|
|||
case Mode::Number::AVOID_ADSB:
|
||||
case Mode::Number::GUIDED:
|
||||
case Mode::Number::LOITER:
|
||||
#if HAL_QUADPLANE_ENABLED
|
||||
case Mode::Number::LOITER_ALT_QLAND:
|
||||
#endif
|
||||
case Mode::Number::THERMAL:
|
||||
if(g.fs_action_short != FS_ACTION_SHORT_BESTGUESS) {
|
||||
failsafe.saved_mode_number = control_mode->mode_number();
|
||||
|
@ -91,6 +88,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
|
|||
#if HAL_QUADPLANE_ENABLED
|
||||
case Mode::Number::QLAND:
|
||||
case Mode::Number::QRTL:
|
||||
case Mode::Number::LOITER_ALT_QLAND:
|
||||
#endif
|
||||
case Mode::Number::INITIALISING:
|
||||
break;
|
||||
|
@ -117,9 +115,6 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
|
|||
case Mode::Number::TRAINING:
|
||||
case Mode::Number::CIRCLE:
|
||||
case Mode::Number::LOITER:
|
||||
#if HAL_QUADPLANE_ENABLED
|
||||
case Mode::Number::LOITER_ALT_QLAND:
|
||||
#endif
|
||||
case Mode::Number::THERMAL:
|
||||
if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
|
||||
#if PARACHUTE == ENABLED
|
||||
|
@ -172,6 +167,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
|
|||
#if HAL_QUADPLANE_ENABLED
|
||||
case Mode::Number::QLAND:
|
||||
case Mode::Number::QRTL:
|
||||
case Mode::Number::LOITER_ALT_QLAND:
|
||||
#endif
|
||||
case Mode::Number::TAKEOFF:
|
||||
case Mode::Number::INITIALISING:
|
||||
|
@ -223,7 +219,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
|
|||
case Failsafe_Action_Land: {
|
||||
bool already_landing = flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND;
|
||||
#if HAL_QUADPLANE_ENABLED
|
||||
if (control_mode == &mode_qland) {
|
||||
if (control_mode == &mode_qland || control_mode == &mode_lotier_qland) {
|
||||
already_landing = true;
|
||||
}
|
||||
#endif
|
||||
|
@ -244,7 +240,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
|
|||
case Failsafe_Action_RTL: {
|
||||
bool already_landing = flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND;
|
||||
#if HAL_QUADPLANE_ENABLED
|
||||
if (control_mode == &mode_qland ||
|
||||
if (control_mode == &mode_qland || control_mode == &mode_lotier_qland ||
|
||||
quadplane.in_vtol_land_sequence()) {
|
||||
already_landing = true;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue