Skip to content

Commit

Permalink
Fixed two bugs in the SBUS parser: 1) the header byte was 0xF0 when i…
Browse files Browse the repository at this point in the history 
…t should have been 0x0F and 2) an else statement was missing so the first byte of the received packet kept ending up as the header byte. Also fixed a big where the Serial was set to 8N2, where the actual serial is 8E2, but we're using 8E1 for the SBUS reading since 8E2 is not available and the functionality will still work ignoring the second stop bit. Fixed and validated with an FrSky X8R. Also added a write function to generate SBUS packets and send to the servos.
  • Loading branch information
@flybrianfly
flybrianfly committed Jul 12, 2016
1 parent 5aa31f3 commit ababf8e
Show file tree
Hide file tree
Showing 4 changed files with 225 additions and 124 deletions.
266 changes: 159 additions & 107 deletions SBUS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
// title: SBUS.cpp
// author: Taylor, Brian R.
// email: [email protected]
// date: 2016-04-28
// date: 2016-07-12
// license:
//

Expand All @@ -11,135 +11,187 @@

/* SBUS object, input the serial bus */
SBUS::SBUS(int bus){
_bus = bus; // serial bus
_bus = bus; // serial bus
}

/* starts the serial communication */
void SBUS::begin(){

// initialize parsing state
_fpos = 0;

// select the serial port
if(_bus == 3){
_port = &Serial3;
}
else if(_bus == 2){
_port = &Serial2;
}
else{
_port = &Serial1;
}

// begin the serial port for SBUS
_port->begin(100000,SERIAL_8N2_RXINV_TXINV);
// initialize parsing state
_fpos = 0;

// select the serial port
if(_bus == 3){
_port = &Serial3;
}
else if(_bus == 2){
_port = &Serial2;
}
else{
_port = &Serial1;
}

// begin the serial port for SBUS
_port->begin(100000,SERIAL_8E1_RXINV_TXINV);
}

/* read the SBUS data and calibrate it to +/- 1 */
bool SBUS::readCal(float* calChannels, uint8_t* failsafe, int* lostFrames){
int16_t channels[16];
int16_t channels[16];

// read the SBUS data
if(read(&channels[0],failsafe,lostFrames)){
// read the SBUS data
if(read(&channels[0],failsafe,lostFrames)){

// linear calibration
for(uint8_t i = 0; i < 16; i++){
calChannels[i] = channels[i] * SBUS_SCALE + SBUS_BIAS;
}
// linear calibration
for(uint8_t i = 0; i < 16; i++){
calChannels[i] = channels[i] * SBUS_SCALE + SBUS_BIAS;
}

// return true on receiving a full packet
return true;
}
else{
// return true on receiving a full packet
return true;
}
else{

// return false if a full packet is not received
return false;
// return false if a full packet is not received
return false;
}

}

/* read the SBUS data */
bool SBUS::read(int16_t* channels, uint8_t* failsafe, int* lostFrames){

// parse the SBUS packet
if(parse()){

// 16 channels of 11 bit data
channels[0] = (int16_t) ((_payload[0] |_payload[1]<<8) & 0x07FF);
channels[1] = (int16_t) ((_payload[1]>>3 |_payload[2]<<5) & 0x07FF);
channels[2] = (int16_t) ((_payload[2]>>6 |_payload[3]<<2 |_payload[4]<<10) & 0x07FF);
channels[3] = (int16_t) ((_payload[4]>>1 |_payload[5]<<7) & 0x07FF);
channels[4] = (int16_t) ((_payload[5]>>4 |_payload[6]<<4) & 0x07FF);
channels[5] = (int16_t) ((_payload[6]>>7 |_payload[7]<<1 |_payload[8]<<9) & 0x07FF);
channels[6] = (int16_t) ((_payload[8]>>2 |_payload[9]<<6) & 0x07FF);
channels[7] = (int16_t) ((_payload[9]>>5 |_payload[10]<<3) & 0x07FF);
channels[8] = (int16_t) ((_payload[11] |_payload[12]<<8) & 0x07FF);
channels[9] = (int16_t) ((_payload[12]>>3|_payload[13]<<5) & 0x07FF);
channels[10] = (int16_t) ((_payload[13]>>6|_payload[14]<<2|_payload[15]<<10) & 0x07FF);
channels[11] = (int16_t) ((_payload[15]>>1|_payload[16]<<7) & 0x07FF);
channels[12] = (int16_t) ((_payload[16]>>4|_payload[17]<<4) & 0x07FF);
channels[13] = (int16_t) ((_payload[17]>>7|_payload[18]<<1|_payload[19]<<9) & 0x07FF);
channels[14] = (int16_t) ((_payload[19]>>2|_payload[20]<<6) & 0x07FF);
channels[15] = (int16_t) ((_payload[20]>>5|_payload[21]<<3) & 0x07FF);

// failsafe state
if ((_payload[22] >> 3) & 0x0001) {
*failsafe = 1;
} else {
*failsafe = 0;
}

// count lost frames
if ((_payload[23] >> 2) & 0x0001) {
*lostFrames = *lostFrames + 1;
}

// return true on receiving a full packet
return true;
}
else{

// return false if a full packet is not received
return false;
}
// parse the SBUS packet
if(parse()){

// 16 channels of 11 bit data
channels[0] = (int16_t) ((_payload[0] |_payload[1]<<8) & 0x07FF);
channels[1] = (int16_t) ((_payload[1]>>3 |_payload[2]<<5) & 0x07FF);
channels[2] = (int16_t) ((_payload[2]>>6 |_payload[3]<<2 |_payload[4]<<10) & 0x07FF);
channels[3] = (int16_t) ((_payload[4]>>1 |_payload[5]<<7) & 0x07FF);
channels[4] = (int16_t) ((_payload[5]>>4 |_payload[6]<<4) & 0x07FF);
channels[5] = (int16_t) ((_payload[6]>>7 |_payload[7]<<1 |_payload[8]<<9) & 0x07FF);
channels[6] = (int16_t) ((_payload[8]>>2 |_payload[9]<<6) & 0x07FF);
channels[7] = (int16_t) ((_payload[9]>>5 |_payload[10]<<3) & 0x07FF);
channels[8] = (int16_t) ((_payload[11] |_payload[12]<<8) & 0x07FF);
channels[9] = (int16_t) ((_payload[12]>>3|_payload[13]<<5) & 0x07FF);
channels[10] = (int16_t) ((_payload[13]>>6|_payload[14]<<2|_payload[15]<<10) & 0x07FF);
channels[11] = (int16_t) ((_payload[15]>>1|_payload[16]<<7) & 0x07FF);
channels[12] = (int16_t) ((_payload[16]>>4|_payload[17]<<4) & 0x07FF);
channels[13] = (int16_t) ((_payload[17]>>7|_payload[18]<<1|_payload[19]<<9) & 0x07FF);
channels[14] = (int16_t) ((_payload[19]>>2|_payload[20]<<6) & 0x07FF);
channels[15] = (int16_t) ((_payload[20]>>5|_payload[21]<<3) & 0x07FF);

// count lost frames
if (_payload[22] == SBUS_LOST_FRAME) {
*lostFrames = *lostFrames + 1;
}

// failsafe state
if (_payload[22] == SBUS_FAILSAFE) {
*failsafe = 1;
}
else{
*failsafe = 0;
}

// return true on receiving a full packet
return true;
}
else{

// return false if a full packet is not received
return false;
}
}

/* parse the SBUS data */
bool SBUS::parse(){

// see if serial data is available
while(_port->available()){
uint8_t c = _port->read();

// find the header
if(_fpos == 0){
if(c == SBUS_HEADER){
_fpos++;
}
else{
_fpos = 0;
}
}

// strip off the data
if((_fpos-1) < PAYLOAD_SIZE){
_payload[_fpos-1] = c;
_fpos++;
}

// check the end byte
if((_fpos-1) == PAYLOAD_SIZE){
if(c == SBUS_FOOTER){
_fpos = 0;
return true;
}
else{
_fpos = 0;
return false;
}
}
}
// see if serial data is available
while(_port->available() > 0){
uint8_t c = _port->read();

// find the header
if(_fpos == 0){
if(c == SBUS_HEADER){
_fpos++;
}
else{
_fpos = 0;
}
}
else{

// strip off the data
if((_fpos-1) < PAYLOAD_SIZE){
_payload[_fpos-1] = c;
_fpos++;
}

// check the end byte
if((_fpos-1) == PAYLOAD_SIZE){
if(c == SBUS_FOOTER){
_fpos = 0;
return true;
}
else{
_fpos = 0;
return false;
}
}
}
}
// return false if a partial packet
return false;
}

// return false if a partial packet
return false;
/* write SBUS packets */
void SBUS::write(int16_t* channels){
uint8_t packet[25];
elapsedMicros byteDelay = 0;
int sendIndex = 0;

/* assemble the SBUS packet */

// SBUS header
packet[0] = SBUS_HEADER;

// 16 channels of 11 bit data
packet[1] = (uint8_t) ((channels[0] & 0x07FF));
packet[2] = (uint8_t) ((channels[0] & 0x07FF)>>8 | (channels[1] & 0x07FF)<<3);
packet[3] = (uint8_t) ((channels[1] & 0x07FF)>>5 | (channels[2] & 0x07FF)<<6);
packet[4] = (uint8_t) ((channels[2] & 0x07FF)>>2);
packet[5] = (uint8_t) ((channels[2] & 0x07FF)>>10 | (channels[3] & 0x07FF)<<1);
packet[6] = (uint8_t) ((channels[3] & 0x07FF)>>7 | (channels[4] & 0x07FF)<<4);
packet[7] = (uint8_t) ((channels[4] & 0x07FF)>>4 | (channels[5] & 0x07FF)<<7);
packet[8] = (uint8_t) ((channels[5] & 0x07FF)>>1);
packet[9] = (uint8_t) ((channels[5] & 0x07FF)>>9 | (channels[6] & 0x07FF)<<2);
packet[10] = (uint8_t) ((channels[6] & 0x07FF)>>6 | (channels[7] & 0x07FF)<<5);
packet[11] = (uint8_t) ((channels[7] & 0x07FF)>>3);
packet[12] = (uint8_t) ((channels[8] & 0x07FF));
packet[13] = (uint8_t) ((channels[8] & 0x07FF)>>8 | (channels[9] & 0x07FF)<<3);
packet[14] = (uint8_t) ((channels[9] & 0x07FF)>>5 | (channels[10] & 0x07FF)<<6);
packet[15] = (uint8_t) ((channels[10] & 0x07FF)>>2);
packet[16] = (uint8_t) ((channels[10] & 0x07FF)>>10 | (channels[11] & 0x07FF)<<1);
packet[17] = (uint8_t) ((channels[11] & 0x07FF)>>7 | (channels[12] & 0x07FF)<<4);
packet[18] = (uint8_t) ((channels[12] & 0x07FF)>>4 | (channels[13] & 0x07FF)<<7);
packet[19] = (uint8_t) ((channels[13] & 0x07FF)>>1);
packet[20] = (uint8_t) ((channels[13] & 0x07FF)>>9 | (channels[14] & 0x07FF)<<2);
packet[21] = (uint8_t) ((channels[14] & 0x07FF)>>6 | (channels[15] & 0x07FF)<<5);
packet[22] = (uint8_t) ((channels[15] & 0x07FF)>>3);

// flags
packet[23] = 0x00;

// footer
packet[24] = SBUS_FOOTER;

// write packet
while(sendIndex < 25){
if(byteDelay >=122){ // theoretically, this is 120, but I'm using 122 to be safe
byteDelay = 0;
_port->write(packet[sendIndex]);
sendIndex++;
}
}
}
37 changes: 20 additions & 17 deletions SBUS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
// title: SBUS.h
// author: Taylor, Brian R.
// email: [email protected]
// date: 2016-04-28
// date: 2016-07-12
// license:
//

Expand All @@ -11,24 +11,27 @@

#include "Arduino.h"

#define SBUS_HEADER 0xF0
#define SBUS_FOOTER 0x00
#define PAYLOAD_SIZE 24
#define SBUS_SCALE 0.0012202562538133
#define SBUS_BIAS -1.20988407565589
const uint8_t SBUS_HEADER = 0x0F;
const uint8_t SBUS_FOOTER = 0x00;
const uint8_t SBUS_LOST_FRAME = 0x20;
const uint8_t SBUS_FAILSAFE = 0x10;
const int PAYLOAD_SIZE = 24;
const double SBUS_SCALE = 0.0012202562538133;
const double SBUS_BIAS = -1.20988407565589;

class SBUS{
public:
SBUS(int bus);
void begin();
bool read(int16_t* channels, uint8_t* failsafe, int* lostFrames);
bool readCal(float* calChannels, uint8_t* failsafe, int* lostFrames);
private:
int _bus;
int _fpos;
uint8_t _payload[PAYLOAD_SIZE];
HardwareSerial* _port;
bool parse();
public:
SBUS(int bus);
void begin();
bool read(int16_t* channels, uint8_t* failsafe, int* lostFrames);
bool readCal(float* calChannels, uint8_t* failsafe, int* lostFrames);
void write(int16_t* channels);
private:
int _bus;
int _fpos;
uint8_t _payload[PAYLOAD_SIZE];
HardwareSerial* _port;
bool parse();
};

#endif
Loading

0 comments on commit ababf8e

Please sign in to comment.