AP_UAVCAN: support ADSB over UAVCAN

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -38,6 +38,7 @@
 #include <uavcan/equipment/indication/RGB565.hpp>
 #include <ardupilot/indication/SafetyState.hpp>
 #include <ardupilot/indication/Button.hpp>
+#include <ardupilot/equipment/trafficmonitor/TrafficReport.hpp>
 
 #include <AP_Baro/AP_Baro_UAVCAN.h>
 #include <AP_RangeFinder/AP_RangeFinder_UAVCAN.h>
@@ -47,6 +48,7 @@
 #include <AP_Airspeed/AP_Airspeed_UAVCAN.h>
 #include <SRV_Channel/SRV_Channel.h>
 #include <AP_OpticalFlow/AP_OpticalFlow_HereFlow.h>
+#include <AP_ADSB/AP_ADSB.h>
 
 #define LED_DELAY_US 50000
 
@@ -105,10 +107,16 @@ static uavcan::Publisher<uavcan::equipment::indication::BeepCommand>* buzzer[MAX
 static uavcan::Publisher<ardupilot::indication::SafetyState>* safety_state[MAX_NUMBER_OF_CAN_DRIVERS];
 
 // subscribers
+
 // handler SafteyButton
 UC_REGISTRY_BINDER(ButtonCb, ardupilot::indication::Button);
 static uavcan::Subscriber<ardupilot::indication::Button, ButtonCb> *safety_button_listener[MAX_NUMBER_OF_CAN_DRIVERS];
 
+// handler TrafficReport
+UC_REGISTRY_BINDER(TrafficReportCb, ardupilot::equipment::trafficmonitor::TrafficReport);
+static uavcan::Subscriber<ardupilot::equipment::trafficmonitor::TrafficReport, TrafficReportCb> *traffic_report_listener[MAX_NUMBER_OF_CAN_DRIVERS];
+
+
 AP_UAVCAN::AP_UAVCAN() :
     _node_allocator()
 {
@@ -245,6 +253,11 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
         safety_button_listener[driver_index]->start(ButtonCb(this, &handle_button));
     }
 
+    traffic_report_listener[driver_index] = new uavcan::Subscriber<ardupilot::equipment::trafficmonitor::TrafficReport, TrafficReportCb>(*_node);
+    if (traffic_report_listener[driver_index]) {
+        traffic_report_listener[driver_index]->start(TrafficReportCb(this, &handle_traffic_report));
+    }
+    
     _led_conf.devices_count = 0;
     if (enable_filters) {
         configureCanAcceptanceFilters(*_node);
@@ -568,4 +581,71 @@ void AP_UAVCAN::handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Butto
     }
 }
 
+/*
+  handle traffic report
+ */
+void AP_UAVCAN::handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb)
+{
+    AP_ADSB *adsb = AP::ADSB();
+    if (!adsb || !adsb->enabled()) {
+        // ADSB not enabled
+        return;
+    }
+
+    const ardupilot::equipment::trafficmonitor::TrafficReport &msg = cb.msg[0];
+    AP_ADSB::adsb_vehicle_t vehicle;
+    mavlink_adsb_vehicle_t &pkt = vehicle.info;
+
+    pkt.ICAO_address = msg.icao_address;
+    pkt.tslc = msg.tslc;
+    pkt.lat = msg.latitude_deg_1e7;
+    pkt.lon = msg.longitude_deg_1e7;
+    pkt.altitude = msg.alt_m * 1000;
+    pkt.heading = degrees(msg.heading) * 100;
+    pkt.hor_velocity = norm(msg.velocity[0], msg.velocity[1]) * 100;
+    pkt.ver_velocity = -msg.velocity[2] * 100;
+    pkt.squawk = msg.squawk;
+    for (uint8_t i=0; i<9; i++) {
+        pkt.callsign[i] = msg.callsign[i];
+    }
+    pkt.emitter_type = msg.traffic_type;
+
+    if (msg.alt_type == ardupilot::equipment::trafficmonitor::TrafficReport::ALT_TYPE_PRESSURE_AMSL) {
+        pkt.flags |= ADSB_FLAGS_VALID_ALTITUDE;
+        pkt.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH;
+    } else if (msg.alt_type == ardupilot::equipment::trafficmonitor::TrafficReport::ALT_TYPE_WGS84) {
+        pkt.flags |= ADSB_FLAGS_VALID_ALTITUDE;
+        pkt.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC;
+    }
+
+    if (msg.lat_lon_valid) {
+        pkt.flags |= ADSB_FLAGS_VALID_COORDS;
+    }
+    if (msg.heading_valid) {
+        pkt.flags |= ADSB_FLAGS_VALID_HEADING;
+    }
+    if (msg.velocity_valid) {
+        pkt.flags |= ADSB_FLAGS_VALID_VELOCITY;
+    }
+    if (msg.callsign_valid) {
+        pkt.flags |= ADSB_FLAGS_VALID_CALLSIGN;
+    }
+    if (msg.ident_valid) {
+        pkt.flags |= ADSB_FLAGS_VALID_SQUAWK;
+    }
+    if (msg.simulated_report) {
+        pkt.flags |= ADSB_FLAGS_SIMULATED;
+    }
+    // flags not in common.xml yet
+    if (msg.vertical_velocity_valid) {
+        pkt.flags |= 0x80;
+    }
+    if (msg.baro_valid) {
+        pkt.flags |= 0x100;
+    }
+
+    vehicle.last_update_ms = AP_HAL::millis() - (vehicle.info.tslc * 1000);
+    adsb->handle_adsb_vehicle(vehicle);
+}
+
 #endif // HAL_WITH_UAVCAN

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -48,6 +48,7 @@
 
 // fwd-declare callback classes
 class ButtonCb;
+class TrafficReportCb;
 
 /*
     Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
@@ -215,6 +216,7 @@ private:
 
     // safety button handling
     static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb);
+    static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb);
 };
 
 #endif /* AP_UAVCAN_H_ */

libraries/AP_UAVCAN/dsdl/ardupilot/equipment/trafficmonitor/20790.TrafficReport.uavcan

@@ -0,0 +1,68 @@
+# Network-synchronized timestamp, 0 if not available. Note: not necessarily a UTC time.
+uavcan.Timestamp timestamp
+
+# icao address
+uint32 icao_address
+
+# Time since last communication in seconds
+uint16 tslc
+
+# Traffic position in lat-lon-alt. Check alt_type for altitude datum
+int32 latitude_deg_1e7
+int32 longitude_deg_1e7
+float32 alt_m
+
+# Traffic heading in radians.
+# Course over ground if heading is unavailable. 0 if neither are available.
+float16 heading
+
+# Traffic velocity in m/s
+float16[3] velocity
+
+# Traffic squawk code
+uint16 squawk
+
+# Traffic callsign
+uint8[9] callsign
+
+# Traffic source
+uint3 SOURCE_ADSB = 0
+uint3 SOURCE_ADSB_UAT = 1
+uint3 SOURCE_FLARM = 2
+uint3 source
+
+# Traffic type
+uint5 TRAFFIC_TYPE_UNKNOWN = 0
+uint5 TRAFFIC_TYPE_LIGHT = 1
+uint5 TRAFFIC_TYPE_SMALL = 2
+uint5 TRAFFIC_TYPE_LARGE = 3
+uint5 TRAFFIC_TYPE_HIGH_VORTEX_LARGE = 4
+uint5 TRAFFIC_TYPE_HEAVY = 5
+uint5 TRAFFIC_TYPE_HIGHLY_MANUV = 6
+uint5 TRAFFIC_TYPE_ROTOCRAFT = 7
+uint5 TRAFFIC_TYPE_GLIDER = 9
+uint5 TRAFFIC_TYPE_LIGHTER_THAN_AIR = 10
+uint5 TRAFFIC_TYPE_PARACHUTE = 11
+uint5 TRAFFIC_TYPE_ULTRA_LIGHT = 12
+uint5 TRAFFIC_TYPE_UAV = 14
+uint5 TRAFFIC_TYPE_SPACE = 15
+uint5 TRAFFIC_TYPE_EMERGENCY_SURFACE = 17
+uint5 TRAFFIC_TYPE_SERVICE_SURFACE = 18
+uint5 TRAFFIC_TYPE_POINT_OBSTACLE = 19
+uint5 traffic_type
+
+# Altitude type
+uint7 ALT_TYPE_ALT_UNKNOWN = 0
+uint7 ALT_TYPE_PRESSURE_AMSL = 1
+uint7 ALT_TYPE_WGS84 = 2
+uint7 alt_type
+
+# Validity flags
+bool lat_lon_valid
+bool heading_valid
+bool velocity_valid
+bool callsign_valid
+bool ident_valid
+bool simulated_report
+bool vertical_velocity_valid
+bool baro_valid