Plane: Add support for mode change to QLand as a failsafe

This commit is contained in:
Michael du Breuil 2018-11-23 16:00:31 -07:00 committed by Tom Pittenger
parent 2c9ba1d954
commit 1e220d3d37
3 changed files with 13 additions and 5 deletions

View File

@ -106,7 +106,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
#endif
};
constexpr int8_t Plane::_failsafe_priorities[5];
constexpr int8_t Plane::_failsafe_priorities[6];
void Plane::setup()
{

View File

@ -1074,12 +1074,14 @@ private:
Failsafe_Action_None = 0,
Failsafe_Action_RTL = 1,
Failsafe_Action_Land = 2,
Failsafe_Action_Terminate = 3
Failsafe_Action_Terminate = 3,
Failsafe_Action_QLand = 4,
};
// list of priorities, highest priority first
static constexpr int8_t _failsafe_priorities[] = {
Failsafe_Action_Terminate,
Failsafe_Action_QLand,
Failsafe_Action_Land,
Failsafe_Action_RTL,
Failsafe_Action_None,

View File

@ -141,17 +141,23 @@ void Plane::failsafe_long_off_event(mode_reason_t reason)
void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
{
switch ((Failsafe_Action)action) {
case Failsafe_Action_QLand:
if (quadplane.available()) {
plane.set_mode(QLAND, MODE_REASON_BATTERY_FAILSAFE);
break;
}
FALLTHROUGH;
case Failsafe_Action_Land:
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND || control_mode == QLAND) {
// never stop a landing if we were already committed
if (plane.mission.jump_to_landing_sequence()) {
plane.set_mode(AUTO, MODE_REASON_UNKNOWN);
plane.set_mode(AUTO, MODE_REASON_BATTERY_FAILSAFE);
break;
}
}
FALLTHROUGH;
case Failsafe_Action_RTL:
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND || control_mode == QLAND ) {
// never stop a landing if we were already committed
set_mode(RTL, MODE_REASON_BATTERY_FAILSAFE);
aparm.throttle_cruise.load();