Merge branch 'odid-scanner' into use-mavlink-structs

Tools/scripts/emulate_odid_recv.py

@@ -0,0 +1,9 @@
+from pymavlink import mavutil
+import pymavlink as mavlink
+import time
+
+if __name__ == '__main__':
+    mav = mavutil.mavlink_connection('tcp:localhost:5762')
+    while True:
+        time.sleep(1)
+        mav.mav.uav_found_send(0,0,0,0,0,0)

libraries/AP_Avoidance/AP_Avoidance.cpp

@@ -7,6 +7,7 @@ extern const AP_HAL::HAL& hal;
 #include <limits>
 #include <AP_AHRS/AP_AHRS.h>
 #include <GCS_MAVLink/GCS.h>
+#include <AP_Vehicle/AP_Vehicle.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 
 #define AVOIDANCE_DEBUGGING 0
@@ -279,6 +280,21 @@ void AP_Avoidance::get_adsb_samples()
     }
 }
 
+void AP_Avoidance::get_odid_samples()
+{
+    //TODO: Process samples here
+    for(int i = 0;i<AP::vehicle()->odidscanner.get_count();i++) {
+        auto v = AP::vehicle()->odidscanner.get_vehicle(i);
+        add_obstacle(v.last_update_ms,
+                    MAV_COLLISION_SRC_ODID,
+                    0,
+                    AP::vehicle()->odidscanner.get_vehicle_location(i),
+                    v.info.heading,
+                    v.info.hor_velocity,
+                    v.info.ver_velocity);
+    }
+}
+
 float closest_approach_xy(const Location &my_loc,
                           const Vector3f &my_vel,
                           const Location &obstacle_loc,
@@ -522,6 +538,9 @@ void AP_Avoidance::update()
     if (_adsb.enabled()) {
         get_adsb_samples();
     }
+    if (AP::vehicle()->odidscanner.enabled()) {
+        get_odid_samples();
+    }
 
     check_for_threats();
 

libraries/AP_Avoidance/AP_Avoidance.h

@@ -26,6 +26,7 @@
 */
 
 #include <AP_ADSB/AP_ADSB.h>
+#include<AP_ODIDScanner/AP_ODIDScanner.h>
 
 #if HAL_ADSB_ENABLED
 
@@ -177,6 +178,7 @@ class AP_Avoidance {
 
     // calls into the AP_ADSB library to retrieve vehicle data
     void get_adsb_samples();
+    void get_odid_samples();
 
     // returns true if the obstacle should be considered more of a
     // threat than the current most serious threat

libraries/AP_ODIDScanner/AP_ODIDScanner.cpp

@@ -16,12 +16,26 @@
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_Common/Location.h>
 
+#define VEHICLE_TIMEOUT_MS 30000
+
+bool mac_eq(uint8_t a[6], uint8_t b[6]) {
+    return a[0] && b[0] &&
+           a[1] && b[1] &&
+           a[2] && b[2] &&
+           a[3] && b[3] &&
+           a[4] && b[4] &&
+           a[5] && b[5];
+}
+
 // TODO: Random default for mav_port needs fix
 AP_ODIDScanner::AP_ODIDScanner() : _mav_port(1){
 
+}
+bool AP_ODIDScanner::enabled() {
+    return true;
 }
 void AP_ODIDScanner::init() {
-   _chan = mavlink_channel_t(gcs().get_channel_from_port_number(_mav_port));
+    _chan = mavlink_channel_t(gcs().get_channel_from_port_number(_mav_port));
     _initialised = true;
     _port = AP::serialmanager().get_serial_by_id(_mav_port);
     if (_port != nullptr) {
@@ -70,7 +84,9 @@ void AP_ODIDScanner::handle_msg(mavlink_message_t msg) {
         {
             mavlink_uav_found_t uav;
             mavlink_msg_uav_found_decode(&msg, &uav);
-            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ODIDScanner: uav's here %f,%f", uav.lat,uav.lon);
+            if(!update_vehicle(uav)) {
+                add_vehicle(uav);
+            }
             break;
         }
         case MAVLINK_MSG_ID_ODID_HEARTBEAT:
@@ -109,4 +125,116 @@ void AP_ODIDScanner::update() {
 
 bool AP_ODIDScanner::message_from_rx(mavlink_channel_t& chan) {
     return chan == _chan;
-}
+}
+void AP_ODIDScanner::update_collide() {
+    Loc loc{};
+    if (!AP::ahrs().get_location(loc)) {
+        loc.zero();
+    }
+
+    const AP_GPS &gps = AP::gps();
+
+    // TODO: Is this necessary? I am lazy.
+    // loc.fix_type = (AP_GPS_FixType)gps.status();
+    loc.epoch_us = gps.time_epoch_usec();
+#if AP_RTC_ENABLED
+    loc.have_epoch_from_rtc_us = AP::rtc().get_utc_usec(loc.epoch_from_rtc_us);
+#endif
+
+    loc.satellites = gps.num_sats();
+
+    loc.horizontal_pos_accuracy_is_valid = gps.horizontal_accuracy(loc.horizontal_pos_accuracy);
+    loc.vertical_pos_accuracy_is_valid = gps.vertical_accuracy(loc.vertical_pos_accuracy);
+    loc.horizontal_vel_accuracy_is_valid = gps.speed_accuracy(loc.horizontal_vel_accuracy);
+
+
+    loc.vel_ned = gps.velocity();
+
+    loc.vertRateD_is_valid = AP::ahrs().get_vert_pos_rate_D(loc.vertRateD);
+
+    const auto &baro = AP::baro();
+    loc.baro_is_healthy = baro.healthy();
+
+    // Altitude difference between sea level pressure and current
+    // pressure (in metres)
+    loc.baro_alt_press_diff_sea_level = baro.get_altitude_difference(SSL_AIR_PRESSURE, baro.get_pressure());
+
+    const uint32_t now = AP_HAL::millis();
+    uint16_t index = 0;
+    while (index < in_state.vehicle_count) {
+        // check list and drop stale vehicles. When disabled, the list will get flushed
+        if (now - in_state.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++;
+        }
+    }
+}
+
+void AP_ODIDScanner::delete_vehicle(const uint16_t index)
+{
+    if (index >= in_state.vehicle_count) {
+        // index out of range
+        return;
+    }
+
+    // if the vehicle is the furthest, invalidate it. It has been bumped
+    if (index == in_state.furthest_vehicle_index && in_state.furthest_vehicle_distance > 0) {
+        in_state.furthest_vehicle_distance = 0;
+        in_state.furthest_vehicle_index = 0;
+    }
+    if (index != (in_state.vehicle_count-1)) {
+        in_state.vehicle_list[index] = in_state.vehicle_list[in_state.vehicle_count-1];
+    }
+    // TODO: is memset needed? When we decrement the index we essentially forget about it
+    memset(&in_state.vehicle_list[in_state.vehicle_count-1], 0, sizeof(rid_vehicle_t));
+    in_state.vehicle_count--;
+}
+
+Location AP_ODIDScanner::get_location(rid_vehicle_t &vehicle) {
+    const Location loc = Location(
+        vehicle.info.lat,
+        vehicle.info.lon,
+        vehicle.info.alt * 0.1f,
+        Location::AltFrame::ABSOLUTE);
+    return loc;
+}
+
+bool AP_ODIDScanner::update_vehicle(mavlink_uav_found_t& uav) {
+    uint32_t now_ms = AP_HAL::millis();
+    for(int i = 0;i<count;i++) {
+        if(mac_eq(uav.mac,vehicles[i].info.mac)) {
+            vehicles[i].info = uav;
+            vehicles[i].last_update_ms = now_ms;
+            return true;
+        }
+    }
+    return false;
+}
+
+bool AP_ODIDScanner::add_vehicle(mavlink_uav_found_t &uav) {
+    uint32_t now_ms = AP_HAL::millis();
+    if(count >= DRONE_TRACK_MAX) {
+        return false;
+    } else {
+        vehicles[count].info = uav;
+        vehicles[count].last_update_ms = now_ms;
+        ++count;
+        return true;
+    }
+}
+
+int AP_ODIDScanner::get_count() {
+    return count;
+}
+
+rid_vehicle_t& AP_ODIDScanner::get_vehicle(int i) {
+    return vehicles[i];
+}
+
+Location AP_ODIDScanner::get_vehicle_location(int i) {
+    auto v = this->get_vehicle(i);
+    return Location(v.info.lat, v.info.lon, v.info.alt, Location::AltFrame::ABSOLUTE);
+}

libraries/AP_ODIDScanner/AP_ODIDScanner.h

@@ -12,17 +12,102 @@
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Common/Location.h>
 
+bool mac_eq(uint8_t a[6], uint8_t b[6]);
+
+struct Loc : Location {
+
+    uint64_t epoch_us;  // microseconds since 1970-01-01
+    uint64_t epoch_from_rtc_us;  // microseconds since 1970-01-01
+    bool have_epoch_from_rtc_us;
+    uint8_t satellites;
+
+    float horizontal_pos_accuracy;
+    bool horizontal_pos_accuracy_is_valid;
+
+    float vertical_pos_accuracy;
+    bool vertical_pos_accuracy_is_valid;
+
+    float horizontal_vel_accuracy;
+    bool horizontal_vel_accuracy_is_valid;
+
+    Vector3f vel_ned;
+
+    float vertRateD;  // m/s down
+    bool vertRateD_is_valid;
+
+    // methods to make us look much like the AP::gps() singleton:
+    const Vector3f &velocity() const {
+        return vel_ned;
+    }
+    uint64_t time_epoch_usec() const { return epoch_us; }
+
+    bool speed_accuracy(float &sacc) const;
+    bool horizontal_accuracy(float &hacc) const;
+    bool vertical_accuracy(float &vacc) const;
+
+    uint8_t num_sats() const { return satellites; }
+
+    // methods to make us look like the AP::ahrs() singleton:
+    const Vector2f &groundspeed_vector() const { return vel_ned.xy(); }
+    bool get_vert_pos_rate_D(float &velocity) const {
+        velocity = vertRateD;
+        return vertRateD_is_valid;
+    }
+
+    // data from a pressure sensor:
+    bool baro_is_healthy;
+    float baro_alt_press_diff_sea_level;
+};
+
+struct rid_vehicle_t {
+    mavlink_uav_found_t info;
+    uint32_t last_update_ms;
+};
+#define DRONE_TRACK_MAX 10
+
 class AP_ODIDScanner
 {
 public:
+    struct {
+        // list management
+        AP_Int16    list_size_param;
+        uint16_t    list_size_allocated;
+        rid_vehicle_t *vehicle_list;
+        uint16_t    vehicle_count;
+        AP_Int32    list_radius;
+        AP_Int16    list_altitude;
+
+        // index of and distance to furthest vehicle in list
+        uint16_t    furthest_vehicle_index;
+        float       furthest_vehicle_distance;
+
+        // streamrate stuff
+        uint32_t    send_start_ms[MAVLINK_COMM_NUM_BUFFERS];
+        uint16_t    send_index[MAVLINK_COMM_NUM_BUFFERS];
+    } in_state;
+
+    rid_vehicle_t vehicles[DRONE_TRACK_MAX];
+    int count = 0;
+    int get_count();
+    bool update_vehicle(mavlink_uav_found_t &);
+    bool add_vehicle(mavlink_uav_found_t &);
+    rid_vehicle_t&  get_vehicle(int i);
+    Location get_vehicle_location(int i);
+
+
     AP_ODIDScanner();
 
     /* Do not allow copies */
     CLASS_NO_COPY(AP_ODIDScanner);
     void init();
     void update();
     void update_recv();
+    void update_collide();
+    Location get_location(const rid_vehicle_t &vehicle);
+    void delete_vehicle(const uint16_t index);
     void handle_msg(mavlink_message_t);
+    Location get_location(rid_vehicle_t &);
+    bool enabled();
     // mavlink_channel_t _chan; // mavlink channel that communicates with the remote id transceiver
     uint8_t _mav_port;
     bool message_from_rx(mavlink_channel_t& chan);
@@ -43,4 +128,5 @@ class AP_ODIDScanner
     mavlink_status_t *channel_status() { return &_channel_status; }
 };
 
+
 #endif