diff --git a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp index 5ecaffc5d7..20d9e21494 100644 --- a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp +++ b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp @@ -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,61 +228,146 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg) break; case GDL90_ID_TRANSPONDER_STATUS: - 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; - } else { - _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE; - } - - if (rx.decoded.transponder_status.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.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.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.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; - } - - if (rx.decoded.transponder_status.x_bit) { - _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_XBIT_ENABLED; - } else { - _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_XBIT_ENABLED; - } - - _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; + 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; + } else { + _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE; + } - if (run_state.last_packet_Transponder_Status_ms == 0) { - // set initial control message contents to transponder defaults - _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; - _frontend.out_state.ctrl.es1090TxEnabled = rx.decoded.transponder_status.es1090TxEnabled; - _frontend.out_state.ctrl.squawkCode = rx.decoded.transponder_status.squawkCode; - _frontend.out_state.ctrl.x_bit = rx.decoded.transponder_status.x_bit; - } - run_state.last_packet_Transponder_Status_ms = AP_HAL::millis(); + if (rx.decoded.transponder_status.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.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.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.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; + } + + if (rx.decoded.transponder_status.x_bit) { + _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_XBIT_ENABLED; + } else { + _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_XBIT_ENABLED; + } + + _frontend.out_state.tx_status.squawk = rx.decoded.transponder_status.squawkCode; + + 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; + _frontend.out_state.ctrl.es1090TxEnabled = rx.decoded.transponder_status.es1090TxEnabled; + _frontend.out_state.ctrl.squawkCode = rx.decoded.transponder_status.squawkCode; + _frontend.out_state.ctrl.x_bit = rx.decoded.transponder_status.x_bit; + } + 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); + 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; diff --git a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h index 742b7924a8..a7b77a4080 100644 --- a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h +++ b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h @@ -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; }; diff --git a/libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h b/libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h index 4a386c7386..456852fb8d 100644 --- a/libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h +++ b/libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h @@ -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__))