|
|
@ -97,34 +97,34 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob |
|
|
|
return actual_action; |
|
|
|
return actual_action; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void AP_Avoidance_Plane::handle_recovery(uint8_t recovery_action) |
|
|
|
void AP_Avoidance_Plane::handle_recovery(RecoveryAction recovery_action) |
|
|
|
{ |
|
|
|
{ |
|
|
|
// check we are coming out of failsafe
|
|
|
|
// check we are coming out of failsafe
|
|
|
|
if (plane.failsafe.adsb) { |
|
|
|
if (plane.failsafe.adsb) { |
|
|
|
plane.failsafe.adsb = false; |
|
|
|
plane.failsafe.adsb = false; |
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Avoid: Resuming with action: %d", recovery_action); |
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Avoid: Resuming with action: %u", (unsigned)recovery_action); |
|
|
|
|
|
|
|
|
|
|
|
// restore flight mode if requested and user has not changed mode since
|
|
|
|
// restore flight mode if requested and user has not changed mode since
|
|
|
|
if (plane.control_mode_reason == ModeReason::AVOIDANCE) { |
|
|
|
if (plane.control_mode_reason == ModeReason::AVOIDANCE) { |
|
|
|
switch (recovery_action) { |
|
|
|
switch (recovery_action) { |
|
|
|
|
|
|
|
|
|
|
|
case AP_AVOIDANCE_RECOVERY_REMAIN_IN_AVOID_ADSB: |
|
|
|
case RecoveryAction::REMAIN_IN_AVOID_ADSB: |
|
|
|
// do nothing, we'll stay in the AVOID_ADSB mode which is guided which will loiter
|
|
|
|
// do nothing, we'll stay in the AVOID_ADSB mode which is guided which will loiter
|
|
|
|
break; |
|
|
|
break; |
|
|
|
|
|
|
|
|
|
|
|
case AP_AVOIDANCE_RECOVERY_RESUME_PREVIOUS_FLIGHTMODE: |
|
|
|
case RecoveryAction::RESUME_PREVIOUS_FLIGHTMODE: |
|
|
|
plane.set_mode_by_number(prev_control_mode_number, ModeReason::AVOIDANCE_RECOVERY); |
|
|
|
plane.set_mode_by_number(prev_control_mode_number, ModeReason::AVOIDANCE_RECOVERY); |
|
|
|
break; |
|
|
|
break; |
|
|
|
|
|
|
|
|
|
|
|
case AP_AVOIDANCE_RECOVERY_RTL: |
|
|
|
case RecoveryAction::RTL: |
|
|
|
plane.set_mode(plane.mode_rtl, ModeReason::AVOIDANCE_RECOVERY); |
|
|
|
plane.set_mode(plane.mode_rtl, ModeReason::AVOIDANCE_RECOVERY); |
|
|
|
break; |
|
|
|
break; |
|
|
|
|
|
|
|
|
|
|
|
case AP_AVOIDANCE_RECOVERY_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 do nothing, same as AP_AVOIDANCE_RECOVERY_LOITER
|
|
|
|
// else do nothing, same as RecoveryAction::LOITER
|
|
|
|
break; |
|
|
|
break; |
|
|
|
|
|
|
|
|
|
|
|
default: |
|
|
|
default: |
|
|
|