AP_RCProtocol: added logging of RC prototol input bytes

libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp

@@ -19,6 +19,7 @@
 #include <AP_Math/AP_Math.h>
 #include <RC_Channel/RC_Channel.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
+#include <AP_Logger/AP_Logger.h>
 
 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
+}

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;

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.

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);
         }

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);
         }

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;

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) {

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 */

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") ;