mirror of https://github.com/ArduPilot/ardupilot
AP_ADSB: remove mavlink specific source, xml not ready yet
This commit is contained in:
Tom Pittenger
parent
d136737c87
commit
a5f256229f
|
|
@ -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
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue