AP_ADSB: add ADSB_streamrate

libraries/AP_ADSB/AP_ADSB.cpp

@@ -23,6 +23,8 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include "AP_ADSB.h"
+#include <GCS_MAVLink/GCS_MAVLink.h>
+#include <GCS_MAVLINK/GCS.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -290,3 +292,44 @@ void AP_ADSB::set_vehicle(uint16_t index, const adsb_vehicle_t &vehicle)
     }
 }
 
+void AP_ADSB::send_adsb_vehicle(mavlink_channel_t chan)
+{
+    if (_vehicle_list == NULL || !_enabled || _vehicle_count == 0) {
+        return;
+    }
+
+    uint32_t now = AP_HAL::millis();
+
+    if (send_index[chan] >= _vehicle_count) {
+        // we've finished a list
+        if (now - send_start_ms[chan] < 1000) {
+            // too soon to start a new one
+            return;
+        } else {
+            // start new list
+            send_start_ms[chan] = now;
+            send_index[chan] = 0;
+        }
+    }
+
+    if (send_index[chan] < _vehicle_count) {
+        mavlink_adsb_vehicle_t vehicle = _vehicle_list[send_index[chan]].info;
+        send_index[chan]++;
+
+        mavlink_msg_adsb_vehicle_send(chan,
+            vehicle.ICAO_address,
+            vehicle.lat,
+            vehicle.lon,
+            vehicle.altitude_type,
+            vehicle.altitude,
+            vehicle.heading,
+            vehicle.hor_velocity,
+            vehicle.ver_velocity,
+            vehicle.callsign,
+            vehicle.emitter_type,
+            vehicle.tslc,
+            vehicle.flags,
+            vehicle.squawk);
+    }
+}
+

libraries/AP_ADSB/AP_ADSB.h

@@ -75,6 +75,9 @@ public:
     void set_is_evading_threat(bool is_evading) { if (_enabled) { _is_evading_threat = is_evading; } }
     uint16_t get_vehicle_count() { return _vehicle_count; }
 
+    // send ADSB_VEHICLE mavlink message, usually as a StreamRate
+    void send_adsb_vehicle(mavlink_channel_t chan);
+
 private:
 
     // initialize _vehicle_list
@@ -115,4 +118,9 @@ private:
     // index of and distance to vehicle with highest threat
     uint16_t    _highest_threat_index = 0;
     float       _highest_threat_distance = 0;
+
+    // streamrate stuff
+    uint32_t    send_start_ms[MAVLINK_COMM_NUM_BUFFERS];
+    uint16_t    send_index[MAVLINK_COMM_NUM_BUFFERS];
+
 };