sbus_output: some reorgs and cleanups

* sbus output frame rate is specified by rate (config->sbusRate),
  instead of interval.
* Add a macro to enable and disable the feature.
* Use the scheduler to invoke the update routine (instead of piggyback
  in the pid routine).
* Some other minor cleanups and test fixes.

Test:
make
(with USE_SBUS_OUTPUT undefined) make
make unit_sbus_output_unittest
Device test performed with a logic analyzer

src/main/cli/settings.c

@@ -1740,11 +1740,13 @@ const clivalue_t valueTable[] = {
     { "box_user_4_name", VAR_UINT8 | MASTER_VALUE | MODE_STRING, .config.string = { 1, MAX_BOX_USER_NAME_LENGTH, STRING_FLAGS_NONE }, PG_MODE_ACTIVATION_CONFIG, offsetof(modeActivationConfig_t, box_user_4_name) },
 #endif
 
+#ifdef USE_SBUS_OUTPUT
     { "sbus_out_source_type",  VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = SBUS_OUT_CHANNELS, PG_DRIVER_SBUS_OUT_CONFIG, offsetof(sbusOutConfig_t, sourceType) },
     { "sbus_out_source_index", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = SBUS_OUT_CHANNELS, PG_DRIVER_SBUS_OUT_CONFIG, offsetof(sbusOutConfig_t, sourceIndex) },
     { "sbus_out_min",          VAR_INT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = SBUS_OUT_CHANNELS, PG_DRIVER_SBUS_OUT_CONFIG, offsetof(sbusOutConfig_t, min) },
     { "sbus_out_max",          VAR_INT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = SBUS_OUT_CHANNELS, PG_DRIVER_SBUS_OUT_CONFIG, offsetof(sbusOutConfig_t, max) },
-    { "sbus_out_interval",     VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = {4, 40}, PG_DRIVER_SBUS_OUT_CONFIG, offsetof(sbusOutConfig_t, interval) },
+    { "sbus_out_sbus_rate",    VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = {25, 250}, PG_DRIVER_SBUS_OUT_CONFIG, offsetof(sbusOutConfig_t, sbusRate) },
+#endif
 
 };
 

src/main/drivers/sbus_output.c

@@ -31,7 +31,6 @@
 #include "rx/rx.h"
 
 STATIC_UNIT_TESTED serialPort_t *sbusOutPort = NULL;
-STATIC_UNIT_TESTED timeUs_t sbusOutLastTxTimeUs = 0;
 
 STATIC_UNIT_TESTED void sbusOutPrepareSbusFrame(sbusOutFrame_t *frame,
                                                 uint16_t *channels) {
@@ -86,12 +85,12 @@ STATIC_UNIT_TESTED float sbusOutGetChannelValue(uint8_t channel) {
         sbusOutConfig()->sourceType[channel];
     const uint8_t source_index = sbusOutConfig()->sourceIndex[channel];
     switch (source_type) {
-        case SBUS_OUT_SOURCE_RX:
-            return sbusOutGetPwmRX(source_index);
-        case SBUS_OUT_SOURCE_MIXER:
-            return sbusOutGetValueMixer(source_index);
-        case SBUS_OUT_SOURCE_SERVO:
-            return sbusOutGetPwmServo(source_index);
+    case SBUS_OUT_SOURCE_RX:
+        return sbusOutGetPwmRX(source_index);
+    case SBUS_OUT_SOURCE_MIXER:
+        return sbusOutGetValueMixer(source_index);
+    case SBUS_OUT_SOURCE_SERVO:
+        return sbusOutGetPwmServo(source_index);
     }
     return 0;
 }
@@ -110,24 +109,25 @@ STATIC_UNIT_TESTED uint16_t sbusOutConvertToSbus(uint8_t channel, float pwm) {
 }
 
 void sbusOutUpdate(timeUs_t currentTimeUs) {
-    const timeUs_t sbusOutTxIntervalUs = 1000 * sbusOutConfig()->interval;
-    if (sbusOutPort &&
-        /* Tx Buff is free */ serialTxBytesFree(sbusOutPort) >
-            sizeof(sbusOutFrame_t) &&
-        /* Tx interval check */ currentTimeUs - sbusOutLastTxTimeUs >
-            sbusOutTxIntervalUs) {
-        sbusOutFrame_t frame;
-        uint16_t channels[SBUS_OUT_CHANNELS];
-        for (int ch = 0; ch < SBUS_OUT_CHANNELS; ch++) {
-            float value = sbusOutGetChannelValue(ch);
-            channels[ch] = sbusOutConvertToSbus(ch, value);
-        }
-        sbusOutPrepareSbusFrame(&frame, channels);
-
-        // serial output
-        serialWriteBuf(sbusOutPort, (const uint8_t *)&frame, sizeof(frame));
-        sbusOutLastTxTimeUs = currentTimeUs;
+    UNUSED(currentTimeUs);
+    if (!sbusOutPort)
+        return;
+
+    // Check TX Buff is free
+    if (serialTxBytesFree(sbusOutPort) <= sizeof(sbusOutFrame_t))
+        return;
+
+    // Start sending.
+    sbusOutFrame_t frame;
+    uint16_t channels[SBUS_OUT_CHANNELS];
+    for (int ch = 0; ch < SBUS_OUT_CHANNELS; ch++) {
+        float value = sbusOutGetChannelValue(ch);
+        channels[ch] = sbusOutConvertToSbus(ch, value);
     }
+    sbusOutPrepareSbusFrame(&frame, channels);
+
+    // serial output
+    serialWriteBuf(sbusOutPort, (const uint8_t *)&frame, sizeof(frame));
 }
 
 void sbusOutInit() {

src/main/fc/core.c

@@ -783,7 +783,6 @@ static void subTaskMotorsServosUpdate(timeUs_t currentTimeUs)
         motorUpdate();
 #endif
     }
-    sbusOutUpdate(currentTimeUs);
 }
 
 static void subTaskFilterUpdate(timeUs_t currentTimeUs)

src/main/fc/init.c

@@ -687,7 +687,10 @@ void init(void)
 #ifdef USE_SERVOS
     servoInit();
 #endif
+
+#ifdef USE_SBUS_OUTPUT
     sbusOutInit();
+#endif
 
 #ifdef USE_PINIO
     pinioInit(pinioConfig());

src/main/fc/tasks.c

@@ -44,6 +44,7 @@
 #include "drivers/stack_check.h"
 #include "drivers/usb_io.h"
 #include "drivers/vtx_common.h"
+#include "drivers/sbus_output.h"
 
 #include "config/config.h"
 #include "fc/core.h"
@@ -77,6 +78,7 @@
 
 #include "pg/rx.h"
 #include "pg/motor.h"
+#include "pg/sbus_output.h"
 
 #include "rx/rx.h"
 
@@ -414,6 +416,11 @@ task_attribute_t task_attributes[TASK_COUNT] = {
 #ifdef USE_TELEMETRY_SBUS2
     [TASK_TELEMETRY_SBUS2] = DEFINE_TASK("SBUS2_TELEMETRY", NULL, NULL, taskSendSbus2Telemetry, TASK_PERIOD_HZ(8000), TASK_PRIORITY_LOWEST),
 #endif
+
+#ifdef USE_SBUS_OUTPUT
+    // 250Hz is the initial period. The actual period will be loaded from config.
+    [TASK_SBUS_OUTPUT] = DEFINE_TASK("SBUS_OUTPUT", NULL, NULL, sbusOutUpdate, TASK_PERIOD_HZ(25), TASK_PRIORITY_MEDIUM),
+#endif
 };
 
 task_t *getTask(unsigned taskId)
@@ -568,5 +575,10 @@ void tasksInit(void)
     setTaskEnabled(TASK_TELEMETRY_SBUS2, useSBUS2);
 #endif
 
+#ifdef USE_SBUS_OUTPUT
+    rescheduleTask(TASK_SBUS_OUTPUT, TASK_PERIOD_HZ(sbusOutConfig()->sbusRate));
+    setTaskEnabled(TASK_SBUS_OUTPUT, true);
+#endif
+
 }
 

src/main/pg/sbus_output.c

@@ -15,17 +15,25 @@
  * along with this software. If not, see <https://www.gnu.org/licenses/>.
  */
 
-#include "pg/sbus_output.h"
 #include "pg/pg_ids.h"
+#include "platform.h"
+
+#include "pg/sbus_output.h"
 
-PG_REGISTER_WITH_RESET_FN(sbusOutConfig_t, sbusOutConfig, PG_DRIVER_SBUS_OUT_CONFIG, 0);
+#ifdef USE_SBUS_OUTPUT
+
+// The config struct is quite large. A ResetFn is smaller than a ResetTemplate.
+PG_REGISTER_WITH_RESET_FN(sbusOutConfig_t, sbusOutConfig,
+                          PG_DRIVER_SBUS_OUT_CONFIG, 0);
 
 void pgResetFn_sbusOutConfig(sbusOutConfig_t *config) {
-    config->interval = 10;
     for (int i = 0; i < SBUS_OUT_CHANNELS; i++) {
         config->sourceType[i] = SBUS_OUT_SOURCE_RX;
         config->sourceIndex[i] = i;
         config->min[i] = 1000;
         config->max[i] = 2000;
     }
-}
+    config->sbusRate = 50;
+}
+
+#endif

src/main/pg/sbus_output.h

@@ -17,8 +17,8 @@
 
 #pragma once
 
-#include "pg/pg.h"
 #include "common/utils.h"
+#include "pg/pg.h"
 
 #define SBUS_OUT_CHANNELS 18
 
@@ -32,24 +32,21 @@ typedef struct sbusOutConfig_s {
     uint8_t sourceType[SBUS_OUT_CHANNELS];
 
     // channel index, rule index or servo index.
-    uint8_t sourceIndex[SBUS_OUT_CHANNELS]; 
-
-    // source value maps to the min sbus value (192 for full-scale channels or 0
-    // for digital channels). Typically:
-    //   * 1000 or 988 for receiver channels
-    //   * (need verify) -1000 for mixer rule (treat as -1.0f)
-    //   * 1000 (us) for wideband servo, 500 (us) for narrowband servo
+    uint8_t sourceIndex[SBUS_OUT_CHANNELS];
+
+    // The min max define how source values map to the sbus valuex for each
+    // channel. The module maps source value [min, max] to sbus value [192,
+    // 1792] (full-scale channels) or [0, 1] (on-off channels). Typically:
+    //   * min = 1000, max = 2000 or min = 988, max = 2012 for receiver channels.
+    //   * min = -1000, max = +1000 for mixer rule (treat as -1.0f and +1.0f).
+    //   * min = 1000, max = 2000 for wideband servo, or min = 500, max = 1000
+    //   for narrowband servo.
     int16_t min[SBUS_OUT_CHANNELS];
-
-    // source value maps to the max sbus value (1792 for full-scale channels or
-    // 1 for digital channels). Typically:
-    //   * 2000 or 2012 for receiver channels
-    //   * (need verify) +1000 for mixer rule (treat as +1.0f)
-    //   * 2000 (us) for wideband servo, 1000 (us) for narrowband servo
     int16_t max[SBUS_OUT_CHANNELS];
 
-    // SBus output frame interval in ms.
-    uint8_t interval;
+    // SBus output frame rate in Hz, typically 50Hz. Your receiver may support
+    // faster updates.
+    uint8_t sbusRate;
 
 } sbusOutConfig_t;
 

src/main/scheduler/scheduler.h

@@ -174,6 +174,9 @@ typedef enum {
     TASK_TELEMETRY_SBUS2,
 #endif
 
+#ifdef USE_SBUS_OUTPUT
+    TASK_SBUS_OUTPUT,
+#endif
     /* Count of real tasks */
     TASK_COUNT,
 

src/main/target/common_pre.h

@@ -242,6 +242,7 @@ extern uint8_t _dmaram_end__;
 #define USE_SERIALRX_SBUS       // Frsky and Futaba receivers
 #define USE_SERIALRX_SPEKTRUM   // SRXL, DSM2 and DSMX protocol
 #define USE_SERIALRX_SUMD       // Graupner Hott protocol
+#define USE_SBUS_OUTPUT         // SBus Output feature
 
 #if (TARGET_FLASH_SIZE > 256)
 #define PID_PROFILE_COUNT 6

src/test/Makefile

@@ -489,6 +489,9 @@ sbus_output_unittest_INCLUDE_DIRS := \
         $(TEST_DIR) \
 		$(USER_DIR)/common 
 
+sbus_output_unittest_DEFINES := \
+        USE_SBUS_OUTPUT=
+
 # Please tweak the following variable definitions as needed by your
 # project, except GTEST_HEADERS, which you can use in your own targets
 # but shouldn't modify.

src/test/unit/sbus_output_unittest.cc

@@ -25,7 +25,6 @@ extern "C" {
 #include "pg/sbus_output.h"
 
 extern serialPort_t *sbusOutPort;
-extern timeUs_t sbusOutLastTxTimeUs;
 extern void sbusOutPrepareSbusFrame(sbusOutFrame_t *frame, uint16_t *channels);
 extern void pgResetFn_sbusOutConfig(sbusOutConfig_t *config);
 
@@ -96,10 +95,16 @@ using ::testing::Invoke;
 using ::testing::Return;
 using ::testing::StrictMock;
 
-// Start testing
-TEST(SBusOutInit, ConfigReset) {
+void pgReset(void) {
     // We will use the real config variable
+    extern const pgRegistry_t _sbusOutConfigRegistry;
     pgResetFn_sbusOutConfig(sbusOutConfigMutable());
+}
+
+// Start testing
+TEST(SBusOutInit, ConfigReset) {
+    // Reset config for testing
+    pgReset();
 
     // Source Type (All SBUS_OUT_SOURCE_RX)
     for (int i = 0; i < 18; i++) {
@@ -114,6 +119,8 @@ TEST(SBusOutInit, ConfigReset) {
         EXPECT_EQ(sbusOutConfig()->min[i], 1000);
         EXPECT_EQ(sbusOutConfig()->max[i], 2000);
     }
+
+    EXPECT_EQ(sbusOutConfig()->sbusRate, 50);
 }
 
 TEST(SBusOutInit, GoodPath) {
@@ -151,10 +158,7 @@ class SBusOutTestBase : public ::testing::Test {
   public:
     void SetUp() override {
         // Reset config for testing
-        pgResetFn_sbusOutConfig(sbusOutConfigMutable());
-
-        // Reset timer for testing
-        sbusOutLastTxTimeUs = 0;
+        pgReset();
 
         // Reset rcChannel
         memset(rcChannel, 0, sizeof(rcChannel));
@@ -433,36 +437,15 @@ TEST_F(SBusOutSerialTest, NoFreeBuffer) {
     sbusOutUpdate(1000000);
 }
 
-TEST_F(SBusOutSerialTest, TooSoon) {
-    EXPECT_CALL(mock_, serialTxBytesFree(&fake_port_)).WillOnce(Return(50));
-    // Expect no-op
-    EXPECT_CALL(mock_, serialWriteBuf).Times(0);
-
-    sbusOutUpdate(1000);
-}
-
 TEST_F(SBusOutSerialTest, GoodUpdate) {
     EXPECT_CALL(mock_, serialTxBytesFree(&fake_port_))
         .Times(2)
         .WillRepeatedly(Return(50));
     EXPECT_CALL(mock_, serialWriteBuf(&fake_port_, _, _)).Times(2);
 
-    // We should at least update every 50ms. (The default value is 10ms)
-    sbusOutUpdate(50 * 1000);
-
-    sbusOutUpdate(100 * 1000);
-}
-
-// Maybe someday our battery will last of 70min. Who knows.
-TEST_F(SBusOutSerialTest, TimeWrap) {
-    EXPECT_CALL(mock_, serialTxBytesFree(&fake_port_))
-        .Times(2)
-        .WillRepeatedly(Return(50));
-    EXPECT_CALL(mock_, serialWriteBuf(&fake_port_, _, _)).Times(2);
-
-    sbusOutUpdate(-1);
-
     sbusOutUpdate(1000000);
+
+    sbusOutUpdate(2000000);
 }
 
 TEST_F(SBusOutSerialTest, SerialFormat) {
@@ -487,5 +470,5 @@ TEST_F(SBusOutSerialTest, SerialFormat) {
                       0);
         }));
 
-    sbusOutUpdate(-1);
+    sbusOutUpdate(0);
 }