diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp index 86a8a7315a..82ffd75335 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp @@ -19,6 +19,7 @@ #include #include #include +#include AP_RCProtocol_Backend::AP_RCProtocol_Backend(AP_RCProtocol &_frontend) : frontend(_frontend), @@ -73,3 +74,26 @@ void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool } rssi = _rssi; } + +/* + optionally log RC input data + */ +void AP_RCProtocol_Backend::log_data(AP_RCProtocol::rcprotocol_t prot, uint32_t timestamp, const uint8_t *data, uint8_t len) const +{ +#if !APM_BUILD_TYPE(APM_BUILD_iofirmware) + if (rc().log_raw_data()) { + uint32_t u32[10] {}; + if (len > sizeof(u32)) { + len = sizeof(u32); + } + memcpy(u32, data, len); + AP::logger().Write("RCDA", "TimeUS,TS,Prot,Len,U0,U1,U2,U3,U4,U5,U6,U7,U8,U9", "QIBBIIIIIIIIII", + AP_HAL::micros64(), + timestamp, + (uint8_t)prot, + len, + u32[0], u32[1], u32[2], u32[3], u32[4], + u32[5], u32[6], u32[7], u32[8], u32[9]); + } +#endif +} diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index f32137755e..34163e0871 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -71,6 +71,8 @@ public: protected: void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe, int16_t rssi=-1); + void log_data(AP_RCProtocol::rcprotocol_t prot, uint32_t timestamp, const uint8_t *data, uint8_t len) const; + private: AP_RCProtocol &frontend; uint32_t rc_input_count; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp index a0c55f5728..b2df3476e2 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp @@ -425,6 +425,8 @@ bool AP_RCProtocol_DSM::dsm_parse_byte(uint32_t frame_time_ms, uint8_t b, uint16 break; } + log_data(AP_RCProtocol::DSM, frame_time_ms*1000U, byte_input.buf, byte_input.ofs); + /* * Great, it looks like we might have a frame. Go ahead and * decode it. diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp index 3232c55dde..760c3d8b1c 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp @@ -317,11 +317,13 @@ void AP_RCProtocol_FPort::_process_byte(uint32_t timestamp_us, uint8_t b) } if (frame->type == FPORT_TYPE_CONTROL && byte_input.ofs == FRAME_LEN_CONTROL + 4) { + log_data(AP_RCProtocol::FPORT, timestamp_us, byte_input.buf, byte_input.ofs); if (check_checksum()) { decode_control(*frame); } goto reset; } else if (frame->type == FPORT_TYPE_DOWNLINK && byte_input.ofs == FRAME_LEN_DOWNLINK + 4) { + log_data(AP_RCProtocol::FPORT, timestamp_us, byte_input.buf, byte_input.ofs); if (check_checksum()) { decode_downlink(*frame); } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.cpp index 4138c7615d..55ac7c0be3 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.cpp @@ -89,6 +89,7 @@ void AP_RCProtocol_IBUS::_process_byte(uint32_t timestamp_us, uint8_t b) if (byte_input.ofs == sizeof(byte_input.buf)) { uint16_t values[IBUS_INPUT_CHANNELS]; bool ibus_failsafe = false; + log_data(AP_RCProtocol::IBUS, timestamp_us, byte_input.buf, byte_input.ofs); if (ibus_decode(byte_input.buf, values, &ibus_failsafe)) { add_input(IBUS_INPUT_CHANNELS, values, ibus_failsafe); } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp index 9d7b84255a..df27ee16fa 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp @@ -246,6 +246,7 @@ void AP_RCProtocol_SBUS::_process_byte(uint32_t timestamp_us, uint8_t b) byte_input.buf[byte_input.ofs++] = b; if (byte_input.ofs == sizeof(byte_input.buf)) { + log_data(AP_RCProtocol::SBUS, timestamp_us, byte_input.buf, byte_input.ofs); uint16_t values[SBUS_INPUT_CHANNELS]; uint16_t num_values=0; bool sbus_failsafe = false; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp index a48e55ec68..8e05bb6f4e 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp @@ -230,6 +230,7 @@ void AP_RCProtocol_SRXL::_process_byte(uint32_t timestamp_us, uint8_t byte) crc_fmu = crc_xmodem_update(crc_fmu,byte); } if (buflen == frame_len_full) { + log_data(AP_RCProtocol::SRXL, timestamp_us, buffer, buflen); /* CRC check here */ crc_receiver = ((uint16_t)buffer[buflen-2] << 8U) | ((uint16_t)buffer[buflen-1]); if (crc_receiver == crc_fmu) { diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp index eec468e59f..a6c16c6cb6 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp @@ -138,6 +138,8 @@ void AP_RCProtocol_ST24::_process_byte(uint8_t byte) _rxpacket.crc8 = byte; _rxlen++; + log_data(AP_RCProtocol::ST24, AP_HAL::micros(), (const uint8_t *)&_rxpacket, _rxlen+3); + if (st24_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) { /* decode the actual packet */ diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp index 03fe09b935..fa57571d2b 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp @@ -248,6 +248,8 @@ void AP_RCProtocol_SUMD::_process_byte(uint32_t timestamp_us, uint8_t byte) } } + log_data(AP_RCProtocol::SUMD, timestamp_us, _rxpacket.sumd_data, _rxlen); + if (_crcOK) { #ifdef SUMD_DEBUG hal.console->printf(" CRC - OK \n") ;