PLane: support Loiter to Qland battery failsafe action

This commit is contained in:
Iampete1 2021-09-27 20:39:18 +01:00 committed by Andrew Tridgell
parent 69624b1c1b
commit 682798d044
2 changed files with 12 additions and 2 deletions

View File

@ -1129,7 +1129,10 @@ private:
#if HAL_QUADPLANE_ENABLED
Failsafe_Action_QLand = 4,
#endif
Failsafe_Action_Parachute = 5
Failsafe_Action_Parachute = 5,
#if HAL_QUADPLANE_ENABLED
Failsafe_Action_Loiter_alt_QLand = 6,
#endif
};
// list of priorities, highest priority first

View File

@ -203,13 +203,20 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
{
switch ((Failsafe_Action)action) {
#if HAL_QUADPLANE_ENABLED
case Failsafe_Action_Loiter_alt_QLand:
if (quadplane.available()) {
plane.set_mode(mode_lotier_qland, ModeReason::BATTERY_FAILSAFE);
break;
}
FALLTHROUGH;
case Failsafe_Action_QLand:
if (quadplane.available()) {
plane.set_mode(mode_qland, ModeReason::BATTERY_FAILSAFE);
break;
}
FALLTHROUGH;
#endif
#endif // HAL_QUADPLANE_ENABLED
case Failsafe_Action_Land: {
bool already_landing = flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND;
#if HAL_QUADPLANE_ENABLED