Hi guys,
http://www.futaba-rc.com/sbus/index.html
I needed to use both regular servos and SBUS servos for a project I have going on right now. SBUS is an interesting thing. It uses a 100000 baud inverted UART with a 25 byte transmission. Each channel is transmitted using 11 bits of data. There is also a start an end byte. They also use 2 stop bits and even parity. Now is where the fun comes in. The signal is transmitted big endian, but each of the individual bytes are little endian. I had figured all this stuff out when I stumbled upon this gem:
http://mbed.org/users/Digixx/notebook/futaba-s-bus-controlled-by-mbed/
Not only did what they have to say confirm my reverse engineering suspicions, but they had already made a great algorithm to bit bang the transmission into the correct format. A thanks is also due to user fat16lib for his excellent serial library which allows things like changing the parity and stop bits. I modified the code the guys over at MBED had done and used a hex inverter and the results are perfect. I wrote basically none of this code and those that did allow free redistribution so go nuts. IMO the ability to use SBUS is pretty big for arduino. If I feel motivated I'll turn this into a proper library. But for now it is what it is. If you can read code, the way this thing works is pretty self explanatory:
#include <SerialPort.h>
#define SBUS_SIGNAL_OK 0x00
#define SBUS_SIGNAL_LOST 0x01
#define SBUS_SIGNAL_FAILSAFE 0x03
SerialPort<0,25,25> port0;
uint8_t sbus_data[25] = {
0x0f,0x01,0x04,0x20,0x00,0xff,0x07,0x40,0x00,0x02,0x10,0x80,0x2c,0x64,0x21,0x0b,0x59,0x08,0x40,0x00,0x02,0x10,0x80,0x00,0x00};
int16_t channels[18] = {
1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,0,0};
int16_t servos[18] = {
1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,0,0};
uint8_t failsafe_status = SBUS_SIGNAL_FAILSAFE;
int sbus_passthrough = 1;
uint8_t byte_in_sbus;
uint8_t bit_in_sbus;
uint8_t ch;
uint8_t bit_in_channel;
uint8_t bit_in_servo;
uint8_t inBuffer[25];
int bufferIndex=0;
uint8_t inData;
int toChannels = 0;
uint32_t baud = 100000;
void setup(){
//Serial.begin(100000);
port0.begin(baud, SP_2_STOP_BIT | SP_EVEN_PARITY | SP_8_BIT_CHAR);
}
void loop(){
feedLine();
if(toChannels==1){
update_channels();
update_servos();
toChannels=0;
}
//update_servos();
}
int16_t channel(uint8_t ch) {
// Read channel data
if ((ch>0)&&(ch<=16)){
return channels[ch-1];
}
else{
return 1023;
}
}
uint8_t digichannel(uint8_t ch) {
// Read digital channel data
if ((ch>0) && (ch<=2)) {
return channels[15+ch];
}
else{
return 0;
}
}
void servo(uint8_t ch, int16_t position) {
// Set servo position
if ((ch>0)&&(ch<=16)) {
if (position>2048) {
position=2048;
}
servos[ch-1] = position;
}
}
void digiservo(uint8_t ch, uint8_t position) {
// Set digital servo position
if ((ch>0) && (ch<=2)) {
if (position>1) {
position=1;
}
servos[15+ch] = position;
}
}
uint8_t failsafe(void) {
return failsafe_status;
}
void passthroughSet(int mode) {
// Set passtrough mode, if true, received channel data is send to servos
sbus_passthrough = mode;
}
int passthroughRet(void) {
// Return current passthrough mode
return sbus_passthrough;
}
void update_servos(void) {
// Send data to servos
// Passtrough mode = false >> send own servo data
// Passtrough mode = true >> send received channel data
uint8_t i;
if (sbus_passthrough==0) {
// clear received channel data
for (i=1; i<24; i++) {
sbus_data[i] = 0;
}
// reset counters
ch = 0;
bit_in_servo = 0;
byte_in_sbus = 1;
bit_in_sbus = 0;
// store servo data
for (i=0; i<176; i++) {
if (servos[ch] & (1<<bit_in_servo)) {
sbus_data[byte_in_sbus] |= (1<<bit_in_sbus);
}
bit_in_sbus++;
bit_in_servo++;
if (bit_in_sbus == 8) {
bit_in_sbus =0;
byte_in_sbus++;
}
if (bit_in_servo == 11) {
bit_in_servo =0;
ch++;
}
}
// DigiChannel 1
if (channels[16] == 1) {
sbus_data[23] |= (1<<0);
}
// DigiChannel 2
if (channels[17] == 1) {
sbus_data[23] |= (1<<1);
}
// Failsafe
if (failsafe_status == SBUS_SIGNAL_LOST) {
sbus_data[23] |= (1<<2);
}
if (failsafe_status == SBUS_SIGNAL_FAILSAFE) {
sbus_data[23] |= (1<<2);
sbus_data[23] |= (1<<3);
}
}
// send data out
//serialPort.write(sbus_data,25);
for (i=0;i<25;i++) {
port0.write(sbus_data[i]);
}
}
void update_channels(void) {
uint8_t i;
uint8_t sbus_pointer = 0;
// clear channels[]
for (i=0; i<16; i++) {
channels[i] = 0;
}
// reset counters
byte_in_sbus = 1;
bit_in_sbus = 0;
ch = 0;
bit_in_channel = 0;
// process actual sbus data
for (i=0; i<176; i++) {
if (sbus_data[byte_in_sbus] & (1<<bit_in_sbus)) {
channels[ch] |= (1<<bit_in_channel);
}
bit_in_sbus++;
bit_in_channel++;
if (bit_in_sbus == 8) {
bit_in_sbus =0;
byte_in_sbus++;
}
if (bit_in_channel == 11) {
bit_in_channel =0;
ch++;
}
}
// DigiChannel 1
if (sbus_data[23] & (1<<0)) {
channels[16] = 1;
}
else{
channels[16] = 0;
}
// DigiChannel 2
if (sbus_data[23] & (1<<1)) {
channels[17] = 1;
}
else{
channels[17] = 0;
}
// Failsafe
failsafe_status = SBUS_SIGNAL_OK;
if (sbus_data[23] & (1<<2)) {
failsafe_status = SBUS_SIGNAL_LOST;
}
if (sbus_data[23] & (1<<3)) {
failsafe_status = SBUS_SIGNAL_FAILSAFE;
}
}
void feedLine(){
while(port0.available()){
inData = port0.read();
if (inData == 0x0f){
bufferIndex = 0;
inBuffer[bufferIndex] = inData;
inBuffer[24] = 0xff;
}
else{
bufferIndex ++;
inBuffer[bufferIndex] = inData;
}
if(inBuffer[0]==0x0f & inBuffer[24] == 0x00){
memcpy(sbus_data,inBuffer,25);
toChannels = 1;
return;
}
}
}