diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 2a5e5ac0e8..2648b8c7ae 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -630,17 +630,6 @@ private: #endif AP_ADSB adsb {ahrs}; - struct { - - // flag to signify the current mode is set by ADSB evasion logic - bool is_evading:1; - - // generic timestamp for evasion algorithms - uint32_t timestamp_ms; - - // previous wp to restore to when switching between modes back to AUTO - Location prev_wp; - } adsb_state; // Outback Challenge Failsafe Support #if OBC_FAILSAFE == ENABLED @@ -1017,9 +1006,6 @@ private: void update_logging2(void); void terrain_update(void); void adsb_update(void); - bool adsb_evasion_start(void); - void adsb_evasion_stop(void); - void adsb_evasion_ongoing(void); void update_flight_mode(void); void stabilize(); void set_servos_idle(void); diff --git a/ArduPlane/adsb.cpp b/ArduPlane/adsb.cpp index d641556964..a28e296b1d 100644 --- a/ArduPlane/adsb.cpp +++ b/ArduPlane/adsb.cpp @@ -27,100 +27,10 @@ */ /* - handle periodic adsb database maintenance and handle threats + handle periodic adsb database maintenance */ void Plane::adsb_update(void) { adsb.update(); - - if (!adsb.get_is_evading_threat()) { - if (adsb.get_possible_threat()) { - bool evasion_start_success = adsb_evasion_start(); - adsb.set_is_evading_threat(evasion_start_success); - } - } else if (adsb_state.is_evading) { - // there has been no manual override, handle an auto-evasion - if (!adsb.get_possible_threat()) { - adsb.set_is_evading_threat(false); - adsb_evasion_stop(); - adsb_state.is_evading = false; - } else { - adsb_evasion_ongoing(); - } - } } -/* - * This fires once at the moment we realize there is a threat - */ -bool Plane::adsb_evasion_start(void) -{ - if (control_mode != AUTO) { - // evasion is only supported while in AUTO mode - return false; - } - if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF || - mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) { - // evasion during a plane take-off or land can result in a crash. Ignore ADS-B traffic - return false; - } - - adsb_state.timestamp_ms = millis(); - adsb_state.prev_wp = prev_WP_loc; // must be done BEFORE set_mode() - - switch(adsb.get_behavior()) { - case AP_ADSB::ADSB_BEHAVIOR_NONE: - default: - gcs_send_text(MAV_SEVERITY_CRITICAL, "ADS-B threat found, no action taken"); - break; - - case AP_ADSB::ADSB_BEHAVIOR_GUIDED: - gcs_send_text(MAV_SEVERITY_CRITICAL, "ADS-B threat found, switching to GUIDED mode"); - set_mode(GUIDED); - adsb_state.is_evading = true; // must be done AFTER set_mode() - break; - - case AP_ADSB::ADSB_BEHAVIOR_LOITER: - case AP_ADSB::ADSB_BEHAVIOR_LOITER_AND_DESCEND: - gcs_send_text(MAV_SEVERITY_CRITICAL, "ADS-B threat found, performing LOITER"); - set_mode(LOITER); - adsb_state.is_evading = true; // must be done after set_mode() - break; - } // switch behavior - - return true; -} - -/* - * This fires once at the moment we realize there is no longer a threat - */ -void Plane::adsb_evasion_stop(void) -{ - gcs_send_text(MAV_SEVERITY_CRITICAL, "ADS-B threat gone, continuing mission"); - - FlightMode prev_control_mode = control_mode; - set_mode(AUTO); - if (prev_control_mode == LOITER) - { - // if resuming from loiter, smoothly get back on track - prev_WP_loc = adsb_state.prev_wp; - auto_state.no_crosstrack = false; - } -} - -/* - * This fires once per second while evading a threat - */ -void Plane::adsb_evasion_ongoing(void) -{ - uint32_t now = millis(); - - if (control_mode == LOITER && - adsb.get_behavior() == AP_ADSB::ADSB_BEHAVIOR_LOITER_AND_DESCEND && - now - adsb_state.timestamp_ms >= 1000) - { - // slowly reduce altitude 100 cm/s while loitering. Drive into the ground if threat persists - adsb_state.timestamp_ms = now; - next_WP_loc.alt -= 100; - } -} diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index d32487b075..51a9917abe 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -344,13 +344,6 @@ void Plane::set_mode(enum FlightMode mode) guided_state.last_forced_rpy_ms.zero(); guided_state.last_forced_throttle_ms = 0; - // always reset this because we don't know who called set_mode. In evasion - // behavior you should set this flag after set_mode so you know the evasion - // logic is controlling the mode. This allows manual override of the mode - // to exit evasion behavior automatically but if the mode is manually switched - // then we won't resume AUTO after an evasion - adsb_state.is_evading = false; - // set mode previous_mode = control_mode; control_mode = mode;