Uwe Gartmann / FutabaSBUS

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.

Files at this revision

API Documentation at this revision

Revision 1:e3c92fba87f2, committed 2012-02-09

Comitter:
Digixx
Date:
Thu Feb 09 21:50:00 2012 +0000
Parent:
0:6618cf21c95c
Commit message:
1.0

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

--- a/FutabaSBUS.h	Wed Dec 14 20:46:10 2011 +0000
+++ b/FutabaSBUS.h	Thu Feb 09 21:50:00 2012 +0000
@@ -59,13 +59,52 @@
 
 class FutabaSBUS {
 public:
+    /** create a FutabaSBUS object connected to the specified serial pins
+    *
+    * &param pin serial tx,rx to connect to
+    */
     FutabaSBUS(PinName tx, PinName rx);
+    
+    /** Read channel(1..16), digital raw data
+    *
+    * &param raw data from receiver range from 0 to 4096, normal from 352 1696
+    */
     int16_t channel(uint8_t ch);
+    
+    /** Read digital channel(1..2), range 0..1
+    *
+    * &param range 0..1
+    */
     uint8_t digichannel(uint8_t ch);
+
+    /** Set servo position, raw data, range 200..2000?
+    *
+    * &param raw data 0..2048
+    */
     void servo(uint8_t ch, int16_t position);
+
+    /** Set digital channel, 0..1
+    *
+    * &param range 0..1
+    */
     void digiservo(uint8_t ch, uint8_t position);
+
+    /** Read failsafe condition
+    *
+    * &param 0=no failsafe 1=lost signal 3=failsafe
+    */
     uint8_t failsafe(void);
+
+    /** Set logical data passtrough - servo values are ignored, using received data
+    *
+    * &param bool
+    */
     void passthrough(bool mode);
+
+    /** Read logical data passtrough
+    *
+    * &param bool
+    */
     bool passthrough(void);
 
 private: