diff --git a/src/main/drivers/sbus_output.c b/src/main/drivers/sbus_output.c index dadd3bc85..63e27c84c 100644 --- 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 index b72e09baf..5f73f499b 100644 --- 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 index cf4d50389..fd292c2c2 100644 --- 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),