Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 17:53:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: add new avoidance recovery modes

Browse Source
This commit is contained in:
Tom Pittenger 2016-08-17 00:17:05 -07:00
parent 2da33e41e9
commit 5c1d49b985
2 changed files with 38 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
ArduCopter/avoidance_adsb.cpp
View File

@ -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);
}
}
}

5
ArduCopter/avoidance_adsb.h
View File

@ -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