AP_IOMCU: cleanup IO protocol and add logging

this cleans up the IOMCU protocol structures now we don't need to be
compatible with the old nuttx based protocol and adds logging of IOMCU
errors

libraries/AP_IOMCU/AP_IOMCU.cpp

@@ -18,6 +18,7 @@
 #include <RC_Channel/RC_Channel.h>
 #include <AP_RCProtocol/AP_RCProtocol.h>
 #include <AP_InternalError/AP_InternalError.h>
+#include <AP_Logger/AP_Logger.h>
 
 extern const AP_HAL::HAL &hal;
 
@@ -45,7 +46,9 @@ AP_IOMCU::AP_IOMCU(AP_HAL::UARTDriver &_uart) :
     uart(_uart)
 {}
 
-#if 0
+#define IOMCU_DEBUG_ENABLE 0
+
+#if IOMCU_DEBUG_ENABLE
 #define debug(fmt, args ...)  do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
 #else
 #define debug(fmt, args ...)
@@ -220,13 +223,6 @@ void AP_IOMCU::thread_main(void)
             last_servo_read_ms = AP_HAL::millis();
         }
 
-#ifdef IOMCU_DEBUG
-        if (now - last_debug_ms > 1000) {
-            print_debug();
-            last_debug_ms = AP_HAL::millis();
-        }
-#endif // IOMCU_DEBUG
-
         if (now - last_safety_option_check_ms > 1000) {
             update_safety_options();
             last_safety_option_check_ms = now;
@@ -284,13 +280,12 @@ void AP_IOMCU::send_servo_out()
  */
 void AP_IOMCU::read_rc_input()
 {
-    // read a min of 9 channels and max of IOMCU_MAX_CHANNELS
+    uint16_t *r = (uint16_t *)&rc_input;
-    uint8_t n = MIN(MAX(9, rc_input.count), IOMCU_MAX_CHANNELS);
+    if (!read_registers(PAGE_RAW_RCIN, 0, sizeof(rc_input)/2, r)) {
-    if (!read_registers(PAGE_RAW_RCIN, 0, 6+n, (uint16_t *)&rc_input)) {
         return;
     }
     if (rc_input.flags_rc_ok && !rc_input.flags_failsafe) {
-        rc_input.last_input_ms = AP_HAL::millis();
+        rc_last_input_ms = AP_HAL::millis();
     }
 }
 
@@ -325,6 +320,31 @@ void AP_IOMCU::read_status()
             force_safety_off();
         }
     }
+
+    uint32_t now = AP_HAL::millis();
+    if (now - last_log_ms >= 1000U) {
+        last_log_ms = now;
+        AP::logger().Write("IOMC", "TimeUS,Mem,TS,NPkt,Nerr,Nerr2", "QHIIII",
+                           AP_HAL::micros64(),
+                           reg_status.freemem,
+                           reg_status.timestamp_ms,
+                           reg_status.total_pkts,
+                           total_errors,
+                           reg_status.num_errors);
+#if IOMCU_DEBUG_ENABLE
+        static uint32_t last_io_err;
+        if (last_io_err != reg_status.num_errors) {
+            debug("t=%u num=%u nerr=%u crc=%u opcode=%u rd=%u wr=%u ur=%u\n",
+                  now, reg_status.total_pkts, reg_status.num_errors,
+                  reg_status.err_crc,
+                  reg_status.err_bad_opcode,
+                  reg_status.err_read,
+                  reg_status.err_write,
+                  reg_status.err_uart);
+            last_io_err = reg_status.num_errors;
+        }
+#endif // IOMCU_DEBUG_ENABLE
+    }
 }
 
 /*
@@ -349,6 +369,21 @@ void AP_IOMCU::discard_input(void)
     }
 }
 
+/*
+  write a packet, retrying as needed
+ */
+size_t AP_IOMCU::write_wait(const uint8_t *pkt, uint8_t len)
+{
+    uint8_t wait_count = 20;
+    size_t ret;
+    do {
+        ret = uart.write(pkt, len);
+        if (ret == 0) {
+            hal.scheduler->delay_microseconds(100);
+        }
+    } while (ret == 0 && wait_count--);
+    return ret;
+}
 
 /*
   read count 16 bit registers
@@ -378,17 +413,22 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
 
     uint8_t pkt_size = pkt.get_size();
     if (is_chibios_backend) {
-        // save bandwidth on reads
+        /*
+          the original read protocol is a bit strange, as it
+          unnecessarily sends the same size packet that it expects to
+          receive. This means reading a large number of registers
+          wastes a lot of serial bandwidth. We avoid this overhead
+          when we know we are talking to a ChibiOS backend
+        */
         pkt_size = 4;
     }
     
-    /*
-      the protocol is a bit strange, as it unnecessarily sends the
-      same size packet that it expects to receive. This means reading
-      a large number of registers wastes a lot of serial bandwidth
-     */
     pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt_size);
-    if (uart.write((uint8_t *)&pkt, pkt_size) != pkt_size) {
+
+    size_t ret = write_wait((uint8_t *)&pkt, pkt_size);
+
+    if (ret != pkt_size) {
+        debug("write failed1 %u %u %u\n", unsigned(pkt_size), page, offset);
         protocol_fail_count++;
         return false;
     }
@@ -443,6 +483,7 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
     if (protocol_fail_count > IOMCU_MAX_REPEATED_FAILURES) {
         handle_repeated_failures();
     }
+    total_errors += protocol_fail_count;
     protocol_fail_count = 0;
     protocol_count++;
     return true;
@@ -474,14 +515,19 @@ bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, cons
     pkt.crc = 0;
     memcpy(pkt.regs, regs, 2*count);
     pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size());
-    if (uart.write((uint8_t *)&pkt, pkt.get_size()) != pkt.get_size()) {
+
+    const uint8_t pkt_size = pkt.get_size();
+    size_t ret = write_wait((uint8_t *)&pkt, pkt_size);
+
+    if (ret != pkt_size) {
+        debug("write failed2 %u %u %u %u\n", pkt_size, page, offset, ret);
         protocol_fail_count++;
         return false;
     }
 
     // wait for the expected number of reply bytes or timeout
     if (!uart.wait_timeout(4, 10)) {
-        //debug("no reply for %u/%u/%u\n", page, offset, count);
+        debug("no reply for %u/%u/%u\n", page, offset, count);
         protocol_fail_count++;
         return false;
     }
@@ -512,6 +558,7 @@ bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, cons
     if (protocol_fail_count > IOMCU_MAX_REPEATED_FAILURES) {
         handle_repeated_failures();
     }
+    total_errors += protocol_fail_count;
     protocol_fail_count = 0;
     protocol_count++;
     return true;
@@ -545,17 +592,6 @@ void AP_IOMCU::write_channel(uint8_t chan, uint16_t pwm)
     }
 }
 
-void AP_IOMCU::print_debug(void)
-{
-#ifdef IOMCU_DEBUG
-    const uint16_t *r = (const uint16_t *)&reg_status;
-    for (uint8_t i=0; i<sizeof(reg_status)/2; i++) {
-        hal.console->printf("%04x ", r[i]);
-    }
-    hal.console->printf("\n");
-#endif // IOMCU_DEBUG
-}
-
 // trigger an ioevent
 void AP_IOMCU::trigger_event(uint8_t event)
 {
@@ -644,10 +680,10 @@ bool AP_IOMCU::enable_sbus_out(uint16_t rate_hz)
 */
 bool AP_IOMCU::check_rcinput(uint32_t &last_frame_us, uint8_t &num_channels, uint16_t *channels, uint8_t max_chan)
 {
-    if (last_frame_us != uint32_t(rc_input.last_input_ms * 1000U)) {
+    if (last_frame_us != uint32_t(rc_last_input_ms * 1000U)) {
         num_channels = MIN(MIN(rc_input.count, IOMCU_MAX_CHANNELS), max_chan);
         memcpy(channels, rc_input.pwm, num_channels*2);
-        last_frame_us = uint32_t(rc_input.last_input_ms * 1000U);
+        last_frame_us = uint32_t(rc_last_input_ms * 1000U);
         return true;
     }
     return false;
@@ -909,7 +945,7 @@ const char *AP_IOMCU::get_rc_protocol(void)
     if (!is_chibios_backend) {
         return nullptr;
     }
-    return AP_RCProtocol::protocol_name_from_protocol((AP_RCProtocol::rcprotocol_t)rc_input.data);
+    return AP_RCProtocol::protocol_name_from_protocol((AP_RCProtocol::rcprotocol_t)rc_input.rc_protocol);
 }
 
 /*

libraries/AP_IOMCU/AP_IOMCU.h

@@ -134,7 +134,6 @@ private:
     uint32_t last_status_read_ms;
     uint32_t last_rc_read_ms;
     uint32_t last_servo_read_ms;
-    uint32_t last_debug_ms;
     uint32_t last_safety_option_check_ms;
 
     // last value of safety options
@@ -147,7 +146,6 @@ private:
     void read_rc_input(void);
     void read_servo(void);
     void read_status(void);
-    void print_debug(void);
     void discard_input(void);
     void event_failed(uint8_t event);
     void update_safety_options(void);
@@ -157,9 +155,11 @@ private:
 
     // PAGE_STATUS values
     struct page_reg_status reg_status;
+    uint32_t last_log_ms;
 
     // PAGE_RAW_RCIN values
     struct page_rc_input rc_input;
+    uint32_t rc_last_input_ms;
 
     // MIXER values
     struct page_mixing mixing;
@@ -206,6 +206,7 @@ private:
 
     uint32_t protocol_fail_count;
     uint32_t protocol_count;
+    uint32_t total_errors;
     uint32_t last_iocmu_timestamp_ms;
 
     // firmware upload
@@ -213,6 +214,7 @@ private:
     uint8_t *fw;
     uint32_t fw_size;
 
+    size_t write_wait(const uint8_t *pkt, uint8_t len);
     bool upload_fw(void);
     bool recv_byte_with_timeout(uint8_t *c, uint32_t timeout_ms);
     bool recv_bytes(uint8_t *p, uint32_t count);

libraries/AP_IOMCU/iofirmware/iofirmware.cpp

@@ -49,11 +49,6 @@ enum ioevents {
     IOEVENT_PWM=1,
 };
 
-static struct {
-    uint32_t num_code_read, num_bad_crc, num_write_pkt, num_unknown_pkt;
-    uint32_t num_idle_rx, num_dma_complete_rx, num_total_rx, num_rx_error;
-} stats;
-
 static void dma_rx_end_cb(UARTDriver *uart)
 {
     osalSysLockFromISR();
@@ -66,8 +61,6 @@ static void dma_rx_end_cb(UARTDriver *uart)
     dmaStreamDisable(uart->dmatx);
 
     iomcu.process_io_packet();
-    stats.num_total_rx++;
-    stats.num_dma_complete_rx = stats.num_total_rx - stats.num_idle_rx;
 
     dmaStreamSetMemory0(uart->dmarx, &iomcu.rx_io_packet);
     dmaStreamSetTransactionSize(uart->dmarx, sizeof(iomcu.rx_io_packet));
@@ -97,7 +90,8 @@ static void idle_rx_handler(UARTDriver *uart)
         osalSysLockFromISR();
         uart->usart->SR = ~USART_SR_LBD;
         uart->usart->CR1 |= USART_CR1_SBK;
-        stats.num_rx_error++;
+        iomcu.reg_status.num_errors++;
+        iomcu.reg_status.err_uart++;
         uart->usart->CR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
         (void)uart->usart->SR;
         (void)uart->usart->DR;
@@ -117,7 +111,6 @@ static void idle_rx_handler(UARTDriver *uart)
 
     if (sr & USART_SR_IDLE) {
         dma_rx_end_cb(uart);
-        stats.num_idle_rx++;
     }
 }
 
@@ -311,9 +304,9 @@ void AP_IOMCU_FW::rcin_update()
         for (uint8_t i = 0; i < IOMCU_MAX_CHANNELS; i++) {
             rc_input.pwm[i] = hal.rcin->read(i);
         }
-        rc_input.last_input_ms = last_ms;
+        rc_last_input_ms = last_ms;
-        rc_input.data = (uint16_t)rcprotocol->protocol_detected();
+        rc_input.rc_protocol = (uint16_t)rcprotocol->protocol_detected();
-    } else if (last_ms - rc_input.last_input_ms > 200U) {
+    } else if (last_ms - rc_last_input_ms > 200U) {
         rc_input.flags_rc_ok = false;
     }
     if (update_rcout_freq) {
@@ -345,6 +338,8 @@ void AP_IOMCU_FW::rcin_update()
 
 void AP_IOMCU_FW::process_io_packet()
 {
+    iomcu.reg_status.total_pkts++;
+
     uint8_t rx_crc = rx_io_packet.crc;
     uint8_t calc_crc;
     rx_io_packet.crc = 0;
@@ -365,12 +360,12 @@ void AP_IOMCU_FW::process_io_packet()
         tx_io_packet.page = 0;
         tx_io_packet.offset = 0;
         tx_io_packet.crc =  crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
-        stats.num_bad_crc++;
+        iomcu.reg_status.num_errors++;
+        iomcu.reg_status.err_crc++;
         return;
     }
     switch (rx_io_packet.code) {
     case CODE_READ: {
-        stats.num_code_read++;
         if (!handle_code_read()) {
             tx_io_packet.count = 0;
             tx_io_packet.code = CODE_ERROR;
@@ -378,11 +373,12 @@ void AP_IOMCU_FW::process_io_packet()
             tx_io_packet.page = 0;
             tx_io_packet.offset = 0;
             tx_io_packet.crc =  crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
+            iomcu.reg_status.num_errors++;
+            iomcu.reg_status.err_read++;
         }
     }
     break;
     case CODE_WRITE: {
-        stats.num_write_pkt++;
         if (!handle_code_write()) {
             tx_io_packet.count = 0;
             tx_io_packet.code = CODE_ERROR;
@@ -390,11 +386,14 @@ void AP_IOMCU_FW::process_io_packet()
             tx_io_packet.page = 0;
             tx_io_packet.offset = 0;
             tx_io_packet.crc =  crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
+            iomcu.reg_status.num_errors++;
+            iomcu.reg_status.err_write++;
         }
     }
     break;
     default: {
-        stats.num_unknown_pkt++;
+        iomcu.reg_status.num_errors++;
+        iomcu.reg_status.err_bad_opcode++;
     }
     break;
     }
@@ -585,8 +584,6 @@ bool AP_IOMCU_FW::handle_code_write()
             i++;
         }
         fmu_data_received_time = last_ms;
-        reg_status.flag_fmu_ok = true;
-        reg_status.flag_raw_pwm = true;
         chEvtSignalI(thread_ctx, EVENT_MASK(IOEVENT_PWM));
         break;
     }

libraries/AP_IOMCU/iofirmware/iofirmware.h

@@ -22,7 +22,6 @@ public:
     void update();
     void calculate_fw_crc(void);
 
-private:
     void pwm_out_update();
     void heater_update();
     void rcin_update();
@@ -44,7 +43,7 @@ private:
     int16_t mix_elevon_vtail(int16_t angle1, int16_t angle2, bool first_output) const;
     void dsm_bind_step(void);
 
-    struct PACKED {
+    struct {
         /* default to RSSI ADC functionality */
         uint16_t features;
         uint16_t arming;
@@ -78,6 +77,7 @@ private:
 
     // PAGE_RAW_RCIN values
     struct page_rc_input rc_input;
+    uint32_t rc_last_input_ms;
 
     // PAGE_SERVO values
     struct {
@@ -98,7 +98,7 @@ private:
     struct {
         uint16_t pwm[IOMCU_MAX_CHANNELS];
     } reg_safety_pwm;
-    
+
     // output rates
     struct {
         uint16_t freq;

libraries/AP_IOMCU/iofirmware/ioprotocol.h

@@ -97,62 +97,38 @@ enum iopage {
 #define PAGE_REG_SETUP_FORCE_SAFETY_ON  14
 #define FORCE_SAFETY_MAGIC 22027
 
-struct PACKED page_config {
+struct page_config {
     uint16_t protocol_version;
     uint16_t protocol_version2;
 };
 
-struct PACKED page_reg_status {
+struct page_reg_status {
     uint16_t freemem;
-    uint16_t cpuload;
-
-    // status flags
-    uint16_t flag_outputs_armed:1;
-    uint16_t flag_override:1;
-    uint16_t flag_rc_ok:1;
-    uint16_t flag_rc_ppm:1;
-    uint16_t flag_rc_dsm:1;
-    uint16_t flag_rc_sbus:1;
-    uint16_t flag_fmu_ok:1;
-    uint16_t flag_raw_pwm:1;
-    uint16_t flag_mixer_ok:1;
-    uint16_t flag_arm_sync:1;
-    uint16_t flag_init_ok:1;
-    uint16_t flag_failsafe:1;
-    uint16_t flag_safety_off:1;
-    uint16_t flag_fmu_initialised:1;
-    uint16_t flag_rc_st24:1;
-    uint16_t flag_rc_sumd_srxl:1;
-
-    uint16_t alarms;
     uint32_t timestamp_ms;
     uint16_t vservo;
     uint16_t vrssi;
-    uint16_t prssi;
+    uint32_t num_errors;
+    uint32_t total_pkts;
+    uint8_t flag_safety_off;
+    uint8_t err_crc;
+    uint8_t err_bad_opcode;
+    uint8_t err_read;
+    uint8_t err_write;
+    uint8_t err_uart;
 };
 
-struct PACKED page_rc_input {
+struct page_rc_input {
-    uint16_t count;
+    uint8_t count;
-    uint16_t flags_frame_drop:1;
+    uint8_t flags_failsafe:1;
-    uint16_t flags_failsafe:1;
+    uint8_t flags_rc_ok:1;
-    uint16_t flags_dsm11:1;
+    uint8_t rc_protocol;
-    uint16_t flags_mapping_ok:1;
-    uint16_t flags_rc_ok:1;
-    uint16_t flags_unused:11;
-    uint16_t nrssi;
-    uint16_t data;
-    uint16_t frame_count;
-    uint16_t lost_frame_count;
     uint16_t pwm[IOMCU_MAX_CHANNELS];
-    // the following two fields are not transferred to the FMU
-    uint16_t last_frame_count;
-    uint32_t last_input_ms;
 };
 
 /*
   data for mixing on FMU failsafe
  */
-struct PACKED page_mixing {
+struct page_mixing {
     uint16_t servo_min[IOMCU_MAX_CHANNELS];
     uint16_t servo_max[IOMCU_MAX_CHANNELS];
     uint16_t servo_trim[IOMCU_MAX_CHANNELS];