Diff: sbus/sbus.cpp

Revision:

0:17f575135219

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sbus/sbus.cpp	Fri Sep 07 04:11:48 2018 +0000
@@ -0,0 +1,124 @@
+#include "mbed.h"
+#include "sbus.h"
+
+
+SBUS::SBUS(PinName tx, PinName rx)
+    :
+    rawserial_p(new RawSerial(tx,rx)),
+    rawserial(*rawserial_p)
+{} 
+
+SBUS::~SBUS()
+{
+    if(rawserial_p != NULL) delete rawserial_p;
+}
+
+void SBUS::initialize(void){
+    flg_ch_update = false;
+    failsafe_status = SBUS_SIGNAL_FAILSAFE;
+    cnt_sbus = 0;
+    lastfunc = NULL;
+  for (uint8_t i = 0; i < 25; i++)  buf_sbus[i] = 0;
+
+  rawserial.baud(100000);
+  rawserial.format(8, Serial::Even, 2);
+}
+
+void SBUS::startInterrupt(void){
+  rawserial.attach(this, &SBUS::func_interrupt, RawSerial::RxIrq);  //fromSBUStoPWMの前に＆が必要?
+}
+
+//SBUS::stopInterrupt(void){
+//}
+
+void SBUS::setLastfuncPoint(void (*func_p)(void)){
+    lastfunc = func_p;
+}
+
+//シリアル割り込みで実行する関数
+void SBUS::func_interrupt(void){
+    if(catchSerial()){
+        unpackSerialdata();
+        inputPwm();
+        if(lastfunc != NULL)    (*lastfunc)();  //setLastfuncPointで設定した関数を最後に実行
+    }
+    return;
+}
+
+int8_t SBUS::catchSerial(void){
+    buf_sbus[cnt_sbus] = rawserial.getc();
+  if(buf_sbus[0] == 0x0f)   cnt_sbus ++;
+  if(cnt_sbus >=25){  
+    if(buf_sbus[24] == 0x00){
+      return(1);     //calculatePwm実行
+    }else{
+      cnt_sbus = 0;
+    }
+  }
+    return(0);
+}
+
+void SBUS::unpackSerialdata(void){
+/*  for (uint8_t i=0; i<25; i++){
+        pc.printf("%x ", buf_sbus[i]);
+        }pc.printf("\n"); 
+*/
+    // clear channels[]
+    for (uint8_t i=0; i<18; i++) {channels[i] = 0;}
+
+    // reset counters
+    byte_in_sbus = 1;
+    bit_in_sbus = 0;
+    ch = 0;
+    bit_in_channel = 0;
+
+    // process actual sbus data
+    for (uint8_t i=0; i<176; i++) {
+        if (buf_sbus[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 (buf_sbus[23] & (1<<0)) {
+        channels[16] = 1;
+    }else{
+        channels[16] = 0;
+    }
+    // DigiChannel 2
+    if (buf_sbus[23] & (1<<1)) {
+        channels[17] = 1;
+    }else{
+        channels[17] = 0;
+    }
+    // Failsafe
+    failsafe_status = SBUS_SIGNAL_OK;
+    if (buf_sbus[23] & (1<<2)) {
+        failsafe_status = SBUS_SIGNAL_LOST;
+    }
+    if (buf_sbus[23] & (1<<3)) {
+        failsafe_status = SBUS_SIGNAL_FAILSAFE;
+    }
+    //channels[i] = channels[i]>>1;
+    for (uint8_t i=0; i<25; i++) {buf_sbus[i] = 0;}
+    cnt_sbus = 0;
+    flg_ch_update = true;
+    
+    return;
+}
+
+void SBUS::inputPwm(void){
+  for(uint8_t i=0; i<8; i++){
+    manualpwm[i] = (channels[i]>>1) + 1000;
+  }
+}
\ No newline at end of file