my sbus
Fork of FutabaSBUS by
Diff: FutabaSBUS.cpp
- Revision:
- 1:e3c92fba87f2
- Parent:
- 0:6618cf21c95c
- Child:
- 2:07dbb77a6f1a
diff -r 6618cf21c95c -r e3c92fba87f2 FutabaSBUS.cpp --- a/FutabaSBUS.cpp Wed Dec 14 20:46:10 2011 +0000 +++ b/FutabaSBUS.cpp Thu Feb 09 21:50:00 2012 +0000 @@ -30,22 +30,27 @@ 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}; +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) { + // Set Baudrate sbus_.baud(100000); + // Set Datalenght & Frame sbus_.format(8, Serial::Even, 2); + // Attach interrupt routines sbus_.attach(this, &FutabaSBUS::SBUS_irq_rx, MODSERIAL::RxIrq); + // init ticker 500us rxSBUS.attach_us(this, &FutabaSBUS::rx_ticker_500us,500); rx_timeout=50; tx_timeout=60; } int16_t FutabaSBUS::channel(uint8_t ch) { + // Read channel data if ((ch>0)&&(ch<=16)){ return channels[ch-1]; }else{ @@ -54,6 +59,7 @@ } uint8_t FutabaSBUS::digichannel(uint8_t ch) { + // Read digital channel data if ((ch>0) && (ch<=2)) { return channels[15+ch]; }else{ @@ -62,6 +68,7 @@ } void FutabaSBUS::servo(uint8_t ch, int16_t position) { + // Set servo position if ((ch>0)&&(ch<=16)) { if (position>2048) {position=2048;} servos[ch-1] = position; @@ -69,6 +76,7 @@ } void FutabaSBUS::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; @@ -78,10 +86,12 @@ uint8_t FutabaSBUS::failsafe(void) {return failsafe_status;} void FutabaSBUS::passthrough(bool mode) { + // Set passtrough mode, if true, received channel data is send to servos sbus_passthrough = mode; } bool FutabaSBUS::passthrough(void) { + // Return current passthrough mode return sbus_passthrough; } @@ -93,65 +103,8 @@ 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) { + // Read all received data and calculate channel data uint8_t i; uint8_t sbus_pointer = 0; while (sbus_.readable()) { @@ -223,6 +176,65 @@ } } +void FutabaSBUS::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) { + // clear received 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; + + // 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 + for (i=0;i<25;i++) { + sbus_.putc(sbus_data[i]); + } +} void FutabaSBUS::rx_ticker_500us(void) { // RX @@ -243,6 +255,3 @@ tx_timeout--; } } - - -