Diff: FutabaSBUS.cpp

Revision:

0:6618cf21c95c

Child:

1:e3c92fba87f2

--- /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--;
+    }
+}
+
+
+