AP_ADSB: new ADS-B library support

- contains a list of ADS-B vehicles
- does threat analysis and sets a flag when threat is found
- dynamically allocated database, consumes 40*25 =1kB of RAM when param ADSB_ENABLE = 1

libraries/AP_ADSB/AP_ADSB.cpp

@@ -0,0 +1,228 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   This program 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 program 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/>.
+ */
+
+/*
+    AP_ADSB.cpp
+
+    ADS-B RF based collision avoidance module
+    https://en.wikipedia.org/wiki/Automatic_dependent_surveillance_%E2%80%93_broadcast
+*/
+
+#include <AP_HAL/AP_HAL.h>
+#include "AP_ADSB.h"
+
+extern const AP_HAL::HAL& hal;
+
+// table of user settable parameters
+const AP_Param::GroupInfo AP_ADSB::var_info[] = {
+    // @Param: ENABLE
+    // @DisplayName: Enable
+    // @Description: Enable ADS-B
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO("ENABLE",     0, AP_ADSB, _enabled,    0),
+
+    // @Param: BEHAVIOR
+    // @DisplayName: Collision Avoidance Behavior
+    // @Description: Collision Avoidance Behavior selector
+    // @Values: 0:None,1:Loiter,2:LoiterAndDescend
+    // @User: Advanced
+    AP_GROUPINFO("BEHAVIOR",   1, AP_ADSB, _behavior, ADSB_BEHAVIOR_NONE),
+
+    AP_GROUPEND
+};
+
+/*
+ * Initialize variables and allocate memory for array
+ */
+void AP_ADSB::init(void)
+{
+    if (_vehicle_list == NULL) {
+        _vehicle_list = new adsb_vehicle_t[VEHICLE_LIST_LENGTH];
+
+        if (_vehicle_list == NULL) {
+            // dynamic RAM allocation of _vehicle_list[] failed, disable gracefully
+            hal.console->printf("Unable to initialize ADS-B vehicle list\n");
+            _enabled.set_and_save(0);
+        }
+    }
+    _vehicle_count = 0;
+    _furthest_vehicle_index = 0;
+    _furthest_vehicle_distance = 0;
+    _another_vehicle_within_radius = false;
+    _is_evading_threat = false;
+}
+
+/*
+ * de-initialize and free up some memory
+ */
+void AP_ADSB::deinit(void)
+{
+    if (_vehicle_list != NULL) {
+        delete [] _vehicle_list;
+        _vehicle_list = NULL;
+    }
+    _vehicle_count = 0;
+}
+
+/*
+ * periodic update to handle vehicle timeouts and trigger collision detection
+ */
+void AP_ADSB::update(void)
+{
+    if (!_enabled) {
+        if (_vehicle_list != NULL) {
+            deinit();
+        }
+        // nothing to do
+        return;
+    } else if (_vehicle_list == NULL)  {
+        init();
+        return;
+    }
+
+    uint16_t index = 0;
+    while (index < _vehicle_count) {
+        // check list and drop stale vehicles. When disabled, the list will get flushed
+        if (AP_HAL::millis() - _vehicle_list[index].last_update_ms > VEHICLE_TIMEOUT_MS) {
+             // don't increment index, we want to check this same index again because the contents changed
+            // also, if we're disabled then clear the list
+            delete_vehicle(index);
+        } else {
+            index++;
+        }
+    }
+
+    perform_threat_detection();
+}
+
+/*
+ * calculate threat vectors
+ */
+void AP_ADSB::perform_threat_detection(void)
+{
+    Location my_loc;
+    if (_vehicle_count == 0 ||
+        _ahrs.get_position(my_loc) == false) {
+        // nothing to do or current location is unknown so we can't calculate any collisions
+        _another_vehicle_within_radius = false;
+        return;
+    }
+
+    bool no_threat_within_radius = true;
+    for (uint16_t index = 0; index < _vehicle_count; index++) {
+        // TODO: perform more advanced threat detection
+        Location vehicle_loc = get_location(_vehicle_list[index]);
+
+        // if within radius, set flag and enforce a double radius to clear flag
+        float threat_distance = get_distance(vehicle_loc, my_loc);
+        if (threat_distance <= 2*VEHICLE_THREAT_RADIUS_M) {
+            no_threat_within_radius = false;
+            if (threat_distance <= VEHICLE_THREAT_RADIUS_M) {
+                _another_vehicle_within_radius = true;
+            }
+        } // if get
+    } // for
+
+    if (no_threat_within_radius) {
+        _another_vehicle_within_radius = false;
+    }
+
+}
+
+/*
+ * Convert/Extract a Location from a vehicle
+ */
+Location AP_ADSB::get_location(const adsb_vehicle_t &vehicle) const
+{
+    Location loc;
+    loc.alt = vehicle.info.altitude * 100;
+    loc.lat = vehicle.info.lat * 1e7;
+    loc.lng = vehicle.info.lon * 1e7;
+    loc.flags.relative_alt = false;
+    return loc;
+}
+
+/*
+ *  delete a vehicle by copying last vehicle to
+ *  current index then decrementing count
+ */
+void AP_ADSB::delete_vehicle(uint16_t index)
+{
+    if (index < _vehicle_count) {
+        if (index != _vehicle_count-1) {
+            _vehicle_list[index] = _vehicle_list[_vehicle_count-1];
+        }
+        memset(&_vehicle_list[_vehicle_count-1], 0, sizeof(adsb_vehicle_t));
+        _vehicle_count--;
+    }
+}
+
+/*
+ * Search _vehicle_list for the given vehicle. A match
+ * depends on ICAO_address. Returns true if match found
+ * and index is populated. otherwise, return false.
+ */
+bool AP_ADSB::find_index(const adsb_vehicle_t &vehicle, uint16_t *index) const
+{
+    for (uint16_t i = 0; i < _vehicle_count; i++) {
+        if (_vehicle_list[i].info.ICAO_address == vehicle.info.ICAO_address) {
+            *index = i;
+            return true;
+        }
+    }
+    return false;
+}
+
+/*
+ * Update the vehicle list. If the vehicle is already in the
+ * list then it will update it, otherwise it will be added.
+ */
+void AP_ADSB::update_vehicle(const mavlink_message_t* packet)
+{
+    if (_vehicle_list == NULL) {
+        // We are only null when disabled. Updating is inhibited.
+        return;
+    }
+
+    uint16_t index;
+    adsb_vehicle_t vehicle {};
+
+    mavlink_msg_adsb_vehicle_decode(packet, &vehicle.info);
+
+    if (find_index(vehicle, &index)) {
+        // found, update it
+        set_vehicle(index, vehicle);
+    } else if (_vehicle_count < VEHICLE_LIST_LENGTH-1) {
+        // not found, add it if there's room
+        set_vehicle(_vehicle_count, vehicle);
+        _vehicle_count++;
+    } else {
+        // TODO: buffer is full, delete the vehicle that is furthest away since it is probably not a useful threat to monitor
+    }
+}
+
+/*
+ * Copy a vehicle's data into the list
+ */
+void AP_ADSB::set_vehicle(uint16_t index, const adsb_vehicle_t &vehicle)
+{
+    if (index < VEHICLE_LIST_LENGTH) {
+        _vehicle_list[index] = vehicle;
+        _vehicle_list[index].last_update_ms = AP_HAL::millis();
+    }
+}
+

libraries/AP_ADSB/AP_ADSB.h

@@ -0,0 +1,106 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef AP_ADSB_H
+#define AP_ADSB_H
+/*
+   This program 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 program 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/>.
+ */
+/*
+    ADS-B RF based collision avoidance module
+    https://en.wikipedia.org/wiki/Automatic_dependent_surveillance_%E2%80%93_broadcast
+
+  Tom Pittenger, November 2015
+*/
+
+#include <AP_Common/AP_Common.h>
+#include <AP_Param/AP_Param.h>
+#include <GCS_MAVLink/GCS.h>
+
+#define VEHICLE_THREAT_RADIUS_M         200
+#define VEHICLE_LIST_LENGTH             25      // # of ADS-B vehicles to remember. Additional ones are ignored
+#define VEHICLE_TIMEOUT_MS              10000   // if no updates in this time, drop it from the list
+
+class AP_ADSB
+{
+public:
+    enum ADSB_BEHAVIOR {
+        ADSB_BEHAVIOR_NONE = 0,
+        ADSB_BEHAVIOR_LOITER = 1,
+        ADSB_BEHAVIOR_LOITER_AND_DESCEND = 2
+    };
+
+    struct adsb_vehicle_t {
+        mavlink_adsb_vehicle_t info; // the whole mavlink struct with all the juicy details. sizeof() == 40
+        uint32_t last_update_ms; // last time this was refreshed, allows timeouts
+    };
+
+
+    // Constructor
+    AP_ADSB(AP_AHRS &ahrs) :
+        _ahrs(ahrs)
+    {
+        AP_Param::setup_object_defaults(this, var_info);
+    }
+
+    // for holding parameters
+    static const struct AP_Param::GroupInfo var_info[];
+
+    // periodic task that maintains vehicle_list
+    void update(void);
+
+    // add or update vehicle_list from inbounc mavlink msg
+    void update_vehicle(const mavlink_message_t* msg);
+
+    bool get_another_vehicle_within_radius()  { return _enabled && _another_vehicle_within_radius; }
+
+    ADSB_BEHAVIOR get_behavior()  { return (ADSB_BEHAVIOR)(_behavior.get()); }
+    bool get_is_evading_threat()  { return _enabled && _is_evading_threat; }
+    void set_is_evading_threat(bool is_evading) { if (_enabled) { _is_evading_threat = is_evading; } }
+
+private:
+
+    // initialize _vehicle_list
+    void init();
+
+    // free _vehicle_list
+    void deinit();
+
+    // compares current vector against vehicle_list to detect threats
+    void perform_threat_detection(void);
+
+    // extract a location out of a vehicle item
+    Location get_location(const adsb_vehicle_t &vehicle) const;
+
+    // return index of given vehicle if ICAO_ADDRESS matches. return -1 if no match
+    bool find_index(const adsb_vehicle_t &vehicle, uint16_t *index) const;
+
+    // remove a vehicle from the list
+    void delete_vehicle(uint16_t index);
+
+    void set_vehicle(uint16_t index, const adsb_vehicle_t &vehicle);
+
+    // reference to AHRS, so we can ask for our position,
+    // heading and speed
+    const AP_AHRS &_ahrs;
+
+    AP_Int8     _enabled;
+    AP_Int8     _behavior;
+    adsb_vehicle_t *_vehicle_list;
+    uint16_t    _vehicle_count = 0;
+    uint16_t    _furthest_vehicle_index = 0;
+    float       _furthest_vehicle_distance = 0;
+    bool        _another_vehicle_within_radius = false;
+    bool        _is_evading_threat = false;
+};
+#endif // AP_ADSB_H