From b9e4916c173af5de47c4c63a2c0913b46db0c634 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 1 Nov 2018 09:54:18 +1100
Subject: [PATCH] AP_IOMCU: fixed delay in DSM bind

---
 libraries/AP_IOMCU/iofirmware/iofirmware.cpp | 56 +--------------
 libraries/AP_IOMCU/iofirmware/rc.cpp         | 72 ++++++++++++++++++++
 2 files changed, 75 insertions(+), 53 deletions(-)

diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
index 8d8164cc4d..9dbc3d1cca 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
@@ -505,7 +505,9 @@ bool AP_IOMCU_FW::handle_code_write()
             break;
 
         case PAGE_REG_SETUP_DSM_BIND:
-            dsm_bind_state = 1;
+            if (dsm_bind_state == 0) {
+                dsm_bind_state = 1;
+            }
             break;
             
         default:
@@ -678,58 +680,6 @@ void AP_IOMCU_FW::fill_failsafe_pwm(void)
     }
 }
 
-/*
-  perform a DSM bind operation
- */
-void AP_IOMCU_FW::dsm_bind_step(void)
-{
-    uint32_t now = last_ms;
-    switch (dsm_bind_state) {
-    case 1:
-        palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_PWR_EN, PAL_MODE_OUTPUT_PUSHPULL);
-        palClearLine(HAL_GPIO_PIN_SPEKTRUM_PWR_EN);
-        palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_OUT, PAL_MODE_OUTPUT_PUSHPULL);
-        palSetLine(HAL_GPIO_PIN_SPEKTRUM_OUT);
-        dsm_bind_state = 2;
-        last_dsm_bind_ms = now;
-        break;
-
-    case 2:
-        if (now - last_dsm_bind_ms >= 500) {
-            palSetLine(HAL_GPIO_PIN_SPEKTRUM_PWR_EN);
-            dsm_bind_state = 3;
-            last_dsm_bind_ms = now;
-        }
-        break;
-
-    case 3: {
-        if (now - last_dsm_bind_ms >= 72) {
-            // 9 pulses works with all satellite receivers, and supports the highest
-            // available protocol
-            const uint8_t num_pulses = 9;
-            for (uint8_t i=0; i<num_pulses; i++) {
-                hal.scheduler->delay_microseconds(120);
-                palClearLine(HAL_GPIO_PIN_SPEKTRUM_OUT);
-                hal.scheduler->delay_microseconds(120);
-                palSetLine(HAL_GPIO_PIN_SPEKTRUM_OUT);
-            }
-            last_dsm_bind_ms = now;
-            dsm_bind_state = 4;
-        }
-        break;
-    }
-
-    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);
-            dsm_bind_state = 0;
-        }
-        break;
-    }
-}
-
 AP_HAL_MAIN();
 
 
diff --git a/libraries/AP_IOMCU/iofirmware/rc.cpp b/libraries/AP_IOMCU/iofirmware/rc.cpp
index 30ef13fb02..a2f8a97277 100644
--- a/libraries/AP_IOMCU/iofirmware/rc.cpp
+++ b/libraries/AP_IOMCU/iofirmware/rc.cpp
@@ -19,9 +19,12 @@
 
 #include "ch.h"
 #include "hal.h"
+#include "iofirmware.h"
 #include "rc.h"
 #include <AP_SBusOut/AP_SBusOut.h>
 
+extern const AP_HAL::HAL& hal;
+
 static const SerialConfig uart3_cfg = {
     100000,   // speed
     USART_CR1_PCE | USART_CR1_M, // cr1, enable even parity
@@ -42,3 +45,72 @@ void sbus_out_write(uint16_t *channels, uint8_t nchannels)
     AP_SBusOut::sbus_format_frame(channels, nchannels, buffer);
     chnWrite(&SD3, buffer, sizeof(buffer));
 }
+
+/*
+  sleep for 1ms using a busy loop
+ */
+static void delay_one_ms(uint32_t &now)
+{
+    while (now == AP_HAL::millis()) ;
+    now = AP_HAL::millis();
+}
+
+/*
+  perform a DSM bind operation
+ */
+void AP_IOMCU_FW::dsm_bind_step(void)
+{
+    uint32_t now = last_ms;
+    switch (dsm_bind_state) {
+    case 1:
+        palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_PWR_EN, PAL_MODE_OUTPUT_PUSHPULL);
+        palClearLine(HAL_GPIO_PIN_SPEKTRUM_PWR_EN);
+        palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_OUT, PAL_MODE_OUTPUT_PUSHPULL);
+        palSetLine(HAL_GPIO_PIN_SPEKTRUM_OUT);
+        dsm_bind_state = 2;
+        last_dsm_bind_ms = now;
+        break;
+
+    case 2:
+        if (now - last_dsm_bind_ms >= 500) {
+            palSetLine(HAL_GPIO_PIN_SPEKTRUM_PWR_EN);
+            dsm_bind_state = 3;
+            last_dsm_bind_ms = now;
+        }
+        break;
+
+    case 3: {
+        if (now - last_dsm_bind_ms >= 72) {
+            // 9 pulses works with all satellite receivers, and supports the highest
+            // available protocol
+            delay_one_ms(now);
+            const uint8_t num_pulses = 9;
+            for (uint8_t i=0; i<num_pulses; i++) {
+                // the delay should be 120us, but we are running our
+                // clock at 1kHz, and 1ms works fine
+                delay_one_ms(now);
+                palClearLine(HAL_GPIO_PIN_SPEKTRUM_OUT);
+                delay_one_ms(now);
+                palSetLine(HAL_GPIO_PIN_SPEKTRUM_OUT);
+            }
+            last_dsm_bind_ms = now;
+            dsm_bind_state = 4;
+        }
+        break;
+    }
+
+    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);
+            dsm_bind_state = 0;
+        }
+        break;
+
+    default:
+        dsm_bind_state = 0;
+        break;
+    }
+}
+