Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_ADSB: remove mavlink specific source, xml not ready yet

Browse Source
This commit is contained in:
Tom Pittenger 2016-07-20 23:19:11 -07:00
parent d136737c87
commit a5f256229f
2 changed files with 30 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

145
libraries/AP_ADSB/AP_ADSB.cpp
View File

@ -26,6 +26,19 @@
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <stdio.h> // for sprintf
#include <limits.h>
#include <AP_Vehicle/AP_Vehicle.h>
#define VEHICLE_THREAT_RADIUS_M 1000
#define VEHICLE_TIMEOUT_MS 5000 // if no updates in this time, drop it from the list
#define ADSB_VEHICLE_LIST_SIZE_DEFAULT 25
#define ADSB_VEHICLE_LIST_SIZE_MAX 100
#define ADSB_CHAN_TIMEOUT_MS 15000
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
#define ADSB_LIST_RADIUS_DEFAULT 10000 // in meters
#else // APM_BUILD_TYPE(APM_BUILD_ArduCopter), Rover, Boat
#define ADSB_LIST_RADIUS_DEFAULT 2000 // in meters
#endif
extern const AP_HAL::HAL& hal;
@ -72,25 +85,25 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = {
// @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),
AP_GROUPINFO("EMIT_TYPE", 5, AP_ADSB, out_state.cfg.emitterType, 14),
// @Param: LEN_WIDTH
// @DisplayName: Aircraft length and width
// @Description: Aircraft length and width encoding. See ADSB_TRANSPONDER_STATIC_INPUT_ALW_ENCODE
// @Description: Aircraft length and width encoding.
// @User: Advanced
AP_GROUPINFO("LEN_WIDTH", 6, AP_ADSB, out_state.cfg.lengthWidth, ADSB_TRANSPONDER_AIRCRAFT_SIZE_NO_DATA),
AP_GROUPINFO("LEN_WIDTH", 6, AP_ADSB, out_state.cfg.lengthWidth, 0),
// @Param: OFFSET_LAT
// @DisplayName: GPS antenna lateral offset
// @Description: GPS antenna lateral offset. See ADSB_TRANSPONDER_STATIC_INPUT_GPS_LAT_OFFSET
// @Description: GPS antenna lateral offset.
// @User: Advanced
AP_GROUPINFO("OFFSET_LAT", 7, AP_ADSB, out_state.cfg.gpsLatOffset, ADSB_TRANSPONDER_GPS_LAT_OFFSET_NO_DATA),
AP_GROUPINFO("OFFSET_LAT", 7, AP_ADSB, out_state.cfg.gpsLatOffset, 0),
// @Param: OFFSET_LON
// @DisplayName: GPS antenna longitudinal offset
// @Description: GPS antenna longitudinal offset. See ADSB_TRANSPONDER_STATIC_INPUT_GPS_LON_OFFSET
// @Description: GPS antenna longitudinal offset.
// @User: Advanced
AP_GROUPINFO("OFFSET_LON", 8, AP_ADSB, out_state.cfg.gpsLonOffset, ADSB_TRANSPONDER_GPS_LON_OFFSET_NO_DATA),
AP_GROUPINFO("OFFSET_LON", 8, AP_ADSB, out_state.cfg.gpsLonOffset, 0),
@ -220,13 +233,15 @@ void AP_ADSB::update(void)
out_state.chan = -1;
} else {
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0 + out_state.chan);
if (now - out_state.last_config_ms >= 5000 && HAVE_PAYLOAD_SPACE(chan, ADSB_TRANSPONDER_STATIC_INPUT)) {
// TODO: add check HAVE_PAYLOAD_SPACE(config)
if (now - out_state.last_config_ms >= 5000) {
out_state.last_config_ms = now;
send_configure(chan);
} // last_config_ms
// send dynamic data to transceiver at 5Hz
if (now - out_state.last_report_ms >= 200 && HAVE_PAYLOAD_SPACE(chan, ADSB_TRANSPONDER_DYNAMIC_INPUT)) {
// TODO: add check HAVE_PAYLOAD_SPACE(dynamic)
if (now - out_state.last_report_ms >= 200) {
out_state.last_report_ms = now;
send_dynamic_out(chan);
} // last_report_ms
@ -464,101 +479,12 @@ void AP_ADSB::send_adsb_vehicle(const mavlink_channel_t chan)
void AP_ADSB::send_dynamic_out(const 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
// TODO: implement http://www.srh.noaa.gov/images/epz/wxcalc/pressureAltitude.pdf
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);
// TODO: send dynamic packet
}
void AP_ADSB::send_configure(const 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);
// TODO configure packet
}
/*
@ -568,20 +494,7 @@ void AP_ADSB::send_configure(const mavlink_channel_t chan)
void AP_ADSB::transceiver_report(const 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_last_ms = AP_HAL::millis();
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;
// TODO: parse transceiver report
}
/*
@ -615,7 +528,7 @@ void AP_ADSB::set_callsign(const char* str, const bool append_icao)
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'
// to restrict values to only 'A' - 'Z' and '0' - '9' and pad
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
@ -656,6 +569,6 @@ void AP_ADSB::set_callsign(const char* str, const bool append_icao)
out_state.cfg.callsign[7] = str_icao[3];
}
out_state.cfg.callsign[8] = 0; // always null terminate just to be sure
out_state.cfg.callsign[sizeof(out_state.cfg.callsign)-1] = 0; // always null terminate just to be sure
}

15
libraries/AP_ADSB/AP_ADSB.h
View File

@ -25,21 +25,8 @@
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_Common/Location.h>
#define VEHICLE_THREAT_RADIUS_M 1000
#define VEHICLE_TIMEOUT_MS 10000 // if no updates in this time, drop it from the list
#define ADSB_VEHICLE_LIST_SIZE_DEFAULT 25
#define ADSB_VEHICLE_LIST_SIZE_MAX 100
#define ADSB_CHAN_TIMEOUT_MS 15000
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
#define ADSB_LIST_RADIUS_DEFAULT 10000 // in meters
#else // APM_BUILD_TYPE(APM_BUILD_ArduCopter), Rover, Boat
#define ADSB_LIST_RADIUS_DEFAULT 2000 // in meters
#endif
class AP_ADSB
{
public:
@ -160,7 +147,7 @@ private:
uint32_t last_report_ms; // send at 5Hz
int8_t chan = -1; // channel that contains an ADS-b Transceiver. -1 means broadcast to all
uint32_t chan_last_ms;
ADSB_TRANSPONDER_DYNAMIC_OUTPUT_STATUS_FLAGS status; // transceiver status
// TODO: add enum "status"
bool is_flying;
// ADSB-OUT configuration