AP_IOMCU: fixed compat with nuttx firmwares

this allows older firmwares to change the IO firmware back to a nuttx
based firmware

libraries/AP_IOMCU/AP_IOMCU.cpp

@@ -105,11 +105,12 @@ void AP_IOMCU::thread_main(void)
 
         if (mask & EVENT_MASK(IOEVENT_INIT)) {
             // get protocol version
-            if (!read_registers(PAGE_CONFIG, PAGE_CONFIG_PROTOCOL_VERSION, sizeof(config)/2, (uint16_t *)&config)) {
+            if (!read_registers(PAGE_CONFIG, 0, sizeof(config)/2, (uint16_t *)&config)) {
                 event_failed(IOEVENT_INIT);
                 continue;
             }
-            debug("io protocol: %u\n", config.protocol_version);
+            is_chibios_backend = (config.protocol_version == IOMCU_PROTOCOL_VERSION &&
+                                  config.protocol_version2 == IOMCU_PROTOCOL_VERSION2);
 
             // set IO_ARM_OK and FMU_ARMED
             if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, 0,
@@ -359,7 +360,7 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
     pkt.crc = 0;
 
     uint8_t pkt_size = pkt.get_size();
-    if (config.protocol_version == IOMCU_PROTOCOL_VERSION) {
+    if (is_chibios_backend) {
         // save bandwidth on reads
         pkt_size = 4;
     }
@@ -789,8 +790,7 @@ void AP_IOMCU::shutdown(void)
  */
 void AP_IOMCU::bind_dsm(uint8_t mode)
 {
-    if (config.protocol_version != IOMCU_PROTOCOL_VERSION ||
-        hal.util->get_soft_armed()) {
+    if (!is_chibios_backend || hal.util->get_soft_armed()) {
         // only with ChibiOS IO firmware, and disarmed
         return;
     }
@@ -805,7 +805,7 @@ void AP_IOMCU::bind_dsm(uint8_t mode)
 bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan,
                             float mixing_gain, uint16_t manual_rc_mask)
 {
-    if (config.protocol_version != IOMCU_PROTOCOL_VERSION) {
+    if (!is_chibios_backend) {
         return false;
     }
     bool changed = false;

libraries/AP_IOMCU/AP_IOMCU.h

@@ -195,6 +195,7 @@ private:
 
     bool crc_is_ok;
     bool initialised;
+    bool is_chibios_backend;
 
     uint32_t protocol_fail_count;
 

libraries/AP_IOMCU/iofirmware/iofirmware.cpp

@@ -153,7 +153,10 @@ void loop(void)
 
 void AP_IOMCU_FW::init()
 {
+    // the first protocol version must be 4 to allow downgrade to
+    // old NuttX based firmwares
     config.protocol_version = IOMCU_PROTOCOL_VERSION;
+    config.protocol_version2 = IOMCU_PROTOCOL_VERSION2;
 
     thread_ctx = chThdGetSelfX();
 
@@ -408,9 +411,12 @@ bool AP_IOMCU_FW::handle_code_read()
 
     /* correct the data pointer and count for the offset */
     values += rx_io_packet.offset;
+    tx_io_packet.page = rx_io_packet.page;
+    tx_io_packet.offset = rx_io_packet.offset;
     tx_io_packet.count -= rx_io_packet.offset;
     tx_io_packet.count = MIN(tx_io_packet.count, rx_io_packet.count);
     tx_io_packet.count = MIN(tx_io_packet.count, PKT_MAX_REGS);
+    tx_io_packet.code = CODE_SUCCESS;
     memcpy(tx_io_packet.regs, values, sizeof(uint16_t)*tx_io_packet.count);
     tx_io_packet.crc = 0;
     tx_io_packet.crc =  crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());

libraries/AP_IOMCU/iofirmware/ioprotocol.h

@@ -81,8 +81,10 @@ enum iopage {
 #define PAGE_REG_SETUP_DSM_BIND     22
 
 // config page registers
-#define PAGE_CONFIG_PROTOCOL_VERSION 0
-#define IOMCU_PROTOCOL_VERSION       10
+#define PAGE_CONFIG_PROTOCOL_VERSION  0
+#define PAGE_CONFIG_PROTOCOL_VERSION2 1
+#define IOMCU_PROTOCOL_VERSION       4
+#define IOMCU_PROTOCOL_VERSION2     10
 
 // magic value for rebooting to bootloader
 #define REBOOT_BL_MAGIC 14662
@@ -93,6 +95,7 @@ enum iopage {
 
 struct PACKED page_config {
     uint16_t protocol_version;
+    uint16_t protocol_version2;
 };
 
 struct PACKED page_reg_status {