|
|
@ -137,11 +137,18 @@ void AP_Avoidance_Plane::handle_recovery(RecoveryAction recovery_action) |
|
|
|
case RecoveryAction::RESUME_IF_AUTO_ELSE_LOITER: |
|
|
|
case RecoveryAction::RESUME_IF_AUTO_ELSE_LOITER: |
|
|
|
if (prev_control_mode_number == Mode::Number::AUTO) { |
|
|
|
if (prev_control_mode_number == Mode::Number::AUTO) { |
|
|
|
plane.set_mode(plane.mode_auto, ModeReason::AVOIDANCE_RECOVERY); |
|
|
|
plane.set_mode(plane.mode_auto, ModeReason::AVOIDANCE_RECOVERY); |
|
|
|
|
|
|
|
} else { |
|
|
|
|
|
|
|
// let ModeAvoidADSB continue in its guided
|
|
|
|
|
|
|
|
// behaviour, but reset the loiter location,
|
|
|
|
|
|
|
|
// rather than where the avoidance location was
|
|
|
|
|
|
|
|
plane.set_guided_WP(plane.current_loc); |
|
|
|
} |
|
|
|
} |
|
|
|
// else do nothing, same as RecoveryAction::LOITER
|
|
|
|
|
|
|
|
break; |
|
|
|
break; |
|
|
|
|
|
|
|
|
|
|
|
default: |
|
|
|
default: |
|
|
|
|
|
|
|
// user has specified an invalid recovery action;
|
|
|
|
|
|
|
|
// loiter where we are
|
|
|
|
|
|
|
|
plane.set_guided_WP(plane.current_loc); |
|
|
|
break; |
|
|
|
break; |
|
|
|
} // switch
|
|
|
|
} // switch
|
|
|
|
} |
|
|
|
} |
|
|
|