mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 17:38:32 -04:00
PLane: support Loiter to Qland battery failsafe action
This commit is contained in:
parent
69624b1c1b
commit
682798d044
@ -1129,7 +1129,10 @@ private:
|
|||||||
#if HAL_QUADPLANE_ENABLED
|
#if HAL_QUADPLANE_ENABLED
|
||||||
Failsafe_Action_QLand = 4,
|
Failsafe_Action_QLand = 4,
|
||||||
#endif
|
#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
|
// list of priorities, highest priority first
|
||||||
|
@ -203,13 +203,20 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
|
|||||||
{
|
{
|
||||||
switch ((Failsafe_Action)action) {
|
switch ((Failsafe_Action)action) {
|
||||||
#if HAL_QUADPLANE_ENABLED
|
#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:
|
case Failsafe_Action_QLand:
|
||||||
if (quadplane.available()) {
|
if (quadplane.available()) {
|
||||||
plane.set_mode(mode_qland, ModeReason::BATTERY_FAILSAFE);
|
plane.set_mode(mode_qland, ModeReason::BATTERY_FAILSAFE);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
FALLTHROUGH;
|
FALLTHROUGH;
|
||||||
#endif
|
#endif // HAL_QUADPLANE_ENABLED
|
||||||
case Failsafe_Action_Land: {
|
case Failsafe_Action_Land: {
|
||||||
bool already_landing = flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND;
|
bool already_landing = flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND;
|
||||||
#if HAL_QUADPLANE_ENABLED
|
#if HAL_QUADPLANE_ENABLED
|
||||||
|
Loading…
Reference in New Issue
Block a user