Improve SBus code. Fix SBus Failsafe detection, tested with FrSky X8R
and X4RSB. Check for end-byte to improve robustness. Less logic. Better definition of frame structure eliminates the '-1's.
This commit is contained in:
Dominic Clifton
parent
ca8f197979
commit
5401092afa
|
|
@ -35,9 +35,14 @@
|
|||
#include "rx/rx.h"
|
||||
#include "rx/sbus.h"
|
||||
|
||||
#define DEBUG_SBUS_PACKETS
|
||||
|
||||
|
||||
#define SBUS_MAX_CHANNEL 16
|
||||
#define SBUS_FRAME_SIZE 25
|
||||
#define SBUS_SYNCBYTE 0x0F
|
||||
|
||||
#define SBUS_FRAME_BEGIN_BYTE 0x0F
|
||||
#define SBUS_FRAME_END_BYTE 0x00
|
||||
|
||||
#define SBUS_BAUDRATE 100000
|
||||
|
||||
|
|
@ -48,6 +53,7 @@ static uint16_t sbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
|
|||
static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
|
||||
|
||||
static serialPort_t *sBusPort;
|
||||
static uint32_t sbusSignalLostEventCount = 0;
|
||||
|
||||
void sbusUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint)
|
||||
{
|
||||
|
|
@ -71,7 +77,14 @@ bool sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa
|
|||
return sBusPort != NULL;
|
||||
}
|
||||
|
||||
struct sbus_dat {
|
||||
#define SBUS_FLAG_RESERVED_1 (1 << 0)
|
||||
#define SBUS_FLAG_RESERVED_2 (1 << 1)
|
||||
#define SBUS_FLAG_SIGNAL_LOSS (1 << 2)
|
||||
#define SBUS_FLAG_FAILSAFE_ACTIVE (1 << 3)
|
||||
|
||||
struct sbusFrame_s {
|
||||
uint8_t syncByte;
|
||||
// 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
|
||||
unsigned int chan0 : 11;
|
||||
unsigned int chan1 : 11;
|
||||
unsigned int chan2 : 11;
|
||||
|
|
@ -88,39 +101,53 @@ struct sbus_dat {
|
|||
unsigned int chan13 : 11;
|
||||
unsigned int chan14 : 11;
|
||||
unsigned int chan15 : 11;
|
||||
uint8_t flags;
|
||||
uint8_t endByte;
|
||||
} __attribute__ ((__packed__));
|
||||
|
||||
typedef union {
|
||||
uint8_t in[SBUS_FRAME_SIZE];
|
||||
struct sbus_dat msg;
|
||||
} sbus_msg;
|
||||
uint8_t bytes[SBUS_FRAME_SIZE];
|
||||
struct sbusFrame_s frame;
|
||||
} sbusFrame_t;
|
||||
|
||||
static sbus_msg sbus;
|
||||
static sbusFrame_t sbusFrame;
|
||||
|
||||
// Receive ISR callback
|
||||
static void sbusDataReceive(uint16_t c)
|
||||
{
|
||||
uint32_t sbusTime;
|
||||
static uint32_t sbusTimeLast;
|
||||
static uint8_t sbusFramePosition;
|
||||
#ifdef DEBUG_SBUS_PACKETS
|
||||
static uint8_t sbusUnusedFrameCount = 0;
|
||||
#endif
|
||||
|
||||
sbusTime = micros();
|
||||
if ((sbusTime - sbusTimeLast) > 2500) // sbus2 fast timing
|
||||
static uint8_t sbusFramePosition = 0;
|
||||
static uint32_t sbusTimeoutAt = 0;
|
||||
uint32_t now = millis();
|
||||
|
||||
if ((int32_t)(sbusTimeoutAt - now) < 0) {
|
||||
sbusFramePosition = 0;
|
||||
sbusTimeLast = sbusTime;
|
||||
}
|
||||
sbusTimeoutAt = now + 2500;
|
||||
|
||||
if (sbusFramePosition == 0 && c != SBUS_SYNCBYTE)
|
||||
sbusFrame.bytes[sbusFramePosition] = (uint8_t)c;
|
||||
|
||||
if (sbusFramePosition == 0 && c != SBUS_FRAME_BEGIN_BYTE) {
|
||||
return;
|
||||
}
|
||||
|
||||
sbusFrameDone = false; // lazy main loop didnt fetch the stuff
|
||||
if (sbusFramePosition != 0)
|
||||
sbus.in[sbusFramePosition - 1] = (uint8_t)c;
|
||||
sbusFramePosition++;
|
||||
|
||||
if (sbusFramePosition == SBUS_FRAME_SIZE - 1) {
|
||||
sbusFrameDone = true;
|
||||
if (sbusFramePosition == SBUS_FRAME_SIZE) {
|
||||
if (sbusFrame.frame.endByte == SBUS_FRAME_END_BYTE) {
|
||||
sbusFrameDone = true;
|
||||
}
|
||||
sbusFramePosition = 0;
|
||||
} else {
|
||||
sbusFramePosition++;
|
||||
#ifdef DEBUG_SBUS_PACKETS
|
||||
if (sbusFrameDone) {
|
||||
sbusUnusedFrameCount++;
|
||||
}
|
||||
#endif
|
||||
sbusFrameDone = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -130,26 +157,32 @@ bool sbusFrameComplete(void)
|
|||
return false;
|
||||
}
|
||||
sbusFrameDone = false;
|
||||
if ((sbus.in[SBUS_FRAME_SIZE - 3] >> 3) & 0x0001) {
|
||||
|
||||
if (sbusFrame.frame.flags & SBUS_FLAG_SIGNAL_LOSS) {
|
||||
// internal failsafe enabled and rx failsafe flag set
|
||||
sbusSignalLostEventCount++;
|
||||
}
|
||||
if (sbusFrame.frame.flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
|
||||
// internal failsafe enabled and rx failsafe flag set
|
||||
return false;
|
||||
}
|
||||
sbusChannelData[0] = sbus.msg.chan0;
|
||||
sbusChannelData[1] = sbus.msg.chan1;
|
||||
sbusChannelData[2] = sbus.msg.chan2;
|
||||
sbusChannelData[3] = sbus.msg.chan3;
|
||||
sbusChannelData[4] = sbus.msg.chan4;
|
||||
sbusChannelData[5] = sbus.msg.chan5;
|
||||
sbusChannelData[6] = sbus.msg.chan6;
|
||||
sbusChannelData[7] = sbus.msg.chan7;
|
||||
sbusChannelData[8] = sbus.msg.chan8;
|
||||
sbusChannelData[9] = sbus.msg.chan9;
|
||||
sbusChannelData[10] = sbus.msg.chan10;
|
||||
sbusChannelData[11] = sbus.msg.chan11;
|
||||
sbusChannelData[12] = sbus.msg.chan12;
|
||||
sbusChannelData[13] = sbus.msg.chan13;
|
||||
sbusChannelData[14] = sbus.msg.chan14;
|
||||
sbusChannelData[15] = sbus.msg.chan15;
|
||||
|
||||
sbusChannelData[0] = sbusFrame.frame.chan0;
|
||||
sbusChannelData[1] = sbusFrame.frame.chan1;
|
||||
sbusChannelData[2] = sbusFrame.frame.chan2;
|
||||
sbusChannelData[3] = sbusFrame.frame.chan3;
|
||||
sbusChannelData[4] = sbusFrame.frame.chan4;
|
||||
sbusChannelData[5] = sbusFrame.frame.chan5;
|
||||
sbusChannelData[6] = sbusFrame.frame.chan6;
|
||||
sbusChannelData[7] = sbusFrame.frame.chan7;
|
||||
sbusChannelData[8] = sbusFrame.frame.chan8;
|
||||
sbusChannelData[9] = sbusFrame.frame.chan9;
|
||||
sbusChannelData[10] = sbusFrame.frame.chan10;
|
||||
sbusChannelData[11] = sbusFrame.frame.chan11;
|
||||
sbusChannelData[12] = sbusFrame.frame.chan12;
|
||||
sbusChannelData[13] = sbusFrame.frame.chan13;
|
||||
sbusChannelData[14] = sbusFrame.frame.chan14;
|
||||
sbusChannelData[15] = sbusFrame.frame.chan15;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue