mirror of https://github.com/ArduPilot/ardupilot
AP_ADSB: Bugfixes and improvements to ping200X integration
AP_ADSB: uAvionix Transponder Status V3 + Current version of ping200X sends the v1 status message periodically and the v3 status message in response to the transponder control message, so ardupilot needs to handle both gracefully; version 1 and version 3 are very different in structure and naively assuming one version over another will cause errors. AP_ADSB: Process additional xpdr status v3 fields AP_ADSB: Send GCS xpdr status at least every 10s AP_ADSB: Send ping200X estimated HPL + When AP sends the ping200X the GPS data GDL90 message, it needs to provide a valid HPL for the ping200X to report a valid NIC. AP_ADSB: Don't send unsolicited transponder status AP_ADSB: Better initialization of xpdr id/config AP_ADSB: Better initialization of frontend status AP_ADSB: Suggestions from review
This commit is contained in:
nicholas-inocencio
Tom Pittenger
parent
be5c68d74d
commit
7a6031c746
|
|
@ -59,8 +59,10 @@ bool AP_ADSB_uAvionix_UCP::init()
|
|||
return false;
|
||||
}
|
||||
|
||||
request_msg(GDL90_ID_IDENTIFICATION);
|
||||
request_msg(GDL90_ID_TRANSPONDER_CONFIG);
|
||||
_frontend.out_state.ctrl.squawkCode = 1200;
|
||||
_frontend.out_state.tx_status.squawk = 1200;
|
||||
_frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -88,8 +90,32 @@ void AP_ADSB_uAvionix_UCP::update()
|
|||
}
|
||||
} // while nbytes
|
||||
|
||||
|
||||
if (run_state.last_packet_Transponder_Id_ms == 0 && run_state.request_Transponder_Id_tries < 5)
|
||||
{
|
||||
if (now_ms - run_state.last_packet_Request_Transponder_Id_ms >= 1000)
|
||||
{
|
||||
request_msg(GDL90_ID_IDENTIFICATION);
|
||||
run_state.request_Transponder_Id_tries++;
|
||||
}
|
||||
}
|
||||
|
||||
if (run_state.last_packet_Transponder_Config_ms == 0 && run_state.request_Transponder_Config_tries < 5)
|
||||
{
|
||||
if (now_ms - run_state.last_packet_Request_Transponder_Config_ms >= 1000)
|
||||
{
|
||||
request_msg(GDL90_ID_TRANSPONDER_CONFIG);
|
||||
run_state.request_Transponder_Config_tries++;
|
||||
}
|
||||
}
|
||||
|
||||
if (now_ms - run_state.last_packet_Transponder_Control_ms >= 1000) {
|
||||
run_state.last_packet_Transponder_Control_ms = now_ms;
|
||||
|
||||
// We want to use the defaults stored on the ping200X, if possible.
|
||||
// Until we get the config message (or we've tried requesting it several times),
|
||||
// don't send the control message.
|
||||
if (run_state.last_packet_Transponder_Config_ms != 0 || run_state.request_Transponder_Config_tries >= 5)
|
||||
send_Transponder_Control();
|
||||
}
|
||||
|
||||
|
|
@ -99,12 +125,13 @@ void AP_ADSB_uAvionix_UCP::update()
|
|||
}
|
||||
|
||||
// if the transponder has stopped giving us the data needed to
|
||||
// fill the transponder status mavlink message, reset that data.
|
||||
if ((now_ms - run_state.last_packet_Transponder_Status_ms >= 10000 && run_state.last_packet_Transponder_Status_ms != 0)
|
||||
&& (now_ms - run_state.last_packet_Transponder_Heartbeat_ms >= 10000 && run_state.last_packet_Transponder_Heartbeat_ms != 0)
|
||||
&& (now_ms - run_state.last_packet_Transponder_Ownship_ms >= 10000 && run_state.last_packet_Transponder_Ownship_ms != 0))
|
||||
// fill the transponder status mavlink message, indicate status unavailable.
|
||||
if ((now_ms - run_state.last_packet_Transponder_Status_ms >= 10000)
|
||||
&& (now_ms - run_state.last_packet_Transponder_Heartbeat_ms >= 10000)
|
||||
&& (now_ms - run_state.last_packet_Transponder_Ownship_ms >= 10000))
|
||||
{
|
||||
_frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL;
|
||||
// TODO reset the data for each message when timeout occurs
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -118,9 +145,7 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg)
|
|||
// protocol.
|
||||
memcpy(&rx.decoded.heartbeat, msg.raw, sizeof(rx.decoded.heartbeat));
|
||||
run_state.last_packet_Transponder_Heartbeat_ms = AP_HAL::millis();
|
||||
|
||||
// this is always true. The "ground/air bit place" is set meaning we're always in the air
|
||||
_frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_ON_GROUND;
|
||||
_frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL;
|
||||
|
||||
if (rx.decoded.heartbeat.status.one.maintenanceRequired) {
|
||||
_frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_MAINT_REQ;
|
||||
|
|
@ -145,13 +170,11 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg)
|
|||
} else {
|
||||
_frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_TX_SYSTEM_FAIL;
|
||||
}
|
||||
|
||||
_frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL;
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
case GDL90_ID_IDENTIFICATION:
|
||||
run_state.last_packet_Transponder_Id_ms = AP_HAL::millis();
|
||||
// The Identification message contains information used to identify the connected device. The
|
||||
// Identification message will be transmitted with a period of one second regardless of data status
|
||||
// or update for the UCP protocol and will be transmitted upon request for the UCP-HD protocol.
|
||||
|
|
@ -176,11 +199,13 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg)
|
|||
break;
|
||||
|
||||
case GDL90_ID_TRANSPONDER_CONFIG:
|
||||
run_state.last_packet_Transponder_Config_ms = AP_HAL::millis();
|
||||
memcpy(&rx.decoded.transponder_config, msg.raw, sizeof(rx.decoded.transponder_config));
|
||||
break;
|
||||
|
||||
#if AP_ADSB_UAVIONIX_UCP_CAPTURE_ALL_RX_PACKETS
|
||||
case GDL90_ID_OWNSHIP_REPORT:
|
||||
_frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL;
|
||||
// The Ownship message contains information on the GNSS position. If the Ownship GNSS
|
||||
// position fix is invalid, the Latitude, Longitude, and NIC fields will all have the ZERO value. The
|
||||
// Ownship message will be transmitted with a period of one second regardless of data status or
|
||||
|
|
@ -189,7 +214,6 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg)
|
|||
run_state.last_packet_Transponder_Ownship_ms = AP_HAL::millis();
|
||||
_frontend.out_state.tx_status.NIC_NACp = rx.decoded.ownship_report.report.NIC | (rx.decoded.ownship_report.report.NACp << 4);
|
||||
memcpy(_frontend.out_state.tx_status.flight_id, rx.decoded.ownship_report.report.callsign, sizeof(_frontend.out_state.tx_status.flight_id));
|
||||
//_frontend.out_state.tx_status.temperature = rx.decoded.ownship_report.report.temperature; // there is no message in the vocabulary of the 200x that has board temperature
|
||||
break;
|
||||
|
||||
case GDL90_ID_OWNSHIP_GEOMETRIC_ALTITUDE:
|
||||
|
|
@ -204,6 +228,13 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg)
|
|||
break;
|
||||
|
||||
case GDL90_ID_TRANSPONDER_STATUS:
|
||||
{
|
||||
_frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL;
|
||||
switch (msg.payload[0])
|
||||
{
|
||||
case 1:
|
||||
{
|
||||
// version 1 of the transponder status message is sent at 1 Hz (if UCP protocol out is enabled on the transponder)
|
||||
memcpy(&rx.decoded.transponder_status, msg.raw, sizeof(rx.decoded.transponder_status));
|
||||
if (rx.decoded.transponder_status.identActive) {
|
||||
_frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE;
|
||||
|
|
@ -243,10 +274,10 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg)
|
|||
|
||||
_frontend.out_state.tx_status.squawk = rx.decoded.transponder_status.squawkCode;
|
||||
|
||||
_frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL;
|
||||
|
||||
if (run_state.last_packet_Transponder_Status_ms == 0) {
|
||||
// set initial control message contents to transponder defaults
|
||||
if (run_state.last_packet_Transponder_Status_ms == 0 && run_state.last_packet_Transponder_Config_ms == 0) {
|
||||
// If this is the first time we've seen a status message,
|
||||
// and we haven't initialized the control message from the config message,
|
||||
// set initial control message contents to match transponder's current behavior.
|
||||
_frontend.out_state.ctrl.modeAEnabled = rx.decoded.transponder_status.modeAEnabled;
|
||||
_frontend.out_state.ctrl.modeCEnabled = rx.decoded.transponder_status.modeCEnabled;
|
||||
_frontend.out_state.ctrl.modeSEnabled = rx.decoded.transponder_status.modeSEnabled;
|
||||
|
|
@ -257,8 +288,86 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg)
|
|||
run_state.last_packet_Transponder_Status_ms = AP_HAL::millis();
|
||||
#if AP_MAVLINK_MSG_UAVIONIX_ADSB_OUT_STATUS_ENABLED
|
||||
GCS_SEND_MESSAGE(MSG_UAVIONIX_ADSB_OUT_STATUS);
|
||||
run_state.last_gcs_send_message_Transponder_Status_ms = AP_HAL::millis();
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
// deprecated
|
||||
break;
|
||||
case 3:
|
||||
{
|
||||
// Version 3 of the transponder status message is sent in response to the transponder control message (if UCP-HD protocol out is enabled on the transponder)
|
||||
memcpy(&rx.decoded.transponder_status_v3, msg.raw, sizeof(rx.decoded.transponder_status_v3));
|
||||
|
||||
if (rx.decoded.transponder_status_v3.indicatingOnGround) {
|
||||
_frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_ON_GROUND;
|
||||
} else {
|
||||
_frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_ON_GROUND;
|
||||
}
|
||||
|
||||
if (rx.decoded.transponder_status_v3.fault) {
|
||||
// unsure what fault is indicated, query heartbeat for more info
|
||||
request_msg(GDL90_ID_HEARTBEAT);
|
||||
}
|
||||
|
||||
if (rx.decoded.transponder_status_v3.identActive) {
|
||||
_frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE;
|
||||
} else {
|
||||
_frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE;
|
||||
}
|
||||
|
||||
if (rx.decoded.transponder_status_v3.modeAEnabled) {
|
||||
_frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_A_ENABLED;
|
||||
} else {
|
||||
_frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_A_ENABLED;
|
||||
}
|
||||
|
||||
if (rx.decoded.transponder_status_v3.modeCEnabled) {
|
||||
_frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_C_ENABLED;
|
||||
} else {
|
||||
_frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_C_ENABLED;
|
||||
}
|
||||
|
||||
if (rx.decoded.transponder_status_v3.modeSEnabled) {
|
||||
_frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_S_ENABLED;
|
||||
} else {
|
||||
_frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_S_ENABLED;
|
||||
}
|
||||
|
||||
if (rx.decoded.transponder_status_v3.es1090TxEnabled) {
|
||||
_frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_1090ES_TX_ENABLED;
|
||||
} else {
|
||||
_frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_1090ES_TX_ENABLED;
|
||||
}
|
||||
|
||||
_frontend.out_state.tx_status.squawk = rx.decoded.transponder_status_v3.squawkCode;
|
||||
_frontend.out_state.tx_status.NIC_NACp = rx.decoded.transponder_status_v3.NIC | (rx.decoded.transponder_status_v3.NACp << 4);
|
||||
_frontend.out_state.tx_status.boardTemp = rx.decoded.transponder_status_v3.temperature;
|
||||
|
||||
// TODO not the best approach
|
||||
if (run_state.last_packet_Transponder_Status_ms == 0 && run_state.last_packet_Transponder_Config_ms == 0) {
|
||||
// If this is the first time we've seen a status message,
|
||||
// and we haven't initialized the control message from the config message,
|
||||
// set initial control message contents to match transponder's current behavior.
|
||||
_frontend.out_state.ctrl.modeAEnabled = rx.decoded.transponder_status_v3.modeAEnabled;
|
||||
_frontend.out_state.ctrl.modeCEnabled = rx.decoded.transponder_status_v3.modeCEnabled;
|
||||
_frontend.out_state.ctrl.modeSEnabled = rx.decoded.transponder_status_v3.modeSEnabled;
|
||||
_frontend.out_state.ctrl.es1090TxEnabled = rx.decoded.transponder_status_v3.es1090TxEnabled;
|
||||
_frontend.out_state.ctrl.squawkCode = rx.decoded.transponder_status_v3.squawkCode;
|
||||
}
|
||||
run_state.last_packet_Transponder_Status_ms = AP_HAL::millis();
|
||||
#if AP_MAVLINK_MSG_UAVIONIX_ADSB_OUT_STATUS_ENABLED
|
||||
GCS_SEND_MESSAGE(MSG_UAVIONIX_ADSB_OUT_STATUS);
|
||||
run_state.last_gcs_send_message_Transponder_Status_ms = AP_HAL::millis();
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
#endif // AP_ADSB_UAVIONIX_UCP_CAPTURE_ALL_RX_PACKETS
|
||||
|
||||
case GDL90_ID_TRANSPONDER_CONTROL:
|
||||
|
|
@ -339,7 +448,6 @@ void AP_ADSB_uAvionix_UCP::send_Transponder_Control()
|
|||
gdl90Transmit((GDL90_TX_MESSAGE&)msg, sizeof(msg));
|
||||
}
|
||||
|
||||
|
||||
void AP_ADSB_uAvionix_UCP::send_GPS_Data()
|
||||
{
|
||||
GDL90_GPS_DATA_V2 msg {};
|
||||
|
|
@ -358,12 +466,19 @@ void AP_ADSB_uAvionix_UCP::send_GPS_Data()
|
|||
msg.altitudeGnss_mm = fix_is_good ? (_frontend._my_loc.alt * 10): INT32_MAX;
|
||||
|
||||
// Protection Limits. FD or SBAS-based depending on state bits
|
||||
msg.HPL_mm = UINT32_MAX;
|
||||
msg.VPL_cm = UINT32_MAX;
|
||||
// Estimate HPL based on horizontal accuracy/HFOM to a probability of 10^-7:
|
||||
// Using the central limit theorem for a Gaussian distribution,
|
||||
// this is 5.326724*stdDev.
|
||||
// Conservatively round up to 6 as a scaling factor,
|
||||
// and asssume HFOM of 95% was calculated as 2*stdDev*HDOP.
|
||||
// This yields a factor of 3 to estimate HPL from horizontal accuracy.
|
||||
float accHoriz;
|
||||
bool gotAccHoriz = gps.horizontal_accuracy(accHoriz);
|
||||
msg.HPL_mm = gotAccHoriz ? 3 * accHoriz * 1E3 : UINT32_MAX; // required to calculate NIC
|
||||
msg.VPL_cm = UINT32_MAX; // unused by ping200X
|
||||
|
||||
// Figure of Merits
|
||||
float accHoriz;
|
||||
msg.horizontalFOM_mm = gps.horizontal_accuracy(accHoriz) ? accHoriz * 1E3 : UINT32_MAX;
|
||||
msg.horizontalFOM_mm = gotAccHoriz ? accHoriz * 1E3 : UINT32_MAX;
|
||||
float accVert;
|
||||
msg.verticalFOM_cm = gps.vertical_accuracy(accVert) ? accVert * 1E2 : UINT16_MAX;
|
||||
float accVel;
|
||||
|
|
|
|||
|
|
@ -64,6 +64,7 @@ private:
|
|||
GDL90_TRANSPONDER_CONFIG_MSG_V4_V5 transponder_config;
|
||||
GDL90_HEARTBEAT heartbeat;
|
||||
GDL90_TRANSPONDER_STATUS_MSG transponder_status;
|
||||
GDL90_TRANSPONDER_STATUS_MSG_V3 transponder_status_v3;
|
||||
#if AP_ADSB_UAVIONIX_UCP_CAPTURE_ALL_RX_PACKETS
|
||||
GDL90_OWNSHIP_REPORT ownship_report;
|
||||
GDL90_OWNSHIP_GEO_ALTITUDE ownship_geometric_altitude;
|
||||
|
|
@ -73,11 +74,19 @@ private:
|
|||
} rx;
|
||||
|
||||
struct {
|
||||
uint32_t last_packet_GPS_ms;
|
||||
uint32_t last_packet_Transponder_Control_ms;
|
||||
uint32_t last_packet_Transponder_Status_ms;
|
||||
uint32_t last_packet_Transponder_Heartbeat_ms;
|
||||
uint32_t last_packet_Transponder_Ownship_ms;
|
||||
uint32_t last_packet_GPS_ms; // out
|
||||
uint32_t last_packet_Transponder_Control_ms; // out
|
||||
uint32_t last_packet_Transponder_Status_ms; // in
|
||||
uint32_t last_packet_Transponder_Heartbeat_ms; // in
|
||||
uint32_t last_packet_Transponder_Ownship_ms; // in
|
||||
uint32_t last_gcs_send_message_Transponder_Status_ms; // out
|
||||
uint32_t last_packet_Request_Transponder_Config_ms; // out
|
||||
uint32_t last_packet_Transponder_Config_ms; // in
|
||||
uint32_t request_Transponder_Config_tries;
|
||||
uint32_t last_packet_Request_Transponder_Id_ms; // out
|
||||
uint32_t last_packet_Transponder_Id_ms; // in
|
||||
uint32_t request_Transponder_Id_tries;
|
||||
|
||||
} run_state;
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -103,10 +103,8 @@ typedef struct __attribute__((__packed__))
|
|||
} GDL90_TRANSPONDER_CONTROL_MSG;
|
||||
#endif
|
||||
|
||||
#define GDL90_TRANSPONDER_STATUS_VERSION (1) // Version 1 is the correct UCP format; version 3 is half-duplex and not used by the ping200x
|
||||
#define GDL90_STATUS_MAX_ALTITUDE_FT (101338)
|
||||
#define GDL90_STATUS_MIN_ALTITUDE_FT (-1000)
|
||||
#if GDL90_TRANSPONDER_STATUS_VERSION == 1
|
||||
typedef struct __attribute__((__packed__))
|
||||
{
|
||||
GDL90_MESSAGE_ID messageId;
|
||||
|
|
@ -122,9 +120,9 @@ typedef struct __attribute__((__packed__))
|
|||
uint16_t modecRepliesPerSecond;
|
||||
uint16_t modeSRepliesPerSecond;
|
||||
uint16_t squawkCode;
|
||||
uint16_t crc;
|
||||
} GDL90_TRANSPONDER_STATUS_MSG;
|
||||
#endif
|
||||
#if GDL90_TRANSPONDER_STATUS_VERSION == 3
|
||||
|
||||
typedef struct __attribute__((__packed__))
|
||||
{
|
||||
GDL90_MESSAGE_ID messageId;
|
||||
|
|
@ -147,8 +145,7 @@ typedef struct __attribute__((__packed__))
|
|||
uint8_t NACp : 4;
|
||||
uint8_t temperature;
|
||||
uint16_t crc;
|
||||
} GDL90_TRANSPONDER_STATUS_MSG;
|
||||
#endif
|
||||
} GDL90_TRANSPONDER_STATUS_MSG_V3;
|
||||
|
||||
|
||||
typedef struct __attribute__((__packed__))
|
||||
|
|
|
|||
Loading…
Reference in New Issue