Library decoding Futaba SBUS data and control SBUS Servos. Hint! The TTL signal from the Futaba SBUS receiver / the TTL signal to the SBUS servos must be inverted. Use some 3.3V compatibel TTL logic chip such as 74LVT04 etc.
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 }
Generated on Fri Jul 15 2022 00:23:20 by 1.7.2