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

5
crossbow/crossbow.ino
View File

@@ -342,8 +342,13 @@ void loop(void)
*/ */
#ifdef DEVICE_MODE_TX #ifdef DEVICE_MODE_TX
txInput.recoverStuckFrames();
static uint32_t serialRestartMillis = 0; static uint32_t serialRestartMillis = 0;
/*
* Final guard for SBUS input. If there is no input, try to restart serial port
*/
if (!txInput.isReceiving() && serialRestartMillis + 100 < currentMillis) { if (!txInput.isReceiving() && serialRestartMillis + 100 < currentMillis) {
txInput.restart(); txInput.restart();
serialRestartMillis = currentMillis; serialRestartMillis = currentMillis;

77
crossbow/sbus.cpp
View File

@@ -102,33 +102,21 @@ void SbusInput::restart(void)
start(); start();
} }
void SbusInput::sbusRead() { /*
static byte buffer[25]; * We prevent from frame decoding being stuck somewhere
static byte buffer_index = 0; */
void SbusInput::recoverStuckFrames(void)
static uint32_t _decoderErrorFrames; {
static uint32_t _goodFrames; if (
_protocolState == SBUS_DECODING_STATE_IN_PROGRESS &&
while (_serial.available()) { millis() - _frameDecodingStartedAt > 6
byte rx = _serial.read(); ) {
if (buffer_index == 0 && rx != SBUS_FRAME_HEADER) { _protocolState = SBUS_DECODING_STATE_IDLE;
//incorrect start byte, out of sync _serial.flush();
_decoderErrorFrames++;
continue;
} }
}
buffer[buffer_index] = rx; void sbusToChannels(volatile int16_t channels[], byte buffer[]) {
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[0] = ((buffer[1] |buffer[2]<<8) & 0x07FF);
channels[1] = ((buffer[2]>>3 |buffer[3]<<5) & 0x07FF); channels[1] = ((buffer[2]>>3 |buffer[3]<<5) & 0x07FF);
channels[2] = ((buffer[3]>>6 |buffer[4]<<2 |buffer[5]<<10) & 0x07FF); channels[2] = ((buffer[3]>>6 |buffer[4]<<2 |buffer[5]<<10) & 0x07FF);
@@ -149,12 +137,47 @@ void SbusInput::sbusRead() {
for (uint8_t channelIndex = 0; channelIndex < SBUS_CHANNEL_NUMBER; channelIndex++) { for (uint8_t channelIndex = 0; channelIndex < SBUS_CHANNEL_NUMBER; channelIndex++) {
channels[channelIndex] = mapSbusToChannel(channels[channelIndex]); channels[channelIndex] = mapSbusToChannel(channels[channelIndex]);
} }
}
_lastChannelReceivedAt = millis(); void SbusInput::sbusRead() {
static byte buffer[25];
static byte buffer_index = 0;
static uint32_t _decoderErrorFrames;
static uint32_t _goodFrames;
while (_serial.available()) {
byte rx = _serial.read();
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 (
_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() { bool SbusInput::isReceiving() {
return !(millis() - _lastChannelReceivedAt > SBUS_IS_RECEIVING_THRESHOLD); return !(millis() - _frameDecodingEndedAt > SBUS_IS_RECEIVING_THRESHOLD);
} }

9
crossbow/sbus.h
View File

@@ -5,6 +5,10 @@
#include "Arduino.h" #include "Arduino.h"
#include "tx_input.h" #include "tx_input.h"
enum sbusProtocolStates {
SBUS_DECODING_STATE_IDLE,
SBUS_DECODING_STATE_IN_PROGRESS
};
class SbusInput : public TxInput class SbusInput : public TxInput
{ {
@@ -14,9 +18,12 @@ class SbusInput : public TxInput
void restart(void); void restart(void);
void loop(void); void loop(void);
bool isReceiving(void); bool isReceiving(void);
void recoverStuckFrames(void);
private: private:
HardwareSerial &_serial; HardwareSerial &_serial;
uint32_t _lastChannelReceivedAt = 0; uint32_t _frameDecodingStartedAt = 0;
uint32_t _frameDecodingEndedAt = 0 ;
uint8_t _protocolState = SBUS_DECODING_STATE_IDLE;
void sbusRead(void); void sbusRead(void);
}; };