piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

WiP

Browse Source
This commit is contained in:
Raphael Coeffic
2022-03-20 16:22:14 +01:00
parent da59fca1bc
commit 530d395204
66 changed files with 1267 additions and 1081 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/sbus.cpp
+41 -12
View File
@@ -36,14 +36,45 @@
#define SBUS_CH_CENTER 0x3E0
static int (*_sbusAuxGetByte)(void*, uint8_t*) = nullptr;
static void* _sbusAuxGetByteCtx = nullptr;
void sbusSetAuxGetByte(void* ctx, int (*fct)(void*, uint8_t*))
{
_sbusAuxGetByte = nullptr;
_sbusAuxGetByteCtx = ctx;
_sbusAuxGetByte = fct;
}
int sbusAuxGetByte(uint8_t* byte)
{
auto _getByte = _sbusAuxGetByte;
auto _ctx = _sbusAuxGetByteCtx;
if (_getByte) {
return _getByte(_ctx, byte);
}
return -1;
}
static int (*sbusGetByte)(uint8_t*) = nullptr;
void sbusSetGetByte(int (*fct)(uint8_t*))
{
sbusGetByte = fct;
}
// Range for pulses (ppm input) is [-512:+512]
void processSbusFrame(uint8_t * sbus, int16_t * pulses, 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 (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
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
@@ -65,31 +96,29 @@ void processSbusFrame(uint8_t * sbus, int16_t * pulses, uint32_t size)
void processSbusInput()
{
#if !defined(SIMU)
uint8_t rxchar;
uint32_t active = 0;
static uint8_t SbusIndex = 0;
static uint16_t SbusTimer;
static uint8_t SbusFrame[SBUS_FRAME_SIZE];
while (sbusGetByte(&rxchar)) {
auto _getByte = sbusGetByte;
while (_getByte && (_getByte(&rxchar) > 0)) {
active = 1;
if (SbusIndex > SBUS_FRAME_SIZE-1) {
SbusIndex = SBUS_FRAME_SIZE-1;
if (SbusIndex > SBUS_FRAME_SIZE - 1) {
SbusIndex = SBUS_FRAME_SIZE - 1;
}
SbusFrame[SbusIndex++] = rxchar;
}
if (active) {
SbusTimer = getTmr2MHz();
return;
}
else {
} else {
if (SbusIndex) {
if ((uint16_t) (getTmr2MHz() - SbusTimer) > SBUS_FRAME_GAP_DELAY) {
if ((uint16_t)(getTmr2MHz() - SbusTimer) > SBUS_FRAME_GAP_DELAY) {
processSbusFrame(SbusFrame, ppmInput, SbusIndex);
SbusIndex = 0;
}
}
}
#endif
}