AP_IOMCU: enable DSM input in iofirmware

libraries/AP_IOMCU/iofirmware/iofirmware.cpp

@@ -132,10 +132,6 @@ static UARTConfig uart_cfg = {
 
 void setup(void)
 {
-    // we need to release the JTAG reset pin to be used as a GPIO, otherwise we can't enable
-    // or disable SBUS out
-    AFIO->MAPR = AFIO_MAPR_SWJ_CFG_NOJNTRST;
-
     hal.rcin->init();
     hal.rcout->init();
 
@@ -166,12 +162,14 @@ void AP_IOMCU_FW::init()
     }
 
     adc_init();
-    sbus_out_init();
+    rcin_serial_init();
 
     // power on spektrum port
     palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_PWR_EN, PAL_MODE_OUTPUT_PUSHPULL);
     palSetLine(HAL_GPIO_PIN_SPEKTRUM_PWR_EN);
 
+    rcprotocol = AP_RCProtocol::get_instance();
+
     // we do no allocations after setup completes
     reg_status.freemem = hal.util->available_memory();
 }
@@ -233,6 +231,7 @@ void AP_IOMCU_FW::update()
         rcin_update();
         safety_update();
         rcout_mode_update();
+        rcin_serial_update();
         hal.rcout->timer_tick();
         if (dsm_bind_state) {
             dsm_bind_step();
@@ -475,6 +474,11 @@ bool AP_IOMCU_FW::handle_code_write()
 
                 // enable SBUS output at specified rate
                 sbus_interval_ms = MAX(1000U / reg_setup.sbus_rate,3);
+
+                // we need to release the JTAG reset pin to be used as a GPIO, otherwise we can't enable
+                // or disable SBUS out
+                AFIO->MAPR = AFIO_MAPR_SWJ_CFG_NOJNTRST;
+
                 palClearLine(HAL_GPIO_PIN_SBUS_OUT_EN);
             } else {
                 palSetLine(HAL_GPIO_PIN_SBUS_OUT_EN);

libraries/AP_IOMCU/iofirmware/iofirmware.h

@@ -1,4 +1,5 @@
 #include <AP_HAL/AP_HAL.h>
+#include <AP_RCProtocol/AP_RCProtocol.h>
 
 
 #include "ch.h"
@@ -27,6 +28,8 @@ private:
     void schedule_reboot(uint32_t time_ms);
     void safety_update();
     void rcout_mode_update();
+    void rcin_serial_init();
+    void rcin_serial_update();
     void page_status_update(void);
     void fill_failsafe_pwm(void);
     void run_mixer(void);
@@ -110,6 +113,8 @@ private:
     uint32_t sbus_last_ms;
     uint32_t sbus_interval_ms;
 
+    AP_RCProtocol *rcprotocol;
+
     uint32_t fmu_data_received_time;
     uint32_t last_heater_ms;
     uint32_t reboot_time;

libraries/AP_IOMCU/iofirmware/rc.cpp

@@ -25,6 +25,9 @@
 
 extern const AP_HAL::HAL& hal;
 
+static bool sbus_out_initialised;
+
+// usart3 is for SBUS input and output
 static const SerialConfig uart3_cfg = {
     100000,   // speed
     USART_CR1_PCE | USART_CR1_M, // cr1, enable even parity
@@ -34,18 +37,55 @@ static const SerialConfig uart3_cfg = {
     nullptr,  // ctx
 };
 
-void sbus_out_init(void)
-{
-    sdStart(&SD3, &uart3_cfg);
-}
-
 void sbus_out_write(uint16_t *channels, uint8_t nchannels)
 {
+    if (!sbus_out_initialised) {
+        sdStart(&SD3, &uart3_cfg);
+        sbus_out_initialised = true;
+    }
     uint8_t buffer[25];
     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 = {
+    115200,   // speed
+    0,        // cr1
+    0,        // cr2
+    0,        // cr3
+    nullptr,  // irq_cb
+    nullptr,  // ctx
+};
+
+/*
+  init rcin on DSM USART1
+ */
+void AP_IOMCU_FW::rcin_serial_init(void)
+{
+    sdStart(&SD1, &uart1_cfg);
+}
+
+
+static uint32_t num_dsm_bytes;
+
+/*
+  check for data on DSM RX uart
+ */
+void AP_IOMCU_FW::rcin_serial_update(void)
+{
+    uint8_t b[16];
+    uint32_t n;
+    if ((n = chnReadTimeout(&SD1, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
+        n = MIN(n, sizeof(b));
+        num_dsm_bytes += n;
+        for (uint8_t i=0; i<n; i++) {
+            rcprotocol->process_byte(b[i]);
+        }
+        //palToggleLine(HAL_GPIO_PIN_HEATER);
+    }
+}
+
 /*
   sleep for 1ms using a busy loop
  */
@@ -101,9 +141,8 @@ void AP_IOMCU_FW::dsm_bind_step(void)
 
     case 4:
         if (now - last_dsm_bind_ms >= 50) {
-            // set back as alternate function with pullup for UART input
-            palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_OUT, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
-            palSetLine(HAL_GPIO_PIN_SPEKTRUM_OUT);
+            // set back as input pin
+            palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_OUT, PAL_MODE_INPUT);
             dsm_bind_state = 0;
         }
         break;

libraries/AP_IOMCU/iofirmware/rc.h

@@ -1,3 +1,2 @@
 
-void sbus_out_init(void);
 void sbus_out_write(uint16_t *channels, uint8_t nchannels);