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.
Revision 0:6618cf21c95c, committed 2011-12-14
- Comitter:
- Digixx
- Date:
- Wed Dec 14 20:46:10 2011 +0000
- Child:
- 1:e3c92fba87f2
- Commit message:
- beta version
Changed in this revision
FutabaSBUS.cpp | Show annotated file Show diff for this revision Revisions of this file |
FutabaSBUS.h | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FutabaSBUS.cpp Wed Dec 14 20:46:10 2011 +0000 @@ -0,0 +1,248 @@ +/* mbed R/C Futaba SBUS Library + * Copyright (c) 2011-2012 digixx + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "FutabaSBUS.h" +#include "MODSERIAL.h" +#include "mbed.h" + +//debug only +DigitalOut tst1(p8); +DigitalOut tst2(p9); +DigitalOut tst3(p10); + +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; +bool sbus_passthrough = true; + + +FutabaSBUS::FutabaSBUS(PinName tx, PinName rx) : sbus_(tx, rx) { + sbus_.baud(100000); + sbus_.format(8, Serial::Even, 2); + sbus_.attach(this, &FutabaSBUS::SBUS_irq_rx, MODSERIAL::RxIrq); + rxSBUS.attach_us(this, &FutabaSBUS::rx_ticker_500us,500); + rx_timeout=50; + tx_timeout=60; +} + +int16_t FutabaSBUS::channel(uint8_t ch) { + if ((ch>0)&&(ch<=16)){ + return channels[ch-1]; + }else{ + return 1023; + } +} + +uint8_t FutabaSBUS::digichannel(uint8_t ch) { + if ((ch>0) && (ch<=2)) { + return channels[15+ch]; + }else{ + return 0; + } +} + +void FutabaSBUS::servo(uint8_t ch, int16_t position) { + if ((ch>0)&&(ch<=16)) { + if (position>2048) {position=2048;} + servos[ch-1] = position; + } +} + +void FutabaSBUS::digiservo(uint8_t ch, uint8_t position) { + if ((ch>0) && (ch<=2)) { + if (position>1) {position=1;} + servos[15+ch] = position; + } +} + +uint8_t FutabaSBUS::failsafe(void) {return failsafe_status;} + +void FutabaSBUS::passthrough(bool mode) { + sbus_passthrough = mode; +} + +bool FutabaSBUS::passthrough(void) { + return sbus_passthrough; +} + +/****************************************************************/ +/****************************************************************/ + +void FutabaSBUS::SBUS_irq_rx(MODSERIAL_IRQ_INFO *q) { + rx_timeout=2; + tx_timeout=4; +} + +void FutabaSBUS::update_servos(void) { + + uint8_t i; + if (!sbus_passthrough) { + // clear channel data + for (i=1; i<24; i++) { + sbus_data[i] = 0; + } + + // reset counters + uint8_t ch = 0; + uint8_t bit_in_servo = 0; + uint8_t byte_in_sbus = 1; + uint8_t bit_in_sbus = 0; + + // process actual sbus 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 + for (i=0;i<25;i++) { + sbus_.putc(sbus_data[i]); + } +} + +void FutabaSBUS::update_channels(void) { + uint8_t i; + uint8_t sbus_pointer = 0; + while (sbus_.readable()) { + uint8_t data = sbus_.getc(); // get data from serial rx buffer + switch (sbus_pointer) { + case 0: // Byte 1 + if (data==0x0f) { + sbus_data[sbus_pointer] = data; + sbus_pointer++; + } + break; + + case 24: // Byte 25 >> if last byte == 0x00 >> convert data + if (data==0x00) { + sbus_data[sbus_pointer] = data; + // clear channels[] + for (i=0; i<16; i++) {channels[i] = 0;} + + // reset counters + uint8_t byte_in_sbus = 1; + uint8_t bit_in_sbus = 0; + uint8_t ch = 0; + uint8_t 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; + } + } + break; + + default: // collect Channel data (11bit) / Failsafe information + sbus_data[sbus_pointer] = data; + sbus_pointer++; + } + } +} + + +void FutabaSBUS::rx_ticker_500us(void) { + // RX + switch (rx_timeout) { + case 0: + break; + case 1: + if (sbus_.readable()) {update_channels();} + default: + rx_timeout--; + } + // TX + switch (tx_timeout) { + case 0: + update_servos(); + tx_timeout = 28; + default: + tx_timeout--; + } +} + + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FutabaSBUS.h Wed Dec 14 20:46:10 2011 +0000 @@ -0,0 +1,82 @@ +/* mbed R/C Futaba SBUS Library + * Copyright (c) 2011-2012 digixx + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + + +#ifndef MBED_FUTABA_SBUS_H +#define MBED_FUTABA_SBUS_H + +#define SBUS_SIGNAL_OK 0x00 +#define SBUS_SIGNAL_LOST 0x01 +#define SBUS_SIGNAL_FAILSAFE 0x03 + +#include "MODSERIAL.h" +#include "mbed.h" + +/** SBUS control class, based on MODSERIAL + * + * Example: + * @code + * // Continuously sweep the servo through it's full range + * #include "FutabaSBUS.h" + * #include "mbed.h" + * + * FutabaSBUS sbus(p28, p27); + * + * int main() { + * sbus.passthrough(false); + * while(1) { + * for(int i=0; i<100; i++) { + * sbus.servo(1) = i/100.0; + * wait(0.01); + * } + * for(int i=100; i>0; i--) { + * sbus.servo(1) = i/100.0; + * wait(0.01); + * } + * } + * } + * @endcode + */ + +class FutabaSBUS { +public: + FutabaSBUS(PinName tx, PinName rx); + int16_t channel(uint8_t ch); + uint8_t digichannel(uint8_t ch); + void servo(uint8_t ch, int16_t position); + void digiservo(uint8_t ch, uint8_t position); + uint8_t failsafe(void); + void passthrough(bool mode); + bool passthrough(void); + +private: + MODSERIAL sbus_; + Ticker rxSBUS; + void SBUS_irq_rx(MODSERIAL_IRQ_INFO *q); + void rx_ticker_500us(void); + void update_channels(void); + void update_servos(void); + volatile int rx_timeout; + volatile int tx_timeout; +}; + +#endif \ No newline at end of file