david.tahir/QuadMeUp_Crossbow
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Improved sbus handling with a possibility to abandon stuck SBUS frames

Browse Source
This commit is contained in:
Pawel Spychalski (DzikuVx)
2018-04-28 21:00:19 +02:00
parent 0f0dbf6cb3
commit 6a121df25d
3 changed files with 73 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

97
crossbow/sbus.cpp
View File

@@ -102,6 +102,43 @@ void SbusInput::restart(void)
start();
}
/*
* We prevent from frame decoding being stuck somewhere
*/
void SbusInput::recoverStuckFrames(void)
{
if (
_protocolState == SBUS_DECODING_STATE_IN_PROGRESS &&
millis() - _frameDecodingStartedAt > 6
) {
_protocolState = SBUS_DECODING_STATE_IDLE;
_serial.flush();
}
}
void sbusToChannels(volatile int16_t channels[], byte buffer[]) {
channels[0] = ((buffer[1] |buffer[2]<<8) & 0x07FF);
channels[1] = ((buffer[2]>>3 |buffer[3]<<5) & 0x07FF);
channels[2] = ((buffer[3]>>6 |buffer[4]<<2 |buffer[5]<<10) & 0x07FF);
channels[3] = ((buffer[5]>>1 |buffer[6]<<7) & 0x07FF);
channels[4] = ((buffer[6]>>4 |buffer[7]<<4) & 0x07FF);
channels[5] = ((buffer[7]>>7 |buffer[8]<<1 |buffer[9]<<9) & 0x07FF);
channels[6] = ((buffer[9]>>2 |buffer[10]<<6) & 0x07FF);
channels[7] = ((buffer[10]>>5|buffer[11]<<3) & 0x07FF);
channels[8] = ((buffer[12] |buffer[13]<<8) & 0x07FF);
channels[9] = ((buffer[13]>>3|buffer[14]<<5) & 0x07FF);
channels[10] = ((buffer[14]>>6|buffer[15]<<2|buffer[16]<<10) & 0x07FF);
channels[11] = ((buffer[16]>>1|buffer[17]<<7) & 0x07FF);
channels[12] = ((buffer[17]>>4|buffer[18]<<4) & 0x07FF);
channels[13] = ((buffer[18]>>7|buffer[19]<<1|buffer[20]<<9) & 0x07FF);
channels[14] = ((buffer[20]>>2|buffer[21]<<6) & 0x07FF);
channels[15] = ((buffer[21]>>5|buffer[22]<<3) & 0x07FF);
for (uint8_t channelIndex = 0; channelIndex < SBUS_CHANNEL_NUMBER; channelIndex++) {
channels[channelIndex] = mapSbusToChannel(channels[channelIndex]);
}
}
void SbusInput::sbusRead() {
static byte buffer[25];
static byte buffer_index = 0;
@@ -111,50 +148,36 @@ void SbusInput::sbusRead() {
while (_serial.available()) {
byte rx = _serial.read();
if (buffer_index == 0 && rx != SBUS_FRAME_HEADER) {
//incorrect start byte, out of sync
_decoderErrorFrames++;
continue;
}
if (_protocolState == SBUS_DECODING_STATE_IDLE) {
//We allow next frame to start only when previous frame eneded some time ago
if (
rx == SBUS_FRAME_HEADER &&
millis() - _frameDecodingEndedAt > 5
) {
//Header is correct
_frameDecodingStartedAt = millis();
buffer_index = 0;
_protocolState = SBUS_DECODING_STATE_IN_PROGRESS;
}
}
buffer[buffer_index] = rx;
buffer_index++;
if (buffer_index == 25) {
buffer_index = 0;
if (buffer[24] != SBUS_FRAME_FOOTER) {
//incorrect end byte, out of sync
_decoderErrorFrames++;
continue;
}
_goodFrames++;
channels[0] = ((buffer[1] |buffer[2]<<8) & 0x07FF);
channels[1] = ((buffer[2]>>3 |buffer[3]<<5) & 0x07FF);
channels[2] = ((buffer[3]>>6 |buffer[4]<<2 |buffer[5]<<10) & 0x07FF);
channels[3] = ((buffer[5]>>1 |buffer[6]<<7) & 0x07FF);
channels[4] = ((buffer[6]>>4 |buffer[7]<<4) & 0x07FF);
channels[5] = ((buffer[7]>>7 |buffer[8]<<1 |buffer[9]<<9) & 0x07FF);
channels[6] = ((buffer[9]>>2 |buffer[10]<<6) & 0x07FF);
channels[7] = ((buffer[10]>>5|buffer[11]<<3) & 0x07FF);
channels[8] = ((buffer[12] |buffer[13]<<8) & 0x07FF);
channels[9] = ((buffer[13]>>3|buffer[14]<<5) & 0x07FF);
channels[10] = ((buffer[14]>>6|buffer[15]<<2|buffer[16]<<10) & 0x07FF);
channels[11] = ((buffer[16]>>1|buffer[17]<<7) & 0x07FF);
channels[12] = ((buffer[17]>>4|buffer[18]<<4) & 0x07FF);
channels[13] = ((buffer[18]>>7|buffer[19]<<1|buffer[20]<<9) & 0x07FF);
channels[14] = ((buffer[20]>>2|buffer[21]<<6) & 0x07FF);
channels[15] = ((buffer[21]>>5|buffer[22]<<3) & 0x07FF);
for (uint8_t channelIndex = 0; channelIndex < SBUS_CHANNEL_NUMBER; channelIndex++) {
channels[channelIndex] = mapSbusToChannel(channels[channelIndex]);
}
_lastChannelReceivedAt = millis();
if (
_protocolState == SBUS_DECODING_STATE_IN_PROGRESS &&
buffer_index == 25 &&
buffer[24] == SBUS_FRAME_FOOTER
) {
//We have full frame now
_frameDecodingEndedAt = millis();
sbusToChannels(channels, buffer);
_protocolState = SBUS_DECODING_STATE_IDLE;
}
}
}
bool SbusInput::isReceiving() {
return !(millis() - _lastChannelReceivedAt > SBUS_IS_RECEIVING_THRESHOLD);
return !(millis() - _frameDecodingEndedAt > SBUS_IS_RECEIVING_THRESHOLD);
}