Browse Source

ArduPlane: use an enumeration for the AP_Avoidance recovery action

c415-sdk
Peter Barker 5 years ago committed by Peter Barker
parent
commit
306eaf4d03
  1. 14
      ArduPlane/avoidance_adsb.cpp
  2. 2
      ArduPlane/avoidance_adsb.h

14
ArduPlane/avoidance_adsb.cpp

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

2
ArduPlane/avoidance_adsb.h

@ -20,7 +20,7 @@ protected:
MAV_COLLISION_ACTION handle_avoidance(const AP_Avoidance::Obstacle *obstacle, MAV_COLLISION_ACTION requested_action) override; MAV_COLLISION_ACTION handle_avoidance(const AP_Avoidance::Obstacle *obstacle, MAV_COLLISION_ACTION requested_action) override;
// override recovery handler // override recovery handler
void handle_recovery(uint8_t recovery_action) override; void handle_recovery(RecoveryAction recovery_action) override;
// check flight mode is avoid_adsb // check flight mode is avoid_adsb
bool check_flightmode(bool allow_mode_change); bool check_flightmode(bool allow_mode_change);

Loading…
Cancel
Save