sbus_output: add motor as source

rotorflight · Nov 9, 2024 · 399eeb9 · 399eeb9
1 parent 5141929
commit 399eeb9
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 6 deletions.
diff --git a/src/main/drivers/sbus_output.c b/src/main/drivers/sbus_output.c
@@ -61,36 +61,46 @@ STATIC_UNIT_TESTED void sbusOutPrepareSbusFrame(sbusOutFrame_t *frame,
     frame->endByte = 0;
 }
 
-float sbusOutGetPwmRX(uint8_t channel) {
+static float sbusOutGetRX(uint8_t channel) {
     if (channel < MAX_SUPPORTED_RC_CHANNEL_COUNT)
         return rcChannel[channel];
     return 0;
 }
 
 // Note: this returns -1000 - 1000 range
-float sbusOutGetValueMixer(uint8_t channel) {
+static float sbusOutGetValueMixer(uint8_t channel) {
     if (channel < MIXER_OUTPUT_COUNT)
         return 1000.0f * mixerGetOutput(channel);
     return 0;
 }
 
-float sbusOutGetPwmServo(uint8_t channel) {
+static float sbusOutGetServo(uint8_t channel) {
     if (channel < MAX_SUPPORTED_SERVOS)
         return getServoOutput(channel);
     return 0;
 }
 
+// Note: this returns 0 - 1000 range (or -1000 - 1000 for bidirectional
+// motor)
+static float sbusOutGetMotor(uint8_t channel) {
+    if (channel < MAX_SUPPORTED_MOTORS)
+        return getMotorOutput(channel);
+    return 0;
+}
+
 STATIC_UNIT_TESTED float sbusOutGetChannelValue(uint8_t channel) {
     const sbusOutSourceType_e source_type =
         sbusOutConfig()->sourceType[channel];
     const uint8_t source_index = sbusOutConfig()->sourceIndex[channel];
     switch (source_type) {
     case SBUS_OUT_SOURCE_RX:
-        return sbusOutGetPwmRX(source_index);
+        return sbusOutGetRX(source_index);
     case SBUS_OUT_SOURCE_MIXER:
         return sbusOutGetValueMixer(source_index);
     case SBUS_OUT_SOURCE_SERVO:
-        return sbusOutGetPwmServo(source_index);
+        return sbusOutGetServo(source_index);
+    case SBUS_OUT_SOURCE_MOTOR:
+        return sbusOutGetMotor(source_index);
     }
     return 0;
 }

diff --git a/src/main/pg/sbus_output.h b/src/main/pg/sbus_output.h
@@ -25,7 +25,8 @@
 typedef enum {
     SBUS_OUT_SOURCE_RX = 0,
     SBUS_OUT_SOURCE_MIXER = 1,
-    SBUS_OUT_SOURCE_SERVO = 2
+    SBUS_OUT_SOURCE_SERVO = 2,
+    SBUS_OUT_SOURCE_MOTOR = 3
 } sbusOutSourceType_e;
 
 typedef struct sbusOutConfig_s {
@@ -40,6 +41,7 @@ typedef struct sbusOutConfig_s {
     //   * 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
+    //   * min = 0, max = 1000 for motor (treat as 0 and +1.0f)
     //   for narrowband servo.
     int16_t min[SBUS_OUT_CHANNELS];
     int16_t max[SBUS_OUT_CHANNELS];

diff --git a/src/test/unit/sbus_output_unittest.cc b/src/test/unit/sbus_output_unittest.cc
@@ -61,6 +61,7 @@ class MockInterface {
                 (serialPort_t * instance, const uint8_t *data, int count), ());
     MOCK_METHOD(float, mixerGetOutput, (uint8_t index), ());
     MOCK_METHOD(uint16_t, getServoOutput, (uint8_t index), ());
+    MOCK_METHOD(int16_t, getMotorOutput, (uint8_t index), ());
 } *g_mock = nullptr;
 
 extern "C" {
@@ -88,6 +89,7 @@ void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) {
 float rcChannel[18];
 float mixerGetOutput(uint8_t index) { return g_mock->mixerGetOutput(index); }
 uint16_t getServoOutput(uint8_t index) { return g_mock->getServoOutput(index); }
+int16_t getMotorOutput(uint8_t motor) { return g_mock->getMotorOutput(motor); }
 }
 
 using ::testing::_;
@@ -257,6 +259,23 @@ TEST_P(SBusOutSourceMapping, SourceServo) {
     }
 }
 
+TEST_P(SBusOutSourceMapping, SourceMotor) {
+    uint8_t sbus_channel = std::get<0>(GetParam());
+    uint8_t source_index = std::get<1>(GetParam());
+    sbusOutConfigMutable()->sourceType[sbus_channel] = SBUS_OUT_SOURCE_MOTOR;
+    sbusOutConfigMutable()->sourceIndex[sbus_channel] = source_index;
+
+    constexpr uint16_t kFakeValue = 14285;
+
+    if (source_index < MAX_SUPPORTED_MOTORS) {
+        EXPECT_CALL(mock_, getMotorOutput(source_index))
+            .WillOnce(Return(kFakeValue));
+        EXPECT_EQ(sbusOutGetChannelValue(sbus_channel), kFakeValue);
+    } else {
+        EXPECT_EQ(sbusOutGetChannelValue(sbus_channel), 0);
+    }
+}
+
 // Test 18x18 combinations. This will also include out of bound indexes.
 INSTANTIATE_TEST_SUITE_P(SourceMappingSweep, SBusOutSourceMapping,
                          testing::Combine(testing::Range(0, 18),