From 7594a47a2d6d1d67c23671ee8ea554bc64038108 Mon Sep 17 00:00:00 2001 From: 3djc Date: Mon, 25 Nov 2024 01:58:53 +0100 Subject: [PATCH] feat: trainer SBUS processing in IDLE IRQ (#5672) --- radio/src/sbus.cpp | 109 +++++++++++++-------------------- radio/src/sbus.h | 19 +++--- radio/src/serial.cpp | 6 +- radio/src/tasks/mixer_task.cpp | 3 - radio/src/trainer.cpp | 30 ++++----- 5 files changed, 65 insertions(+), 102 deletions(-) diff --git a/radio/src/sbus.cpp b/radio/src/sbus.cpp index d28925a8a..5a86b2353 100644 --- a/radio/src/sbus.cpp +++ b/radio/src/sbus.cpp @@ -19,54 +19,63 @@ * GNU General Public License for more details. */ -#include "edgetx.h" #include "sbus.h" + +#include "edgetx.h" #include "timers_driver.h" -#define SBUS_FRAME_GAP_DELAY_US 500 +#define SBUS_FRAME_SIZE 25 +#define SBUS_START_BYTE 0x0F +#define SBUS_END_BYTE 0x00 +#define SBUS_FLAGS_IDX 23 +#define SBUS_FRAMELOST_BIT 2 +#define SBUS_FAILSAFE_BIT 3 -#define SBUS_START_BYTE 0x0F -#define SBUS_END_BYTE 0x00 -#define SBUS_FLAGS_IDX 23 -#define SBUS_FRAMELOST_BIT 2 -#define SBUS_FAILSAFE_BIT 3 +#define SBUS_CH_BITS 11 +#define SBUS_CH_MASK ((1 << SBUS_CH_BITS) - 1) -#define SBUS_CH_BITS 11 -#define SBUS_CH_MASK ((1<copyRxBuffer || + !_sbus_drv->getBufferedBytes) + return; + + if (_sbus_drv->getBufferedBytes(_sbus_ctx) != SBUS_FRAME_SIZE) { + _sbus_drv->clearRxBuffer(_sbus_ctx); + return; } - return -1; -} + uint8_t frame[SBUS_FRAME_SIZE]; + int received = _sbus_drv->copyRxBuffer(_sbus_ctx, frame, SBUS_FRAME_SIZE); + if (received < 0) return; -static int (*sbusGetByte)(uint8_t*) = nullptr; - -void sbusSetGetByte(int (*fct)(uint8_t*)) -{ - sbusGetByte = fct; + sbusProcessFrame(trainerInput, frame, received); } // Range for pulses (ppm input) is [-512:+512] -void processSbusFrame(uint8_t * sbus, int16_t * pulses, uint32_t size) +static void sbusProcessFrame(int16_t* pulses, uint8_t* sbus, uint32_t size) { if (size != SBUS_FRAME_SIZE || sbus[0] != SBUS_START_BYTE || sbus[SBUS_FRAME_SIZE - 1] != SBUS_END_BYTE) { @@ -77,55 +86,19 @@ void processSbusFrame(uint8_t * sbus, int16_t * pulses, uint32_t size) return; // SBUS invalid frame or failsafe mode } - sbus++; // skip start byte + sbus++; // skip start byte uint32_t inputbitsavailable = 0; uint32_t inputbits = 0; - for (uint32_t i=0; i>= SBUS_CH_BITS; } trainerResetTimer(); } - -void processSbusInput() -{ - - // TODO: place this outside of the function - static uint8_t SbusIndex = 0; - static uint32_t SbusTimer; - static uint8_t SbusFrame[SBUS_FRAME_SIZE]; - - uint32_t active = 0; - - // Drain input first (if existing) - uint8_t rxchar; - auto _getByte = sbusGetByte; - while (_getByte && (_getByte(&rxchar) > 0)) { - active = 1; - if (SbusIndex > SBUS_FRAME_SIZE - 1) { - SbusIndex = SBUS_FRAME_SIZE - 1; - } - SbusFrame[SbusIndex++] = rxchar; - } - - // Data has been received - if (active) { - SbusTimer = timersGetUsTick(); - return; - } - - // Check if end-of-frame is detected - if (SbusIndex) { - if ((uint32_t)(timersGetUsTick() - SbusTimer) > SBUS_FRAME_GAP_DELAY_US) { - processSbusFrame(SbusFrame, trainerInput, SbusIndex); - SbusIndex = 0; - } - } -} diff --git a/radio/src/sbus.h b/radio/src/sbus.h index 2d0bfc945..e8089d0fc 100644 --- a/radio/src/sbus.h +++ b/radio/src/sbus.h @@ -21,18 +21,17 @@ #pragma once +#include "hal/serial_driver.h" + #define SBUS_BAUDRATE 100000 -#define SBUS_FRAME_SIZE 25 -// Setup SBUS AUX serial input -void sbusSetAuxGetByte(void* ctx, int (*fct)(void*, uint8_t*)); +// SBUS serial driver + context +void sbusSetReceiveCtx(void* ctx, const etx_serial_driver_t* drv); -// SBUS AUX serial getter: -// if set, it will fetch data from the handler set -// with sbusSetAuxGetByte() -int sbusAuxGetByte(uint8_t* byte); +// SBUS AUX idle callback +void sbusAuxFrameReceived(void* param); -// Setup general SBUS input source -void sbusSetGetByte(int (*fct)(uint8_t*)); +// Enable / disable SBUS AUX +void sbusAuxSetEnabled(bool enabled); -void processSbusInput(); +void sbusFrameReceived(void* param); diff --git a/radio/src/serial.cpp b/radio/src/serial.cpp index 605b27062..e95230a99 100644 --- a/radio/src/serial.cpp +++ b/radio/src/serial.cpp @@ -218,8 +218,10 @@ static void serialSetCallBacks(int mode, void* ctx, const etx_serial_port_t* por #endif case UART_MODE_SBUS_TRAINER: - sbusSetAuxGetByte(ctx, getByte); - // TODO: setRxCb (see MODE_LUA) + sbusSetReceiveCtx(ctx, drv); + if (drv && drv->setIdleCb) { + drv->setIdleCb(ctx, sbusAuxFrameReceived, nullptr); + } break; case UART_MODE_TELEMETRY: diff --git a/radio/src/tasks/mixer_task.cpp b/radio/src/tasks/mixer_task.cpp index d7b33a675..37a37a68a 100644 --- a/radio/src/tasks/mixer_task.cpp +++ b/radio/src/tasks/mixer_task.cpp @@ -122,9 +122,6 @@ constexpr uint8_t MIXER_MAX_PERIOD = MAX_REFRESH_RATE / 1000 /*ms*/; void execMixerFrequentActions() { - // SBUS trainer - processSbusInput(); - #if defined(IMU) gyro.wakeup(); #endif diff --git a/radio/src/trainer.cpp b/radio/src/trainer.cpp index aa850d9ec..d4865263c 100644 --- a/radio/src/trainer.cpp +++ b/radio/src/trainer.cpp @@ -103,7 +103,7 @@ void stopTrainer() break; case TRAINER_MODE_MASTER_SERIAL: - sbusSetGetByte(nullptr); + sbusAuxSetEnabled(false); break; case TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE: @@ -111,7 +111,6 @@ void stopTrainer() break; case TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE: - sbusSetGetByte(nullptr); trainer_stop_module_sbus(); break; } @@ -141,7 +140,7 @@ void checkTrainerSettings() break; case TRAINER_MODE_MASTER_SERIAL: - sbusSetGetByte(sbusAuxGetByte); + sbusAuxSetEnabled(true); break; case TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE: @@ -150,7 +149,6 @@ void checkTrainerSettings() case TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE: trainer_init_module_sbus(); - sbusSetGetByte(sbus_trainer_get_byte); break; } @@ -190,7 +188,15 @@ static void trainer_init_module_sbus() } if (sbus_trainer_mod_st) { - modulePortSetPower(EXTERNAL_MODULE,true); + // configure IDLE IRQ callback + auto drv = modulePortGetSerialDrv(sbus_trainer_mod_st->rx); + auto ctx = modulePortGetCtx(sbus_trainer_mod_st->rx); + + sbusSetReceiveCtx(ctx, drv); + drv->setIdleCb(ctx, sbusFrameReceived, nullptr); + + // switch ON + modulePortSetPower(EXTERNAL_MODULE, true); } } @@ -201,17 +207,3 @@ static void trainer_stop_module_sbus() modulePortSetPower(EXTERNAL_MODULE,false); sbus_trainer_mod_st = nullptr; } - -static int sbus_trainer_get_byte(uint8_t* data) -{ - if (!sbus_trainer_mod_st) return 0; - - auto serial_driver = modulePortGetSerialDrv(sbus_trainer_mod_st->rx); - auto ctx = modulePortGetCtx(sbus_trainer_mod_st->rx); - - if (ctx) { - return serial_driver->getByte(ctx, data); - } - - return 0; -}