diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 74b9cf1cd4..7452613477 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -448,6 +448,9 @@ private: bool _armed; + // state to help us not log unneccesary RCIN values: + bool seen_nonzero_rcin15_or_rcin16; + void Write_Baro_instance(uint64_t time_us, uint8_t baro_instance, enum LogMessages type); void Write_IMU_instance(uint64_t time_us, uint8_t imu_instance, diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index f355d18fa6..678d5dd4d3 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -185,7 +185,7 @@ void AP_Logger::Write_GPS(uint8_t i, uint64_t time_us) // Write an RCIN packet void AP_Logger::Write_RCIN(void) { - uint16_t values[14] = {}; + uint16_t values[16] = {}; rc().get_radio_in(values, ARRAY_SIZE(values)); const struct log_RCIN pkt{ LOG_PACKET_HEADER_INIT(LOG_RCIN_MSG), @@ -206,6 +206,23 @@ void AP_Logger::Write_RCIN(void) chan14 : values[13] }; WriteBlock(&pkt, sizeof(pkt)); + + // don't waste logging bandwidth if we haven't seen non-zero + // channels 15/16: + if (!seen_nonzero_rcin15_or_rcin16) { + if (!values[14] && !values[15]) { + return; + } + seen_nonzero_rcin15_or_rcin16 = true; + } + + const struct log_RCIN2 pkt2{ + LOG_PACKET_HEADER_INIT(LOG_RCIN2_MSG), + time_us : AP_HAL::micros64(), + chan15 : values[14], + chan16 : values[15] + }; + WriteBlock(&pkt2, sizeof(pkt2)); } // Write an SERVO packet diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 084fd2637d..f74c4c4321 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -304,6 +304,13 @@ struct PACKED log_RCIN { uint16_t chan14; }; +struct PACKED log_RCIN2 { + LOG_PACKET_HEADER; + uint64_t time_us; + uint16_t chan15; + uint16_t chan16; +}; + struct PACKED log_RCOUT { LOG_PACKET_HEADER; uint64_t time_us; @@ -2450,6 +2457,8 @@ struct PACKED log_Winch { "MSG", "QZ", "TimeUS,Message", "s-", "F-"}, \ { LOG_RCIN_MSG, sizeof(log_RCIN), \ "RCIN", "QHHHHHHHHHHHHHH", "TimeUS,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,C12,C13,C14", "sYYYYYYYYYYYYYY", "F--------------" }, \ + { LOG_RCIN2_MSG, sizeof(log_RCIN2), \ + "RCI2", "QHH", "TimeUS,C15,C16", "sYY", "F--" }, \ { 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), \ @@ -2693,6 +2702,7 @@ enum LogMessages : uint8_t { LOG_IMU_MSG, LOG_MESSAGE_MSG, LOG_RCIN_MSG, + LOG_RCIN2_MSG, LOG_RCOUT_MSG, LOG_RSSI_MSG, LOG_IMU2_MSG,