diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 3607d6599f..6831f1750d 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -210,7 +210,8 @@ void AP_Logger::Write_RSSI() const struct log_RSSI pkt{ LOG_PACKET_HEADER_INIT(LOG_RSSI_MSG), time_us : AP_HAL::micros64(), - RXRSSI : rssi->read_receiver_rssi() + RXRSSI : rssi->read_receiver_rssi(), + RXLQ : rssi->read_receiver_link_quality() }; WriteBlock(&pkt, sizeof(pkt)); } diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 0445134f37..73f7defa1a 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -286,6 +286,7 @@ struct PACKED log_RSSI { LOG_PACKET_HEADER; uint64_t time_us; float RXRSSI; + float RXLQ; }; struct PACKED log_Optflow { @@ -1105,6 +1106,7 @@ struct PACKED log_PSCZ { // @Description: Received Signal Strength Indicator for RC receiver // @Field: TimeUS: Time since system startup // @Field: RXRSSI: RSSI +// @Field: RXLQ: RX Link Quality // @LoggerMessage: SIM // @Description: SITL simulator state @@ -1229,7 +1231,7 @@ LOG_STRUCTURE_FROM_GPS \ { LOG_RCOUT_MSG, sizeof(log_RCOUT), \ "RCOU", "QHHHHHHHHHHHHHH", "TimeUS,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,C12,C13,C14", "sYYYYYYYYYYYYYY", "F--------------" }, \ { LOG_RSSI_MSG, sizeof(log_RSSI), \ - "RSSI", "Qf", "TimeUS,RXRSSI", "s-", "F-" }, \ + "RSSI", "Qff", "TimeUS,RXRSSI,RXLQ", "s--", "F--" }, \ LOG_STRUCTURE_FROM_BARO \ LOG_STRUCTURE_FROM_PRECLAND \ { LOG_POWR_MSG, sizeof(log_POWR), \