From 5c1d49b985ee21cf78c61e1f7d7b58c5d16b276b Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 17 Aug 2016 00:17:05 -0700 Subject: [PATCH] Copter: add new avoidance recovery modes --- ArduCopter/avoidance_adsb.cpp | 39 +++++++++++++++++++++++++++++------ ArduCopter/avoidance_adsb.h | 5 +++++ 2 files changed, 38 insertions(+), 6 deletions(-) diff --git a/ArduCopter/avoidance_adsb.cpp b/ArduCopter/avoidance_adsb.cpp index 64894e463f..89cb56bf76 100644 --- a/ArduCopter/avoidance_adsb.cpp +++ b/ArduCopter/avoidance_adsb.cpp @@ -102,13 +102,40 @@ void AP_Avoidance_Copter::handle_recovery(uint8_t recovery_action) copter.Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_ADSB, ERROR_CODE_ERROR_RESOLVED); // restore flight mode if requested and user has not changed mode since - if (recovery_action == AP_AVOIDANCE_RECOVERY_RETURN_TO_PREVIOUS_FLIGHTMODE && copter.control_mode_reason == MODE_REASON_AVOIDANCE) { - if (!copter.set_mode(prev_control_mode, MODE_REASON_AVOIDANCE_RECOVERY)) { - // on failure RTL or LAND - if (!copter.set_mode(RTL, MODE_REASON_AVOIDANCE_RECOVERY)) { - copter.set_mode(LAND, MODE_REASON_AVOIDANCE_RECOVERY); + if (copter.control_mode_reason == MODE_REASON_AVOIDANCE) { + switch (recovery_action) { + + case AP_AVOIDANCE_RECOVERY_REMAIN_IN_AVOID_ADSB: + // do nothing, we'll stay in the AVOID_ADSB mode which is guided which will loiter forever + break; + + case AP_AVOIDANCE_RECOVERY_RESUME_PREVIOUS_FLIGHTMODE: + set_mode_else_try_RTL_else_LAND(prev_control_mode); + break; + + case AP_AVOIDANCE_RECOVERY_RTL: + set_mode_else_try_RTL_else_LAND(RTL); + break; + + case AP_AVOIDANCE_RECOVERY_RESUME_IF_AUTO_ELSE_LOITER: + if (prev_control_mode == AUTO) { + set_mode_else_try_RTL_else_LAND(AUTO); } - } + break; + + default: + break; + } // switch + } + } +} + +void AP_Avoidance_Copter::set_mode_else_try_RTL_else_LAND(control_mode_t mode) +{ + if (!copter.set_mode(mode, MODE_REASON_AVOIDANCE_RECOVERY)) { + // on failure RTL or LAND + if (!copter.set_mode(RTL, MODE_REASON_AVOIDANCE_RECOVERY)) { + copter.set_mode(LAND, MODE_REASON_AVOIDANCE_RECOVERY); } } } diff --git a/ArduCopter/avoidance_adsb.h b/ArduCopter/avoidance_adsb.h index f4e0ff40d2..f204746579 100644 --- a/ArduCopter/avoidance_adsb.h +++ b/ArduCopter/avoidance_adsb.h @@ -13,6 +13,11 @@ public: AP_Avoidance_Copter(AP_AHRS &ahrs, class AP_ADSB &adsb) : AP_Avoidance(ahrs, adsb) { } +private: + + // helper function to set modes and always succeed + void set_mode_else_try_RTL_else_LAND(control_mode_t mode); + protected: // override avoidance handler