From 16bc7f800e84e34c3bc20896ebf6aa6b989f47f8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Oct 2019 19:44:16 +1000 Subject: [PATCH] AP_UAVCAN: support ADSB over UAVCAN --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 80 +++++++++++++++++++ libraries/AP_UAVCAN/AP_UAVCAN.h | 2 + .../trafficmonitor/20790.TrafficReport.uavcan | 68 ++++++++++++++++ 3 files changed, 150 insertions(+) create mode 100644 libraries/AP_UAVCAN/dsdl/ardupilot/equipment/trafficmonitor/20790.TrafficReport.uavcan diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 49ca37fffe..92d6114618 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include @@ -47,6 +48,7 @@ #include #include #include +#include #define LED_DELAY_US 50000 @@ -105,10 +107,16 @@ static uavcan::Publisher* buzzer[MAX static uavcan::Publisher* safety_state[MAX_NUMBER_OF_CAN_DRIVERS]; // subscribers + // handler SafteyButton UC_REGISTRY_BINDER(ButtonCb, ardupilot::indication::Button); static uavcan::Subscriber *safety_button_listener[MAX_NUMBER_OF_CAN_DRIVERS]; +// handler TrafficReport +UC_REGISTRY_BINDER(TrafficReportCb, ardupilot::equipment::trafficmonitor::TrafficReport); +static uavcan::Subscriber *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(*_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 diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 33103bf5e7..7c7f4bcc49 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/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_ */ diff --git a/libraries/AP_UAVCAN/dsdl/ardupilot/equipment/trafficmonitor/20790.TrafficReport.uavcan b/libraries/AP_UAVCAN/dsdl/ardupilot/equipment/trafficmonitor/20790.TrafficReport.uavcan new file mode 100644 index 0000000000..281b30928f --- /dev/null +++ b/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