diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 33bae9afaf..0905d7e934 100644 --- a/src/main/cli/settings.c +++ b/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 }; diff --git a/src/main/drivers/sbus_output.c b/src/main/drivers/sbus_output.c index 12b12caf90..dadd3bc853 100644 --- a/src/main/drivers/sbus_output.c +++ b/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() { diff --git a/src/main/fc/core.c b/src/main/fc/core.c index dc460de8a3..5297ffd7ee 100644 --- a/src/main/fc/core.c +++ b/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) diff --git a/src/main/fc/init.c b/src/main/fc/init.c index e5b3c36ac0..ae1cd14972 100644 --- a/src/main/fc/init.c +++ b/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()); diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 0478b67bd8..ed8b27cfd8 100644 --- a/src/main/fc/tasks.c +++ b/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 + } diff --git a/src/main/pg/sbus_output.c b/src/main/pg/sbus_output.c index af04433499..a36cccb656 100644 --- a/src/main/pg/sbus_output.c +++ b/src/main/pg/sbus_output.c @@ -15,17 +15,25 @@ * along with this software. If not, see . */ -#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; } -} \ No newline at end of file + config->sbusRate = 50; +} + +#endif \ No newline at end of file diff --git a/src/main/pg/sbus_output.h b/src/main/pg/sbus_output.h index bfc6fc0c96..b72e09baf8 100644 --- a/src/main/pg/sbus_output.h +++ b/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; diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 898c2abc06..9b8fd59167 100644 --- a/src/main/scheduler/scheduler.h +++ b/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, diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index ef4565820f..aae993c347 100644 --- a/src/main/target/common_pre.h +++ b/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 diff --git a/src/test/Makefile b/src/test/Makefile index 724d38b6bb..987c18f239 100644 --- a/src/test/Makefile +++ b/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. diff --git a/src/test/unit/sbus_output_unittest.cc b/src/test/unit/sbus_output_unittest.cc index ecf1f9bc6c..cf4d503898 100644 --- a/src/test/unit/sbus_output_unittest.cc +++ b/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); } \ No newline at end of file