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

AP_ADSB: add support for ADSB transceiver mavlink packets

Browse Source
This commit is contained in:
Tom Pittenger 2016-07-10 16:53:19 -07:00
parent 2223216ca2
commit b6e25e7d64
2 changed files with 347 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

300
libraries/AP_ADSB/AP_ADSB.cpp
View File

@ -24,6 +24,8 @@
#include <AP_HAL/AP_HAL.h>
#include "AP_ADSB.h"
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <stdio.h> // for sprintf
#include <limits.h>
extern const AP_HAL::HAL& hal;
@ -58,6 +60,40 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = {
// @User: Advanced
AP_GROUPINFO("LIST_RADIUS", 3, AP_ADSB, in_state.list_radius, ADSB_LIST_RADIUS_DEFAULT),
// @Param: ICAO_ID
// @DisplayName: ICAO_ID vehicle identifaction number
// @Description: ICAO_ID unique vehicle identifaction number of this aircraft. This is a integer limited to 24bits. If set to 0 then one will be randomly generated. If set to -1 then static information is not sent, transceiver is assumed pre-programmed.
// @Range: -1 16777215
// @User: Advanced
AP_GROUPINFO("ICAO_ID", 4, AP_ADSB, out_state.cfg.ICAO_id_param, 0),
// @Param: EMIT_TYPE
// @DisplayName: Emitter type
// @Description: ADSB classification for the type of vehicle emitting the transponder signal. Default value is 14 (UAV).
// @Values: 0:NoInfo,1:Light,2:Small,3:Large,4:HighVortexlarge,5:Heavy,6:HighlyManuv,7:Rotocraft,8:RESERVED,9:Glider,10:LightAir,11:Parachute,12:UltraLight,13:RESERVED,14:UAV,15:Space,16:RESERVED,17:EmergencySurface,18:ServiceSurface,19:PointObstacle
// @User: Advanced
AP_GROUPINFO("EMIT_TYPE", 5, AP_ADSB, out_state.cfg.emitterType, ADSB_EMITTER_TYPE_UAV),
// @Param: LEN_WIDTH
// @DisplayName: Aircraft length and width
// @Description: Aircraft length and width encoding. See ADSB_TRANSPONDER_STATIC_INPUT_ALW_ENCODE
// @User: Advanced
AP_GROUPINFO("LEN_WIDTH", 6, AP_ADSB, out_state.cfg.lengthWidth, ADSB_TRANSPONDER_AIRCRAFT_SIZE_NO_DATA),
// @Param: OFFSET_LAT
// @DisplayName: GPS antenna lateral offset
// @Description: GPS antenna lateral offset. See ADSB_TRANSPONDER_STATIC_INPUT_GPS_LAT_OFFSET
// @User: Advanced
AP_GROUPINFO("OFFSET_LAT", 7, AP_ADSB, out_state.cfg.gpsLatOffset, ADSB_TRANSPONDER_GPS_LAT_OFFSET_NO_DATA),
// @Param: OFFSET_LON
// @DisplayName: GPS antenna longitudinal offset
// @Description: GPS antenna longitudinal offset. See ADSB_TRANSPONDER_STATIC_INPUT_GPS_LON_OFFSET
// @User: Advanced
AP_GROUPINFO("OFFSET_LON", 8, AP_ADSB, out_state.cfg.gpsLonOffset, ADSB_TRANSPONDER_GPS_LON_OFFSET_NO_DATA),
AP_GROUPEND
};
@ -88,6 +124,9 @@ void AP_ADSB::init(void)
avoid_state.highest_threat_distance = 0;
avoid_state.another_vehicle_within_radius = false;
avoid_state.is_evading_threat = false;
// out_state
set_callsign("PING1234", false);
}
/*
@ -141,7 +180,73 @@ void AP_ADSB::update(void)
}
}
// -----------------------
if (_my_loc.is_zero()) {
// if we don't have a GPS lock then there's nothing else to do
return;
}
// -----------------------
perform_threat_detection();
// ensure it's positive 24bit but allow -1
out_state.cfg.ICAO_id_param = constrain_int32(out_state.cfg.ICAO_id_param, -1, 0x00FFFFFF);
// send dynamic data to transceiver, every 1s
if (now - out_state.last_report_ms >= 1000) {
out_state.last_report_ms = now;
// if the transceiver has been detected on a channel, use that channel. Else, broadcast to all
if (out_state.chan >= 0 && out_state.chan < MAVLINK_COMM_NUM_BUFFERS) {
send_dynamic_out((mavlink_channel_t)(MAVLINK_COMM_0 + out_state.chan));
} else {
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
send_dynamic_out((mavlink_channel_t)(MAVLINK_COMM_0 + i));
}
}
} // last_report_ms
// keep checking for ICAO_id because we may change it during run-time
if (out_state.cfg.ICAO_id_param <= -1) {
// icao param of -1 means static information is not sent, transceiver is assumed pre-programmed.
// reset timer constantly so it never reaches 10s so it never sends
out_state.last_config_ms = now;
} else if (out_state.cfg.ICAO_id == 0 ||
out_state.cfg.ICAO_id_param_prev != out_state.cfg.ICAO_id_param) {
// if param changed then regenerate. This allows the param to be changed back to zero to trigger a re-generate
out_state.cfg.ICAO_id_param_prev = out_state.cfg.ICAO_id_param;
out_state.cfg.ICAO_id = genICAO(_my_loc);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "ADSB: Generated ICAO_id: %d", out_state.cfg.ICAO_id);
set_callsign("PING", true);
out_state.last_config_ms = 0; // send now
} else {
// if param is set (non zero), then always use it
out_state.cfg.ICAO_id = out_state.cfg.ICAO_id_param;
out_state.cfg.ICAO_id_param_prev = out_state.cfg.ICAO_id_param;
set_callsign("PING", true);
out_state.last_config_ms = 0; // send now
}
// send static configuration data to transceiver, every 10s
if (now - out_state.last_config_ms >= 10000) {
out_state.last_config_ms = now;
// if the transceiver has been detected on a channel, use that channel. Else, broadcast to all
if (out_state.chan >= 0 && out_state.chan < MAVLINK_COMM_NUM_BUFFERS) {
send_configure((mavlink_channel_t)(MAVLINK_COMM_0 + out_state.chan));
} else {
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
send_configure((mavlink_channel_t)(MAVLINK_COMM_0 + i));
}
}
} // last_config_ms
}
/*
@ -367,3 +472,198 @@ void AP_ADSB::send_adsb_vehicle(mavlink_channel_t chan)
}
}
void AP_ADSB::send_dynamic_out(mavlink_channel_t chan)
{
// --------------
// Knowns
AP_GPS gps = _ahrs.get_gps();
Vector3f gps_velocity = gps.velocity();
int32_t latitude = _my_loc.lat;
int32_t longitude = _my_loc.lng;
int32_t altGNSS = _my_loc.alt*0.1f; // convert cm to mm
int16_t velVert = gps_velocity.z * 1E2; // convert m/s to cm/s
int16_t nsVog = gps_velocity.x * 1E2; // convert m/s to cm/s
int16_t ewVog = gps_velocity.y * 1E2; // convert m/s to cm/s
uint8_t fixType = gps.status(); // this lines up perfectly with our enum
uint8_t emStatus = 0; // TODO: implement this ENUM. no emergency = 0
uint8_t numSats = gps.num_sats();
uint16_t squawk = 1200; // Mode A code (typically 1200 [0x04B0] for VFR)
uint32_t accHoriz = UINT_MAX;
float accHoriz_f;
if (gps.horizontal_accuracy(accHoriz_f)) {
accHoriz = accHoriz_f * 1E3; // convert m to mm
}
uint16_t accVert = USHRT_MAX;
float accVert_f;
if (gps.vertical_accuracy(accVert_f)) {
accVert = accVert_f * 1E2; // convert m to cm
}
uint16_t accVel = USHRT_MAX;
float accVel_f;
if (gps.speed_accuracy(accVel_f)) {
accVel = accVel_f * 1E3; // convert m/s to mm/s
}
uint16_t state = 0;
if (_is_in_auto_mode) {
state |= ADSB_TRANSPONDER_AUTOPILOT_ENABLED;
}
if (!out_state.is_flying) {
state |= ADSB_TRANSPONDER_ON_GROUND;
}
// --------------
// Not Sure
uint32_t utcTime = UINT_MAX; // uint32_t utcTime,
// TODO: confirm this sets utcTime correctly
const uint64_t gps_time = gps.time_epoch_usec();
utcTime = gps_time / 1000000ULL;
// just set them all, I don't think these are implemented on the transceiver (yet)
uint8_t control = ADSB_TRANSPONDER_RECEIVE_ONLY | ADSB_TRANSPONDER_TX_ENABLE_1090ES | ADSB_TRANSPONDER_TX_ENABLE_UAT;
// --------------
// Unknowns
int32_t altPres = INT_MAX; //_ahrs.get_baro().get_altitude() relative to home, not MSL
mavlink_msg_adsb_transponder_dynamic_input_send(
chan,
utcTime,
latitude,
longitude,
altPres,
altGNSS,
accHoriz,
accVert,
accVel,
velVert,
nsVog,
ewVog,
fixType,
numSats,
emStatus,
control,
state,
squawk);
}
void AP_ADSB::send_configure(mavlink_channel_t chan)
{
mavlink_msg_adsb_transponder_static_input_send(
chan,
(uint32_t)out_state.cfg.ICAO_id,
out_state.cfg.callsign,
(uint8_t)out_state.cfg.emitterType,
(uint8_t)out_state.cfg.lengthWidth,
(uint8_t)out_state.cfg.gpsLatOffset,
(uint8_t)out_state.cfg.gpsLonOffset,
out_state.cfg.stall_speed_cm);
}
/*
* this is a message from the transceiver reporting it's health. Using this packet
* we determine which channel is on so we don't have to send out_state to all channels
*/
void AP_ADSB::transceiver_report(mavlink_channel_t chan, const mavlink_message_t* msg)
{
mavlink_adsb_transponder_dynamic_output_t packet {};
mavlink_msg_adsb_transponder_dynamic_output_decode(msg, &packet);
if (out_state.chan != chan) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_DEBUG, "ADSB: Found transceiver on channel %d", chan);
}
out_state.chan = chan;
if (out_state.status != (ADSB_TRANSPONDER_DYNAMIC_OUTPUT_STATUS_FLAGS)packet.status) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_DEBUG, "ADSB: Transceiver status %d", packet.status);
}
out_state.status = (ADSB_TRANSPONDER_DYNAMIC_OUTPUT_STATUS_FLAGS)packet.status;
}
/*
@brief Generates pseudorandom ICAO from gps time, lat, and lon.
Reference: DO282B, 2.2.4.5.1.3.2
Note gps.time is the number of seconds since UTC midnight
*/
uint32_t AP_ADSB::genICAO(const Location &loc)
{
// gps_time is not seconds since UTC midnight, but it is an equivalent random number
// TODO: use UTC time instead of GPS time
AP_GPS gps = _ahrs.get_gps();
const uint64_t gps_time = gps.time_epoch_usec();
uint32_t timeSum = 0;
uint32_t M3 = 4096 * (loc.lat & 0x00000FFF) + (loc.lng & 0x00000FFF);
for (uint8_t i=0; i<24; i++) {
timeSum += (((gps_time & 0x00FFFFFF)>> i) & 0x00000001);
}
return( (timeSum ^ M3) & 0x00FFFFFF);
}
// assign a string to out_state.cfg.callsign but ensure it's null terminated
void AP_ADSB::set_callsign(const char* str, bool append_icao)
{
bool zero_char_pad = false;
// clean slate
memset(out_state.cfg.callsign, 0, sizeof(out_state.cfg.callsign));
// copy str to cfg.callsign but we can't use strncpy because we need
// to restrict values to only 'A' - 'Z' and '0' - '9'
for (uint8_t i=0; i<sizeof(out_state.cfg.callsign)-1; i++) {
if (!str[i] || zero_char_pad) {
// finish early. Either pad the rest with zero char or null terminate and call it a day
if ((append_icao && i<4) || zero_char_pad) {
out_state.cfg.callsign[i] = '0';
zero_char_pad = true;
} else {
// already null terminated via memset so just stop
break;
}
} else if (('A' <= str[i] && str[i] <= 'Z') ||
('0' <= str[i] && str[i] <= '9')) {
// valid as-is
// spaces are also allowed but are handled in the last else
out_state.cfg.callsign[i] = str[i];
} else if ('a' <= str[i] && str[i] <= 'z') {
// toupper()
out_state.cfg.callsign[i] = str[i] - ('a' - 'A');
} else if (i == 0) {
// invalid, pad to char zero because first index can't be space
out_state.cfg.callsign[i] = '0';
} else {
// invalid, pad with space
out_state.cfg.callsign[i] = ' ';
}
} // for i
if (append_icao) {
char str_icao[5];
sprintf(str_icao, "%4d", out_state.cfg.ICAO_id % 10000);
out_state.cfg.callsign[4] = str_icao[0];
out_state.cfg.callsign[5] = str_icao[1];
out_state.cfg.callsign[6] = str_icao[2];
out_state.cfg.callsign[7] = str_icao[3];
}
out_state.cfg.callsign[8] = 0; // always null terminate just to be sure
}

51
libraries/AP_ADSB/AP_ADSB.h
View File

@ -78,6 +78,8 @@ public:
// add or update vehicle_list from inbound mavlink msg
void update_vehicle(const mavlink_message_t* msg);
// handle ADS-B transceiver report
void transceiver_report(mavlink_channel_t chan, const mavlink_message_t* msg);
bool get_possible_threat() { return _enabled && avoid_state.another_vehicle_within_radius; }
@ -89,6 +91,11 @@ public:
// send ADSB_VEHICLE mavlink message, usually as a StreamRate
void send_adsb_vehicle(mavlink_channel_t chan);
void set_stall_speed_cm(uint16_t stall_speed_cm) { out_state.cfg.stall_speed_cm = stall_speed_cm; }
void set_is_auto_mode(bool is_in_auto_mode) { _is_in_auto_mode = is_in_auto_mode; }
void set_is_flying(bool is_flying) { out_state.is_flying = is_flying; }
private:
// initialize _vehicle_list
@ -111,14 +118,27 @@ private:
void set_vehicle(uint16_t index, const adsb_vehicle_t &vehicle);
// Generates pseudorandom ICAO from gps time, lat, and lon
uint32_t genICAO(const Location &loc);
// set callsign: 8char string (plus null termination) then optionally append last 4 digits of icao
void set_callsign(const char* str, bool append_icao);
// send static and dynamic data to ADSB transceiver
void send_configure(mavlink_channel_t chan);
void send_dynamic_out(mavlink_channel_t chan);
// reference to AHRS, so we can ask for our position,
// heading and speed
const AP_AHRS &_ahrs;
AP_AHRS &_ahrs;
AP_Int8 _enabled;
Location_Class _my_loc;
bool _is_in_auto_mode;
// ADSB-IN state. Maintains list of external vehicles
struct {
// list management
@ -128,12 +148,35 @@ private:
uint16_t vehicle_count = 0;
AP_Int32 list_radius;
// streamrate stuff
uint32_t send_start_ms[MAVLINK_COMM_NUM_BUFFERS];
uint16_t send_index[MAVLINK_COMM_NUM_BUFFERS];
// streamrate stuff
uint32_t send_start_ms[MAVLINK_COMM_NUM_BUFFERS];
uint16_t send_index[MAVLINK_COMM_NUM_BUFFERS];
} in_state;
// ADSB-OUT state. Maintains export data
struct {
uint32_t last_config_ms; // send once every 10s
uint32_t last_report_ms; // send at 5Hz
int8_t chan = -1; // channel that contains an ADS-b Transceiver. -1 means broadcast to all
ADSB_TRANSPONDER_DYNAMIC_OUTPUT_STATUS_FLAGS status; // transceiver status
bool is_flying;
// ADSB-OUT configuration
struct {
int32_t ICAO_id;
AP_Int32 ICAO_id_param;
int32_t ICAO_id_param_prev = -1; // assume we never send
char callsign[9]; //Vehicle identifier (8 characters, null terminated, valid characters are A-Z, 0-9, " " only).
AP_Int8 emitterType;
AP_Int8 lengthWidth; // Aircraft length and width encoding (table 2-35 of DO-282B)
AP_Int8 gpsLatOffset;
AP_Int8 gpsLonOffset;
uint16_t stall_speed_cm;
} cfg;
} out_state;
// Avoidance state
struct {