FutabaSBUS.cpp

00001 /* mbed R/C Futaba SBUS Library
00002  * Copyright (c) 2011-2012 digixx
00003  *
00004  * Permission is hereby granted, free of charge, to any person obtaining a copy
00005  * of this software and associated documentation files (the "Software"), to deal
00006  * in the Software without restriction, including without limitation the rights
00007  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00008  * copies of the Software, and to permit persons to whom the Software is
00009  * furnished to do so, subject to the following conditions:
00010  *
00011  * The above copyright notice and this permission notice shall be included in
00012  * all copies or substantial portions of the Software.
00013  *
00014  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00015  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00016  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00017  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00018  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00019  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00020  * THE SOFTWARE.
00021  */
00022 
00023 #include "FutabaSBUS.h"
00024 #include "MODSERIAL.h"
00025 #include "mbed.h"
00026 
00027 //debug only
00028 DigitalOut tst1(p8);
00029 DigitalOut tst2(p9);
00030 DigitalOut tst3(p10);
00031 
00032 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};
00033 int16_t channels[18]  = {1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,0,0};
00034 int16_t servos[18]    = {1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,0,0};
00035 uint8_t  failsafe_status = SBUS_SIGNAL_FAILSAFE;
00036 bool sbus_passthrough = true;
00037 
00038 
00039 FutabaSBUS::FutabaSBUS(PinName tx, PinName rx) : sbus_(tx, rx) {
00040     // Set Baudrate
00041     sbus_.baud(100000);
00042     // Set Datalenght & Frame
00043     sbus_.format(8, Serial::Even, 2);
00044     // Attach interrupt routines
00045     sbus_.attach(this, &FutabaSBUS::SBUS_irq_rx, MODSERIAL::RxIrq);
00046     // init ticker 500us
00047     rxSBUS.attach_us(this, &FutabaSBUS::rx_ticker_500us,500);
00048     rx_timeout=50;
00049     tx_timeout=60;
00050 }
00051 
00052 int16_t FutabaSBUS::channel(uint8_t ch) {
00053     // Read channel data
00054     if ((ch>0)&&(ch<=16)){
00055         return channels[ch-1];
00056     }else{
00057         return 1023;
00058     }
00059 }
00060 
00061 uint8_t FutabaSBUS::digichannel(uint8_t ch) {
00062     // Read digital channel data
00063     if ((ch>0) && (ch<=2)) {
00064         return channels[15+ch];
00065     }else{
00066         return 0;
00067     }
00068 }
00069 
00070 void FutabaSBUS::servo(uint8_t ch, int16_t position) {
00071     // Set servo position
00072     if ((ch>0)&&(ch<=16)) {
00073         if (position>2048) {position=2048;}
00074         servos[ch-1] = position;
00075     }
00076 }
00077 
00078 void FutabaSBUS::digiservo(uint8_t ch, uint8_t position) {
00079     // Set digital servo position
00080     if ((ch>0) && (ch<=2)) {
00081         if (position>1) {position=1;}
00082         servos[15+ch] = position;
00083     }
00084 }
00085 
00086 uint8_t FutabaSBUS::failsafe(void) {return failsafe_status;}
00087 
00088 void FutabaSBUS::passthrough(bool mode) {
00089     // Set passtrough mode, if true, received channel data is send to servos
00090     sbus_passthrough = mode;
00091 }
00092 
00093 bool FutabaSBUS::passthrough(void) {
00094     // Return current passthrough mode
00095     return sbus_passthrough;
00096 }
00097 
00098 /****************************************************************/
00099 /****************************************************************/
00100 
00101 void FutabaSBUS::SBUS_irq_rx(MODSERIAL_IRQ_INFO *q) {
00102     rx_timeout=2;
00103     tx_timeout=4;
00104 }
00105 
00106 void FutabaSBUS::update_channels(void) {
00107     // Read all received data and calculate channel data
00108     uint8_t i;
00109     uint8_t sbus_pointer = 0;
00110     while (sbus_.readable()) {
00111         uint8_t data = sbus_.getc(); // get data from serial rx buffer
00112         switch (sbus_pointer) {
00113             case 0: // Byte 1
00114                 if (data==0x0f) {
00115                     sbus_data[sbus_pointer] = data;
00116                     sbus_pointer++;
00117                 }
00118                 break;
00119 
00120             case 24:    // Byte 25 >> if last byte == 0x00 >> convert data
00121                 if (data==0x00) {
00122                     sbus_data[sbus_pointer] = data;
00123                     // clear channels[]
00124                     for (i=0; i<16; i++) {channels[i] = 0;}
00125 
00126                     // reset counters
00127                     uint8_t byte_in_sbus = 1;
00128                     uint8_t bit_in_sbus = 0;
00129                     uint8_t ch = 0;
00130                     uint8_t bit_in_channel = 0;
00131 
00132                     // process actual sbus data
00133                     for (i=0; i<176; i++) {
00134                         if (sbus_data[byte_in_sbus] & (1<<bit_in_sbus)) {
00135                             channels[ch] |= (1<<bit_in_channel);
00136                         }
00137                         bit_in_sbus++;
00138                         bit_in_channel++;
00139 
00140                         if (bit_in_sbus == 8) {
00141                             bit_in_sbus =0;
00142                             byte_in_sbus++;
00143                         }
00144                         if (bit_in_channel == 11) {
00145                             bit_in_channel =0;
00146                             ch++;
00147                         }
00148                     }
00149                     // DigiChannel 1
00150                     if (sbus_data[23] & (1<<0)) {
00151                         channels[16] = 1;
00152                     }else{
00153                         channels[16] = 0;
00154                     }
00155                     // DigiChannel 2
00156                     if (sbus_data[23] & (1<<1)) {
00157                         channels[17] = 1;
00158                     }else{
00159                         channels[17] = 0;
00160                     }
00161                     // Failsafe
00162                     failsafe_status = SBUS_SIGNAL_OK;
00163                     if (sbus_data[23] & (1<<2)) {
00164                         failsafe_status = SBUS_SIGNAL_LOST;
00165                     }
00166                     if (sbus_data[23] & (1<<3)) {
00167                         failsafe_status = SBUS_SIGNAL_FAILSAFE;
00168                     }
00169                 }
00170                 break;
00171 
00172             default:  // collect Channel data (11bit) / Failsafe information
00173                 sbus_data[sbus_pointer] = data;
00174                 sbus_pointer++;
00175         }
00176     }
00177 }
00178 
00179 void FutabaSBUS::update_servos(void) {
00180     // Send data to servos
00181     // Passtrough mode = false >> send own servo data
00182     // Passtrough mode = true >> send received channel data
00183     uint8_t i;
00184     if (!sbus_passthrough) {
00185         // clear received channel data
00186         for (i=1; i<24; i++) {
00187             sbus_data[i] = 0;
00188         }
00189     
00190         // reset counters
00191         uint8_t ch = 0;
00192         uint8_t bit_in_servo = 0;
00193         uint8_t byte_in_sbus = 1;
00194         uint8_t bit_in_sbus = 0;
00195         
00196         // store servo data
00197         for (i=0; i<176; i++) {
00198             if (servos[ch] & (1<<bit_in_servo)) {
00199                 sbus_data[byte_in_sbus] |= (1<<bit_in_sbus);
00200             }
00201             bit_in_sbus++;
00202             bit_in_servo++;
00203 
00204             if (bit_in_sbus == 8) {
00205                 bit_in_sbus =0;
00206                 byte_in_sbus++;
00207             }
00208             if (bit_in_servo == 11) {
00209                 bit_in_servo =0;
00210                 ch++;
00211             }
00212         }
00213     
00214         // DigiChannel 1
00215         if (channels[16] == 1) {
00216             sbus_data[23] |= (1<<0);
00217         }
00218         // DigiChannel 2
00219         if (channels[17] == 1) {
00220             sbus_data[23] |= (1<<1);
00221         }        
00222         
00223         // Failsafe
00224         if (failsafe_status == SBUS_SIGNAL_LOST) {
00225             sbus_data[23] |= (1<<2);
00226         }
00227         
00228         if (failsafe_status == SBUS_SIGNAL_FAILSAFE) {
00229             sbus_data[23] |= (1<<2);
00230             sbus_data[23] |= (1<<3);
00231         }
00232     }
00233     // send data out
00234     for (i=0;i<25;i++) {
00235         sbus_.putc(sbus_data[i]);
00236     }
00237 }
00238 
00239 void FutabaSBUS::rx_ticker_500us(void) {
00240     // RX
00241     switch (rx_timeout) {
00242         case 0:
00243             break;
00244         case 1:
00245             if (sbus_.readable()) {update_channels();}
00246         default:
00247             rx_timeout--;
00248     }
00249     // TX
00250     switch (tx_timeout) {
00251         case 0:
00252             update_servos();
00253             tx_timeout = 28;
00254         default:
00255             tx_timeout--;
00256     }
00257 }