From e46a640b2c7d82f3150a13ef6fec3308e0fd188d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 29 Oct 2018 16:51:22 +1100
Subject: [PATCH] AP_SbusOut: expose SBUS formatting function

---
 libraries/AP_SBusOut/AP_SBusOut.cpp | 99 +++++++++++++++++------------
 libraries/AP_SBusOut/AP_SBusOut.h   |  4 ++
 2 files changed, 62 insertions(+), 41 deletions(-)

diff --git a/libraries/AP_SBusOut/AP_SBusOut.cpp b/libraries/AP_SBusOut/AP_SBusOut.cpp
index 1d66585d5c..53d022f84b 100644
--- a/libraries/AP_SBusOut/AP_SBusOut.cpp
+++ b/libraries/AP_SBusOut/AP_SBusOut.cpp
@@ -73,6 +73,45 @@ AP_SBusOut::AP_SBusOut(void)
     AP_Param::setup_object_defaults(this, var_info);
 }
 
+/*
+  format a SBUS output frame into a 25 byte buffer
+ */
+void AP_SBusOut::sbus_format_frame(uint16_t *channels, uint8_t num_channels, uint8_t buffer[SBUS_BSIZE])
+{
+    uint8_t index = 1;
+    uint8_t offset = 0;
+
+    buffer[0] = 0x0f;
+
+    /* construct sbus frame representing channels 1 through 16 (max) */
+    uint8_t nchan = MIN(num_channels, SBUS_CHANNELS);
+    for (unsigned i = 0; i < nchan; ++i) {
+        /*protect from out of bounds values and limit to 11 bits*/
+        uint16_t pwmval = MAX(channels[i], SBUS_MIN);
+        uint16_t value = (uint16_t)((pwmval - SBUS_MIN) * SBUS_SCALE);
+        if (value > 0x07ff) {
+            value = 0x07ff;
+        }
+
+#if SBUS_DEBUG
+        static uint16_t lastch9 = 0;
+        if ((i==8) && (pwmval != lastch9)) {
+            lastch9 = pwmval;
+            hal.console->printf("channel 9 pwm: %04d\n", pwmval);
+        }
+#endif
+
+        while (offset >= 8) {
+            ++index;
+            offset -= 8;
+        }
+
+        buffer[index] |= (value << (offset)) & 0xff;
+        buffer[index + 1] |= (value >> (8 - offset)) & 0xff;
+        buffer[index + 2] |= (value >> (16 - offset)) & 0xff;
+        offset += 11;
+    }
+}
 
 /*
  * build and send sbus1 frame representing first 16 servo channels
@@ -93,59 +132,37 @@ AP_SBusOut::update()
     // constrain output rate using sbus_frame_interval
     static uint32_t last_micros = 0;
     uint32_t now = AP_HAL::micros();
-    if ((now - last_micros) > sbus_frame_interval) {
-        last_micros = now;
-        uint8_t buffer[SBUS_BSIZE] = { 0x0f };  // first byte is always 0x0f
-        uint8_t index = 1;
-        uint8_t offset = 0;
-        uint16_t value;
+    if ((now - last_micros) <= sbus_frame_interval) {
+        return;
+    }
 
-        /* construct sbus frame representing channels 1 through 16 (max) */
-        uint8_t nchan = MIN(NUM_SERVO_CHANNELS, SBUS_CHANNELS);
-        for (unsigned i = 0; i < nchan; ++i) {
-            SRV_Channel *c = SRV_Channels::srv_channel(i);
-            if (c == nullptr) {
-                continue;
-            }
-            /*protect from out of bounds values and limit to 11 bits*/
-            uint16_t pwmval = MAX(c->get_output_pwm(), SBUS_MIN);
-            value = (uint16_t)((pwmval - SBUS_MIN) * SBUS_SCALE);
-            if (value > 0x07ff) {
-                value = 0x07ff;
-            }
+    last_micros = now;
 
-#if SBUS_DEBUG
-            static uint16_t lastch9 = 0;
-            if ((i==8) && (pwmval != lastch9)) {
-                lastch9 = pwmval;
-                hal.console->printf("channel 9 pwm: %04d\n", pwmval);
-            }
-#endif
+    /* construct sbus frame representing channels 1 through 16 (max) */
+    uint8_t nchan = MIN(NUM_SERVO_CHANNELS, SBUS_CHANNELS);
+    uint16_t channels[SBUS_CHANNELS] {};
 
-            while (offset >= 8) {
-                ++index;
-                offset -= 8;
-            }
-
-            buffer[index] |= (value << (offset)) & 0xff;
-            buffer[index + 1] |= (value >> (8 - offset)) & 0xff;
-            buffer[index + 2] |= (value >> (16 - offset)) & 0xff;
-            offset += 11;
+    for (unsigned i = 0; i < nchan; ++i) {
+        SRV_Channel *c = SRV_Channels::srv_channel(i);
+        if (c == nullptr) {
+            continue;
         }
+        channels[i] = c->get_output_pwm();
+    }
+    uint8_t buffer[SBUS_BSIZE];
+    sbus_format_frame(channels, nchan, buffer);
 
 #if SBUS_DEBUG
-        hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
-        hal.gpio->write(55, 1);
+    hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
+    hal.gpio->write(55, 1);
 #endif
 
     sbus1_uart->write(buffer, sizeof(buffer));
 
 #if SBUS_DEBUG
-        hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
-        hal.gpio->write(55, 0);
+    hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
+    hal.gpio->write(55, 0);
 #endif
-
-    }
 }
 
 void AP_SBusOut::init() {
diff --git a/libraries/AP_SBusOut/AP_SBusOut.h b/libraries/AP_SBusOut/AP_SBusOut.h
index 08acfdbdca..84ca672c90 100644
--- a/libraries/AP_SBusOut/AP_SBusOut.h
+++ b/libraries/AP_SBusOut/AP_SBusOut.h
@@ -23,7 +23,11 @@ public:
 
     void update();
 
+    // public format function for use by IOMCU
+    static void sbus_format_frame(uint16_t *channels, uint8_t num_channels, uint8_t buffer[25]);
+
 private:
+
     AP_HAL::UARTDriver *sbus1_uart;
 
     void init(void);