AP_IOMCU: resync for 4.0 update

libraries/AP_IOMCU/AP_IOMCU.cpp

@@ -334,6 +334,16 @@ void AP_IOMCU::read_status()
     uint32_t now = AP_HAL::millis();
     if (now - last_log_ms >= 1000U) {
         last_log_ms = now;
+        if (AP_Logger::get_singleton()) {
+// @LoggerMessage: IOMC
+// @Description: IOMCU diagnostic information
+// @Field: TimeUS: Time since system startup
+// @Field: Mem: Free memory
+// @Field: TS: IOMCU uptime
+// @Field: NPkt: Number of packets received by IOMCU
+// @Field: Nerr: Protocol failures on MCU side
+// @Field: Nerr2: Reported number of failures on IOMCU side
+// @Field: NDel: Number of delayed packets received by MCU
         AP::logger().Write("IOMC", "TimeUS,Mem,TS,NPkt,Nerr,Nerr2,NDel", "QHIIIII",
                            AP_HAL::micros64(),
                            reg_status.freemem,
@@ -342,6 +352,7 @@ void AP_IOMCU::read_status()
                            total_errors,
                            reg_status.num_errors,
                            num_delayed);
+        }
 #if IOMCU_DEBUG_ENABLE
         static uint32_t last_io_print;
         if (now - last_io_print >= 5000) {

libraries/AP_IOMCU/AP_IOMCU.h

@@ -71,6 +71,11 @@ public:
     // get the name of the RC protocol
     const char *get_rc_protocol(void);
 
+    // get receiver RSSI
+    int16_t get_RSSI(void) const {
+        return rc_input.rssi;
+    }
+    
     /*
       get servo rail voltage
      */

libraries/AP_IOMCU/iofirmware/iofirmware.cpp

@@ -304,6 +304,7 @@ void AP_IOMCU_FW::rcin_update()
         hal.rcin->read(rc_input.pwm, IOMCU_MAX_CHANNELS);
         rc_last_input_ms = last_ms;
         rc_input.rc_protocol = (uint16_t)AP::RC().protocol_detected();
+        rc_input.rssi = AP::RC().get_RSSI();
     } else if (last_ms - rc_last_input_ms > 200U) {
         rc_input.flags_rc_ok = false;
     }

libraries/AP_IOMCU/iofirmware/ioprotocol.h

@@ -123,6 +123,7 @@ struct page_rc_input {
     uint8_t flags_rc_ok:1;
     uint8_t rc_protocol;
     uint16_t pwm[IOMCU_MAX_CHANNELS];
+    int16_t rssi;
 };
 
 /*

libraries/AP_IOMCU/iofirmware/rc.cpp

@@ -28,7 +28,7 @@
 extern const AP_HAL::HAL& hal;
 
 // usart3 is for SBUS input and output
-static const SerialConfig uart3_cfg = {
+static const SerialConfig sbus_cfg = {
     100000,   // speed
     USART_CR1_PCE | USART_CR1_M, // cr1, enable even parity
     USART_CR2_STOP_1,            // cr2, two stop bits
@@ -40,15 +40,19 @@ static const SerialConfig uart3_cfg = {
 // listen for parity errors on sd3 input
 static event_listener_t sd3_listener;
 
+static uint8_t sd3_config;
+
 void sbus_out_write(uint16_t *channels, uint8_t nchannels)
 {
-    uint8_t buffer[25];
+    if (sd3_config == 0) {
-    AP_SBusOut::sbus_format_frame(channels, nchannels, buffer);
+        uint8_t buffer[25];
-    chnWrite(&SD3, buffer, sizeof(buffer));
+        AP_SBusOut::sbus_format_frame(channels, nchannels, buffer);
+        chnWrite(&SD3, buffer, sizeof(buffer));
+    }
 }
 
 // usart1 is for DSM input and (optionally) debug to FMU
-static const SerialConfig uart1_cfg = {
+static const SerialConfig dsm_cfg = {
     115200,   // speed
     0,        // cr1
     0,        // cr2
@@ -62,16 +66,17 @@ static const SerialConfig uart1_cfg = {
  */
 void AP_IOMCU_FW::rcin_serial_init(void)
 {
-    sdStart(&SD1, &uart1_cfg);
+    sdStart(&SD1, &dsm_cfg);
-    sdStart(&SD3, &uart3_cfg);
+    sdStart(&SD3, &sbus_cfg);
     chEvtRegisterMaskWithFlags(chnGetEventSource(&SD3),
                                &sd3_listener,
                                EVENT_MASK(1),
                                SD_PARITY_ERROR);
     // disable input for SBUS with pulses, we will use the UART for
-    // SBUS.
+    // SBUS and FPORT
     AP::RC().disable_for_pulses(AP_RCProtocol::SBUS);
     AP::RC().disable_for_pulses(AP_RCProtocol::SBUS_NI);
+    AP::RC().disable_for_pulses(AP_RCProtocol::FPORT);
 }
 
 static struct {
@@ -79,6 +84,7 @@ static struct {
     uint32_t num_sbus_bytes;
     uint32_t num_sbus_errors;
     eventflags_t sbus_error;
+    uint32_t last_good_ms;
 } rc_stats;
 
 /*
@@ -88,13 +94,19 @@ void AP_IOMCU_FW::rcin_serial_update(void)
 {
     uint8_t b[16];
     uint32_t n;
+    uint32_t now = AP_HAL::millis();
 
     // read from DSM port
     if ((n = chnReadTimeout(&SD1, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
         n = MIN(n, sizeof(b));
-        rc_stats.num_dsm_bytes += n;
+        // don't mix two 115200 uarts
-        for (uint8_t i=0; i<n; i++) {
+        if (sd3_config == 0) {
-            AP::RC().process_byte(b[i], 115200);
+            rc_stats.num_dsm_bytes += n;
+            for (uint8_t i=0; i<n; i++) {
+                if (AP::RC().process_byte(b[i], 115200)) {
+                    rc_stats.last_good_ms = now;
+                }
+            }
         }
         //BLUE_TOGGLE();
     }
@@ -102,17 +114,26 @@ void AP_IOMCU_FW::rcin_serial_update(void)
     // read from SBUS port
     if ((n = chnReadTimeout(&SD3, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
         eventflags_t flags;
-        if ((flags = chEvtGetAndClearFlags(&sd3_listener)) & SD_PARITY_ERROR) {
+        if (sd3_config == 0 && ((flags = chEvtGetAndClearFlags(&sd3_listener)) & SD_PARITY_ERROR)) {
             rc_stats.sbus_error = flags;
             rc_stats.num_sbus_errors++;
         } else {
             n = MIN(n, sizeof(b));
             rc_stats.num_sbus_bytes += n;
             for (uint8_t i=0; i<n; i++) {
-                AP::RC().process_byte(b[i], 100000);
+                if (AP::RC().process_byte(b[i], sd3_config==0?100000:115200)) {
+                    rc_stats.last_good_ms = now;
+                }
             }
         }
     }
+    if (now - rc_stats.last_good_ms > 2000) {
+        rc_stats.last_good_ms = now;
+        sd3_config ^= 1;
+        sdStop(&SD3);
+        sdStart(&SD3, sd3_config==0?&sbus_cfg:&dsm_cfg);
+    }
+
 }
 
 /*