126 lines
3.6 KiB
C++
126 lines
3.6 KiB
C++
/*
|
|
* Copyright (C) EdgeTX
|
|
*
|
|
* Based on code named
|
|
* opentx - https://github.com/opentx/opentx
|
|
* th9x - http://code.google.com/p/th9x
|
|
* er9x - http://code.google.com/p/er9x
|
|
* gruvin9x - http://code.google.com/p/gruvin9x
|
|
*
|
|
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
|
*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License version 2 as
|
|
* published by the Free Software Foundation.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*/
|
|
|
|
#include "sbus.h"
|
|
|
|
#include "edgetx.h"
|
|
#include "timers_driver.h"
|
|
|
|
#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_CH_BITS 11
|
|
#define SBUS_CH_MASK ((1 << SBUS_CH_BITS) - 1)
|
|
|
|
#define SBUS_CH_CENTER 0x3E0
|
|
|
|
// Current SBUS trainer input source. Only ever written from the main task
|
|
// (see sbusTrainerAcquire() / sbusTrainerRelease()), only ever read from the
|
|
// USART IRQ, hence no atomics: what makes this safe is the arm/disarm order.
|
|
static const etx_serial_driver_t* _sbus_drv = nullptr;
|
|
static void* _sbus_ctx = nullptr;
|
|
|
|
static void sbusProcessFrame(int16_t* pulses, uint8_t* sbus, uint32_t size);
|
|
|
|
static void sbusFrameReceived(void*)
|
|
{
|
|
if (_sbus_drv->getBufferedBytes(_sbus_ctx) != SBUS_FRAME_SIZE) {
|
|
_sbus_drv->clearRxBuffer(_sbus_ctx);
|
|
return;
|
|
}
|
|
|
|
uint8_t frame[SBUS_FRAME_SIZE];
|
|
int received = _sbus_drv->copyRxBuffer(_sbus_ctx, frame, SBUS_FRAME_SIZE);
|
|
if (received < 0) return;
|
|
|
|
sbusProcessFrame(trainerInput, frame, received);
|
|
}
|
|
|
|
bool sbusTrainerAcquire(void* ctx, const etx_serial_driver_t* drv)
|
|
{
|
|
sbusTrainerRelease();
|
|
|
|
// sbusFrameReceived() relies on all of these being available
|
|
if (!ctx || !drv || !drv->getBufferedBytes || !drv->copyRxBuffer ||
|
|
!drv->clearRxBuffer || !drv->setIdleCb)
|
|
return false;
|
|
|
|
_sbus_ctx = ctx;
|
|
_sbus_drv = drv;
|
|
|
|
// Arm last: the IDLE IRQ may fire as soon as this returns
|
|
drv->setIdleCb(ctx, sbusFrameReceived, nullptr);
|
|
return true;
|
|
}
|
|
|
|
void sbusTrainerRelease()
|
|
{
|
|
auto drv = _sbus_drv;
|
|
auto ctx = _sbus_ctx;
|
|
if (!drv || !ctx) return;
|
|
|
|
// Disarm first: no further callback can start once this returns
|
|
drv->setIdleCb(ctx, nullptr, nullptr);
|
|
|
|
_sbus_ctx = nullptr;
|
|
_sbus_drv = nullptr;
|
|
}
|
|
|
|
void sbusTrainerReleaseCtx(void* ctx)
|
|
{
|
|
if (ctx && ctx == _sbus_ctx) sbusTrainerRelease();
|
|
}
|
|
|
|
bool sbusTrainerActive() { return _sbus_ctx != nullptr; }
|
|
|
|
// Range for pulses (ppm input) is [-512:+512]
|
|
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) {
|
|
return; // not a valid SBUS frame
|
|
}
|
|
if ((sbus[SBUS_FLAGS_IDX] & (1 << SBUS_FAILSAFE_BIT)) ||
|
|
(sbus[SBUS_FLAGS_IDX] & (1 << SBUS_FRAMELOST_BIT))) {
|
|
return; // SBUS invalid frame or failsafe mode
|
|
}
|
|
|
|
sbus++; // skip start byte
|
|
|
|
uint32_t inputbitsavailable = 0;
|
|
uint32_t inputbits = 0;
|
|
for (uint32_t i = 0; i < MAX_TRAINER_CHANNELS; i++) {
|
|
while (inputbitsavailable < SBUS_CH_BITS) {
|
|
inputbits |= *sbus++ << inputbitsavailable;
|
|
inputbitsavailable += 8;
|
|
}
|
|
*pulses++ = ((int32_t)(inputbits & SBUS_CH_MASK) - SBUS_CH_CENTER) * 5 / 8;
|
|
inputbitsavailable -= SBUS_CH_BITS;
|
|
inputbits >>= SBUS_CH_BITS;
|
|
}
|
|
|
|
trainerResetTimer();
|
|
}
|