Plane: Add support for handing ADS-B traffic

- parse MAVLINK_MSG_ADSB_VEHICLE msg
- new 1Hz adsb_update task to compare list against for threat detection
- perform object avoidance via loiter or loiter_and_descend. More methods are welcome!

ArduPlane/ArduPlane.cpp

@@ -84,6 +84,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
     SCHED_TASK(terrain_update,          5,    500),
     SCHED_TASK(update_is_flying_5Hz,   10,    100),
     SCHED_TASK(dataflash_periodic,      1,    300),
+    SCHED_TASK(adsb_update,            50,    500),
 };
 
 void Plane::setup() 
@@ -360,6 +361,7 @@ void Plane::terrain_update(void)
 #endif
 }
 
+
 void Plane::dataflash_periodic(void)
 {
     DataFlash.periodic_tasks();

ArduPlane/GCS_Mavlink.cpp

@@ -1899,6 +1899,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                                 (uint32_t)(new_home_loc.alt*0.01f));
         break;
     }
+
+    case MAVLINK_MSG_ID_ADSB_VEHICLE:
+        plane.adsb.update_vehicle(msg);
+        break;
     } // end switch
 } // end handle mavlink
 

ArduPlane/Parameters.cpp

@@ -1022,6 +1022,10 @@ const AP_Param::Info Plane::var_info[] = {
     GOBJECT(terrain,                "TERRAIN_", AP_Terrain),
 #endif
 
+    // @Group: ADSB_
+    // @Path: ../libraries/AP_ADSB/AP_ADSB.cpp
+    GOBJECT(adsb,                "ADSB_", AP_ADSB),
+
     // RC channel
     //-----------
     // @Group: RC1_

ArduPlane/Parameters.h

@@ -221,6 +221,7 @@ public:
         k_param_camera = 160,
         k_param_camera_mount,
         k_param_camera_mount2,      // unused
+        k_param_adsb,
 
         //
         // Battery monitoring parameters

ArduPlane/Plane.h

@@ -94,6 +94,7 @@
 #include <AP_OpticalFlow/AP_OpticalFlow.h>     // Optical Flow library
 #include <AP_RSSI/AP_RSSI.h>                   // RSSI Library
 #include <AP_Parachute/AP_Parachute.h>
+#include <AP_ADSB/AP_ADSB.h>
 
 // Configuration
 #include "config.h"
@@ -559,6 +560,17 @@ private:
     AP_Terrain terrain {ahrs, mission, rally};
 #endif
 
+    AP_ADSB adsb {ahrs};
+    struct {
+
+        // for Loiter_and_descend behavior, keeps track of rate changes
+        uint32_t time_last_alt_change_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
     APM_OBC obc {mission, barometer, gps, rcmap};
@@ -909,6 +921,8 @@ private:
     void update_logging1(void);
     void update_logging2(void);
     void terrain_update(void);
+    void adsb_update(void);
+    void adsb_handle_vehicle_threats(void);
     void update_flight_mode(void);
     void stabilize();
     void set_servos_idle(void);

ArduPlane/adsb.cpp

@@ -0,0 +1,101 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ * adsb.cpp
+ * Copyright (C) Tom Pittenger 2015
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "Plane.h"
+
+/*
+ *  this module deals with ADS-B handling for ArduPlane
+ *  ADS-B is an RF based collision avoidance protocol to tell nearby aircraft your location
+ *  https://en.wikipedia.org/wiki/Automatic_dependent_surveillance_%E2%80%93_broadcast
+ *
+ */
+
+/*
+  handle periodic adsb database maintenance and handle threats
+ */
+void Plane::adsb_update(void)
+{
+    adsb.update();
+    adsb_handle_vehicle_threats();
+}
+
+/*
+ * Handle ADS-B based threats which are platform dependent
+ */
+void Plane::adsb_handle_vehicle_threats(void)
+{
+    uint32_t now = millis();
+    AP_ADSB::ADSB_BEHAVIOR behavior = adsb.get_behavior();
+
+    switch (control_mode) {
+    case AUTO:
+        if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF) {
+            // for testing purposes ignore ADS-B traffic until we get into the air so we don't screw up the sim takeoff
+            break;
+        }
+        switch(behavior) {
+        case AP_ADSB::ADSB_BEHAVIOR_NONE:
+        default:
+            break;
+
+        case AP_ADSB::ADSB_BEHAVIOR_LOITER:
+        case AP_ADSB::ADSB_BEHAVIOR_LOITER_AND_DESCEND:
+            if (adsb.get_another_vehicle_within_radius() && !adsb.get_is_evading_threat()) {
+                adsb.set_is_evading_threat(true);
+                gcs_send_text(MAV_SEVERITY_CRITICAL, "ADS-B threat found, performing LOITER");
+                adsb_state.prev_wp = prev_WP_loc;
+                set_mode(LOITER);
+                if (behavior == AP_ADSB::ADSB_BEHAVIOR_LOITER_AND_DESCEND) {
+                    adsb_state.time_last_alt_change_ms = now;
+                }
+            }
+        } // switch behavior
+        break; // case auto
+
+    case LOITER:
+        switch(behavior) {
+        case AP_ADSB::ADSB_BEHAVIOR_NONE:
+            // TODO: recover from this
+        default:
+            break;
+
+        case AP_ADSB::ADSB_BEHAVIOR_LOITER:
+        case AP_ADSB::ADSB_BEHAVIOR_LOITER_AND_DESCEND:
+            if (adsb.get_is_evading_threat()) {
+                if (!adsb.get_another_vehicle_within_radius()) {
+                    adsb.set_is_evading_threat(false);
+                    gcs_send_text(MAV_SEVERITY_CRITICAL, "ADS-B threat gone, continuing mission");
+                    set_mode(AUTO);
+                    prev_WP_loc = adsb_state.prev_wp;
+                    auto_state.no_crosstrack = false;
+                } else if (behavior == AP_ADSB::ADSB_BEHAVIOR_LOITER_AND_DESCEND &&
+                            now - adsb_state.time_last_alt_change_ms >= 1000) {
+                   // slowly reduce altitude 1m/s while loitering. Drive into the ground if threat persists
+                    adsb_state.time_last_alt_change_ms = now;
+                    next_WP_loc.alt -= 100;
+                } // get_another_vehicle_within_radius
+            } // get_is_evading_threat
+        } // switch behavior
+        break; // case LOITER
+
+    default:
+        break;
+    } // switch control_mode
+}
+