#include "Teensy_SBUS.h"
void Teensy_SBUS::begin(uint8_t port_nr){
port = port_nr;
uint8_t loc_sbusData[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 loc_channels[18] = {
1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,0,0};
int16_t loc_servos[18] = {
1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,0,0};
switch (port) {
case 1:
port1.begin(BAUDRATE,SERIAL_8E1_RXINV_TXINV);
break;
case 2:
port2.begin(BAUDRATE,SERIAL_8E1_RXINV_TXINV);
break;
case 3:
port3.begin(BAUDRATE,SERIAL_8E1_RXINV_TXINV);
break;
}
memcpy(sbusData,loc_sbusData,25);
memcpy(channels,loc_channels,18);
memcpy(servos,loc_servos,18);
failsafe_status = SBUS_SIGNAL_OK;
sbus_passthrough = 1;
toChannels = 0;
bufferIndex=0;
feedState = 0;
}
int16_t Teensy_SBUS::Channel(uint8_t ch) {
// Read channel data
if ((ch>0)&&(ch<=16)){
return channels[ch-1];
}
else{
return 1023;
}
}
uint8_t Teensy_SBUS::DigiChannel(uint8_t ch) {
// Read digital channel data
if ((ch>0) && (ch<=2)) {
return channels[15+ch];
}
else{
return 0;
}
}
void Teensy_SBUS::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 Teensy_SBUS::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 Teensy_SBUS::Failsafe(void) {
return failsafe_status;
}
void Teensy_SBUS::PassthroughSet(int mode) {
// Set passtrough mode, if true, received channel data is send to servos
sbus_passthrough = mode;
}
int Teensy_SBUS::PassthroughRet(void) {
// Return current passthrough mode
return sbus_passthrough;
}
void Teensy_SBUS::UpdateServos(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++) {
sbusData[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)) {
sbusData[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) {
sbusData[23] |= (1<<0);
}
// DigiChannel 2
if (channels[17] == 1) {
sbusData[23] |= (1<<1);
}
// Failsafe
if (failsafe_status == SBUS_SIGNAL_LOST) {
sbusData[23] |= (1<<2);
}
if (failsafe_status == SBUS_SIGNAL_FAILSAFE) {
sbusData[23] |= (1<<2);
sbusData[23] |= (1<<3);
}
}
// send data out
//serialPort.write(sbusData,25);
elapsedMicros byteDelay1=0;
elapsedMicros byteDelay2=0;
elapsedMicros byteDelay3=0;
switch (port) {
case 1:
for (i=0;i<25;i++) {
byteDelay1 = 0;
port1.write(sbusData[i]);
while (byteDelay1 <120) {delay(1);}
// Sending one byte of data takes 11bit in 8E1 and 110us at 100000baud
// but we need one stopbit longer so we start the next sending after 120us not 110
}
break;
case 2:
for (i=0;i<25;i++) {
port2.write(sbusData[i]);
}
break;
case 3:
for (i=0;i<25;i++) {
port3.write(sbusData[i]);
}
break;
}
}
void Teensy_SBUS::UpdateChannels(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;
//this method is much slower than the other method
// process actual sbus data
for (i=0; i<176; i++) {
if (sbusData[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++;
}
}*/
channels[0] = ((sbusData[1]|sbusData[2]<< 8) & 0x07FF);
channels[1] = ((sbusData[2]>>3|sbusData[3]<<5) & 0x07FF);
channels[2] = ((sbusData[3]>>6|sbusData[4]<<2|sbusData[5]<<10) & 0x07FF);
channels[3] = ((sbusData[5]>>1|sbusData[6]<<7) & 0x07FF);
channels[4] = ((sbusData[6]>>4|sbusData[7]<<4) & 0x07FF);
channels[5] = ((sbusData[7]>>7|sbusData[8]<<1|sbusData[9]<<9) & 0x07FF);
channels[6] = ((sbusData[9]>>2|sbusData[10]<<6) & 0x07FF);
channels[7] = ((sbusData[10]>>5|sbusData[11]<<3) & 0x07FF); // & the other 8 + 2 channels if you need them
#ifdef ALL_CHANNELS
channels[8] = ((sbusData[12]|sbusData[13]<< 8) & 0x07FF);
channels[9] = ((sbusData[13]>>3|sbusData[14]<<5) & 0x07FF);
channels[10] = ((sbusData[14]>>6|sbusData[15]<<2|sbusData[16]<<10) & 0x07FF);
channels[11] = ((sbusData[16]>>1|sbusData[17]<<7) & 0x07FF);
channels[12] = ((sbusData[17]>>4|sbusData[18]<<4) & 0x07FF);
channels[13] = ((sbusData[18]>>7|sbusData[19]<<1|sbusData[20]<<9) & 0x07FF);
channels[14] = ((sbusData[20]>>2|sbusData[21]<<6) & 0x07FF);
channels[15] = ((sbusData[21]>>5|sbusData[22]<<3) & 0x07FF);
#endif
// DigiChannel 1
/*if (sbusData[23] & (1<<0)) {
channels[16] = 1;
}
else{
channels[16] = 0;
}
// DigiChannel 2
if (sbusData[23] & (1<<1)) {
channels[17] = 1;
}
else{
channels[17] = 0;
}*/
// Failsafe
failsafe_status = SBUS_SIGNAL_OK;
if (sbusData[23] & (1<<2)) {
failsafe_status = SBUS_SIGNAL_LOST;
}
if (sbusData[23] & (1<<3)) {
failsafe_status = SBUS_SIGNAL_FAILSAFE;
}
}
void Teensy_SBUS::FeedLine(void){
switch (port) {
case 1:
if (port1.available() > 24){
while(port1.available() > 0){
inData = port1.read();
switch (feedState){
case 0:
if (inData != 0x0f){
while(port1.available() > 0){//read the contents of in buffer this should resync the transmission
inData = port1.read();
}
return;
}
else{
bufferIndex = 0;
inBuffer[bufferIndex] = inData;
inBuffer[24] = 0xff;
feedState = 1;
}
break;
case 1:
bufferIndex ++;
inBuffer[bufferIndex] = inData;
if (bufferIndex < 24 && port1.available() == 0){
feedState = 0;
}
if (bufferIndex == 24){
feedState = 0;
if (inBuffer[0]==0x0f && inBuffer[24] == 0x00){
memcpy(sbusData,inBuffer,25);
toChannels = 1;
}
}
break;
}
}
}
break;
case 2:
if (port2.available() > 24){
while(port2.available() > 0){
inData = port2.read();
switch (feedState){
case 0:
if (inData != 0x0f){
while(port2.available() > 0){//read the contents of in buffer this should resync the transmission
inData = port2.read();
}
return;
}
else{
bufferIndex = 0;
inBuffer[bufferIndex] = inData;
inBuffer[24] = 0xff;
feedState = 1;
}
break;
case 1:
bufferIndex ++;
inBuffer[bufferIndex] = inData;
if (bufferIndex < 24 && port2.available() == 0){
feedState = 0;
}
if (bufferIndex == 24){
feedState = 0;
if (inBuffer[0]==0x0f && inBuffer[24] == 0x00){
memcpy(sbusData,inBuffer,25);
toChannels = 1;
}
}
break;
}
}
}
break;
case 3:
if (port3.available() > 24){
while(port3.available() > 0){
inData = port3.read();
switch (feedState){
case 0:
if (inData != 0x0f){
while(port3.available() > 0){//read the contents of in buffer this should resync the transmission
inData = port3.read();
}
return;
}
else{
bufferIndex = 0;
inBuffer[bufferIndex] = inData;
inBuffer[24] = 0xff;
feedState = 1;
}
break;
case 1:
bufferIndex ++;
inBuffer[bufferIndex] = inData;
if (bufferIndex < 24 && port3.available() == 0){
feedState = 0;
}
if (bufferIndex == 24){
feedState = 0;
if (inBuffer[0]==0x0f && inBuffer[24] == 0x00){
memcpy(sbusData,inBuffer,25);
toChannels = 1;
}
}
break;
}
}
}
break;
}
}
thx 4 the hint.