feat: trainer SBUS processing in IDLE IRQ (#5672)
This commit is contained in:
+41
-68
@@ -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
@@ -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);
|
||||||
|
|||||||
@@ -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:
|
||||||
|
|||||||
@@ -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
@@ -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;
|
|
||||||
}
|
|
||||||
|
|||||||
Reference in New Issue
Block a user