Diff: FutabaSBUS.cpp

Revision:

1:e3c92fba87f2

Parent:

0:6618cf21c95c

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