aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Periph: move ADSB CAN handling into adsb.cpp

Browse Source
This commit is contained in:
Peter Barker 2023-07-04 10:06:40 +10:00 committed by Peter Barker
parent 40fc35c65f
commit 0f1de63d7c
2 changed files with 58 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
Tools/AP_Periph/adsb.cpp
View File

@ -93,4 +93,62 @@ void AP_Periph_FW::adsb_update(void)
} }
} }
/*
map an ADSB_VEHICLE MAVLink message to a UAVCAN TrafficReport message
*/
void AP_Periph_FW::can_send_ADSB(struct __mavlink_adsb_vehicle_t &msg)
{
ardupilot_equipment_trafficmonitor_TrafficReport pkt {};
pkt.timestamp.usec = 0;
pkt.icao_address = msg.ICAO_address;
pkt.tslc = msg.tslc;
pkt.latitude_deg_1e7 = msg.lat;
pkt.longitude_deg_1e7 = msg.lon;
pkt.alt_m = msg.altitude * 1e-3;
pkt.heading = radians(msg.heading * 1e-2);
pkt.velocity[0] = cosf(pkt.heading) * msg.hor_velocity * 1e-2;
pkt.velocity[1] = sinf(pkt.heading) * msg.hor_velocity * 1e-2;
pkt.velocity[2] = -msg.ver_velocity * 1e-2;
pkt.squawk = msg.squawk;
memcpy(pkt.callsign, msg.callsign, MIN(sizeof(msg.callsign),sizeof(pkt.callsign)));
if (msg.flags & 0x8000) {
pkt.source = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SOURCE_ADSB_UAT;
} else {
pkt.source = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SOURCE_ADSB;
}
pkt.traffic_type = msg.emitter_type;
if ((msg.flags & ADSB_FLAGS_VALID_ALTITUDE) != 0 && msg.altitude_type == 0) {
pkt.alt_type = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_PRESSURE_AMSL;
} else if ((msg.flags & ADSB_FLAGS_VALID_ALTITUDE) != 0 && msg.altitude_type == 1) {
pkt.alt_type = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_WGS84;
} else {
pkt.alt_type = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_ALT_UNKNOWN;
}
pkt.lat_lon_valid = (msg.flags & ADSB_FLAGS_VALID_COORDS) != 0;
pkt.heading_valid = (msg.flags & ADSB_FLAGS_VALID_HEADING) != 0;
pkt.velocity_valid = (msg.flags & ADSB_FLAGS_VALID_VELOCITY) != 0;
pkt.callsign_valid = (msg.flags & ADSB_FLAGS_VALID_CALLSIGN) != 0;
pkt.ident_valid = (msg.flags & ADSB_FLAGS_VALID_SQUAWK) != 0;
pkt.simulated_report = (msg.flags & ADSB_FLAGS_SIMULATED) != 0;
// these flags are not in common.xml
pkt.vertical_velocity_valid = (msg.flags & 0x0080) != 0;
pkt.baro_valid = (msg.flags & 0x0100) != 0;
uint8_t buffer[ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_MAX_SIZE] {};
uint16_t total_size = ardupilot_equipment_trafficmonitor_TrafficReport_encode(&pkt, buffer, !periph.canfdout());
canard_broadcast(ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SIGNATURE,
ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ID,
CANARD_TRANSFER_PRIORITY_LOWEST,
&buffer[0],
total_size);
}
#endif // HAL_PERIPH_ENABLE_ADSB #endif // HAL_PERIPH_ENABLE_ADSB

60
Tools/AP_Periph/can.cpp
View File

@ -2460,66 +2460,6 @@ void AP_Periph_FW::can_proximity_update()
#endif #endif
} }
#ifdef HAL_PERIPH_ENABLE_ADSB
/*
map an ADSB_VEHICLE MAVLink message to a UAVCAN TrafficReport message
*/
void AP_Periph_FW::can_send_ADSB(struct __mavlink_adsb_vehicle_t &msg)
{
ardupilot_equipment_trafficmonitor_TrafficReport pkt {};
pkt.timestamp.usec = 0;
pkt.icao_address = msg.ICAO_address;
pkt.tslc = msg.tslc;
pkt.latitude_deg_1e7 = msg.lat;
pkt.longitude_deg_1e7 = msg.lon;
pkt.alt_m = msg.altitude * 1e-3;
pkt.heading = radians(msg.heading * 1e-2);
pkt.velocity[0] = cosf(pkt.heading) * msg.hor_velocity * 1e-2;
pkt.velocity[1] = sinf(pkt.heading) * msg.hor_velocity * 1e-2;
pkt.velocity[2] = -msg.ver_velocity * 1e-2;
pkt.squawk = msg.squawk;
memcpy(pkt.callsign, msg.callsign, MIN(sizeof(msg.callsign),sizeof(pkt.callsign)));
if (msg.flags & 0x8000) {
pkt.source = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SOURCE_ADSB_UAT;
} else {
pkt.source = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SOURCE_ADSB;
}
pkt.traffic_type = msg.emitter_type;
if ((msg.flags & ADSB_FLAGS_VALID_ALTITUDE) != 0 && msg.altitude_type == 0) {
pkt.alt_type = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_PRESSURE_AMSL;
} else if ((msg.flags & ADSB_FLAGS_VALID_ALTITUDE) != 0 && msg.altitude_type == 1) {
pkt.alt_type = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_WGS84;
} else {
pkt.alt_type = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_ALT_UNKNOWN;
}
pkt.lat_lon_valid = (msg.flags & ADSB_FLAGS_VALID_COORDS) != 0;
pkt.heading_valid = (msg.flags & ADSB_FLAGS_VALID_HEADING) != 0;
pkt.velocity_valid = (msg.flags & ADSB_FLAGS_VALID_VELOCITY) != 0;
pkt.callsign_valid = (msg.flags & ADSB_FLAGS_VALID_CALLSIGN) != 0;
pkt.ident_valid = (msg.flags & ADSB_FLAGS_VALID_SQUAWK) != 0;
pkt.simulated_report = (msg.flags & ADSB_FLAGS_SIMULATED) != 0;
// these flags are not in common.xml
pkt.vertical_velocity_valid = (msg.flags & 0x0080) != 0;
pkt.baro_valid = (msg.flags & 0x0100) != 0;
uint8_t buffer[ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_MAX_SIZE] {};
uint16_t total_size = ardupilot_equipment_trafficmonitor_TrafficReport_encode(&pkt, buffer, !periph.canfdout());
canard_broadcast(ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SIGNATURE,
ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ID,
CANARD_TRANSFER_PRIORITY_LOWEST,
&buffer[0],
total_size);
}
#endif // HAL_PERIPH_ENABLE_ADSB
#ifdef HAL_PERIPH_ENABLE_EFI #ifdef HAL_PERIPH_ENABLE_EFI
/* /*