feat: trainer SBUS processing in IDLE IRQ (#5672)

This commit is contained in:
3djc
2024-11-25 10:58:53 +10:00
committed by GitHub
parent b35baecfb9
commit 7594a47a2d
5 changed files with 65 additions and 102 deletions
+41 -68
View File
@@ -19,54 +19,63 @@
* GNU General Public License for more details. * GNU General Public License for more details.
*/ */
#include "edgetx.h"
#include "sbus.h" #include "sbus.h"
#include "edgetx.h"
#include "timers_driver.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_CH_BITS 11
#define SBUS_END_BYTE 0x00 #define SBUS_CH_MASK ((1 << SBUS_CH_BITS) - 1)
#define SBUS_FLAGS_IDX 23
#define SBUS_FRAMELOST_BIT 2
#define SBUS_FAILSAFE_BIT 3
#define SBUS_CH_BITS 11 #define SBUS_CH_CENTER 0x3E0
#define SBUS_CH_MASK ((1<<SBUS_CH_BITS) - 1)
#define SBUS_CH_CENTER 0x3E0 static const etx_serial_driver_t* _sbus_drv = nullptr;
static void* _sbus_ctx = nullptr;
static bool _sbus_aux_enabled = false;
static int (*_sbusAuxGetByte)(void*, uint8_t*) = nullptr; static void sbusProcessFrame(int16_t* pulses, uint8_t* sbus, uint32_t size);
static void* _sbusAuxGetByteCtx = nullptr;
void sbusSetAuxGetByte(void* ctx, int (*fct)(void*, uint8_t*)) void sbusSetReceiveCtx(void* ctx, const etx_serial_driver_t* drv)
{ {
_sbusAuxGetByte = nullptr; _sbus_ctx = ctx;
_sbusAuxGetByteCtx = ctx; _sbus_drv = drv;
_sbusAuxGetByte = fct;
} }
int sbusAuxGetByte(uint8_t* byte) void sbusAuxFrameReceived(void*)
{ {
auto _getByte = _sbusAuxGetByte; if (!_sbus_aux_enabled) return;
auto _ctx = _sbusAuxGetByteCtx; sbusFrameReceived(nullptr);
}
if (_getByte) { void sbusAuxSetEnabled(bool enabled) { _sbus_aux_enabled = enabled; }
return _getByte(_ctx, byte);
void sbusFrameReceived(void*)
{
if (!_sbus_drv || !_sbus_ctx || !_sbus_drv->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; sbusProcessFrame(trainerInput, frame, received);
void sbusSetGetByte(int (*fct)(uint8_t*))
{
sbusGetByte = fct;
} }
// Range for pulses (ppm input) is [-512:+512] // 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 || if (size != SBUS_FRAME_SIZE || sbus[0] != SBUS_START_BYTE ||
sbus[SBUS_FRAME_SIZE - 1] != SBUS_END_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 return; // SBUS invalid frame or failsafe mode
} }
sbus++; // skip start byte sbus++; // skip start byte
uint32_t inputbitsavailable = 0; uint32_t inputbitsavailable = 0;
uint32_t inputbits = 0; uint32_t inputbits = 0;
for (uint32_t i=0; i<MAX_TRAINER_CHANNELS; i++) { for (uint32_t i = 0; i < MAX_TRAINER_CHANNELS; i++) {
while (inputbitsavailable < SBUS_CH_BITS) { while (inputbitsavailable < SBUS_CH_BITS) {
inputbits |= *sbus++ << inputbitsavailable; inputbits |= *sbus++ << inputbitsavailable;
inputbitsavailable += 8; inputbitsavailable += 8;
} }
*pulses++ = ((int32_t) (inputbits & SBUS_CH_MASK) - SBUS_CH_CENTER) * 5 / 8; *pulses++ = ((int32_t)(inputbits & SBUS_CH_MASK) - SBUS_CH_CENTER) * 5 / 8;
inputbitsavailable -= SBUS_CH_BITS; inputbitsavailable -= SBUS_CH_BITS;
inputbits >>= SBUS_CH_BITS; inputbits >>= SBUS_CH_BITS;
} }
trainerResetTimer(); 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;
}
}
}
+9 -10
View File
@@ -21,18 +21,17 @@
#pragma once #pragma once
#include "hal/serial_driver.h"
#define SBUS_BAUDRATE 100000 #define SBUS_BAUDRATE 100000
#define SBUS_FRAME_SIZE 25
// Setup SBUS AUX serial input // SBUS serial driver + context
void sbusSetAuxGetByte(void* ctx, int (*fct)(void*, uint8_t*)); void sbusSetReceiveCtx(void* ctx, const etx_serial_driver_t* drv);
// SBUS AUX serial getter: // SBUS AUX idle callback
// if set, it will fetch data from the handler set void sbusAuxFrameReceived(void* param);
// with sbusSetAuxGetByte()
int sbusAuxGetByte(uint8_t* byte);
// Setup general SBUS input source // Enable / disable SBUS AUX
void sbusSetGetByte(int (*fct)(uint8_t*)); void sbusAuxSetEnabled(bool enabled);
void processSbusInput(); void sbusFrameReceived(void* param);
+4 -2
View File
@@ -218,8 +218,10 @@ static void serialSetCallBacks(int mode, void* ctx, const etx_serial_port_t* por
#endif #endif
case UART_MODE_SBUS_TRAINER: case UART_MODE_SBUS_TRAINER:
sbusSetAuxGetByte(ctx, getByte); sbusSetReceiveCtx(ctx, drv);
// TODO: setRxCb (see MODE_LUA) if (drv && drv->setIdleCb) {
drv->setIdleCb(ctx, sbusAuxFrameReceived, nullptr);
}
break; break;
case UART_MODE_TELEMETRY: case UART_MODE_TELEMETRY:
-3
View File
@@ -122,9 +122,6 @@ constexpr uint8_t MIXER_MAX_PERIOD = MAX_REFRESH_RATE / 1000 /*ms*/;
void execMixerFrequentActions() void execMixerFrequentActions()
{ {
// SBUS trainer
processSbusInput();
#if defined(IMU) #if defined(IMU)
gyro.wakeup(); gyro.wakeup();
#endif #endif
+11 -19
View File
@@ -103,7 +103,7 @@ void stopTrainer()
break; break;
case TRAINER_MODE_MASTER_SERIAL: case TRAINER_MODE_MASTER_SERIAL:
sbusSetGetByte(nullptr); sbusAuxSetEnabled(false);
break; break;
case TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE: case TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE:
@@ -111,7 +111,6 @@ void stopTrainer()
break; break;
case TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE: case TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE:
sbusSetGetByte(nullptr);
trainer_stop_module_sbus(); trainer_stop_module_sbus();
break; break;
} }
@@ -141,7 +140,7 @@ void checkTrainerSettings()
break; break;
case TRAINER_MODE_MASTER_SERIAL: case TRAINER_MODE_MASTER_SERIAL:
sbusSetGetByte(sbusAuxGetByte); sbusAuxSetEnabled(true);
break; break;
case TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE: case TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE:
@@ -150,7 +149,6 @@ void checkTrainerSettings()
case TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE: case TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE:
trainer_init_module_sbus(); trainer_init_module_sbus();
sbusSetGetByte(sbus_trainer_get_byte);
break; break;
} }
@@ -190,7 +188,15 @@ static void trainer_init_module_sbus()
} }
if (sbus_trainer_mod_st) { 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); modulePortSetPower(EXTERNAL_MODULE,false);
sbus_trainer_mod_st = nullptr; 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;
}