sbus.cpp

00001 #include "mbed.h"
00002 #include "sbus.h"
00003 
00004 
00005 SBUS::SBUS(PinName tx, PinName rx)
00006     :
00007     rawserial_p(new RawSerial(tx,rx)),
00008     rawserial(*rawserial_p)
00009 {} 
00010 
00011 SBUS::~SBUS()
00012 {
00013     if(rawserial_p != NULL) delete rawserial_p;
00014 }
00015 
00016 void SBUS::initialize(void){
00017     flg_ch_update = false;
00018     failsafe_status = SBUS_SIGNAL_FAILSAFE;
00019     cnt_sbus = 0;
00020     lastfunc = NULL;
00021   for (uint8_t i = 0; i < 25; i++)  buf_sbus[i] = 0;
00022 
00023   rawserial.baud(100000);
00024   rawserial.format(8, Serial::Even, 2);
00025 }
00026 
00027 void SBUS::startInterrupt(void){
00028   rawserial.attach(this, &SBUS::func_interrupt, RawSerial::RxIrq);  //fromSBUStoPWMの前に＆が必要?
00029 }
00030 
00031 //SBUS::stopInterrupt(void){
00032 //}
00033 
00034 void SBUS::setLastfuncPoint(void (*func_p)(void)){
00035     lastfunc = func_p;
00036 }
00037 
00038 //シリアル割り込みで実行する関数
00039 void SBUS::func_interrupt(void){
00040     if(catchSerial()){
00041         unpackSerialdata();
00042         inputPwm();
00043         if(lastfunc != NULL)    (*lastfunc)();  //setLastfuncPointで設定した関数を最後に実行
00044     }
00045     return;
00046 }
00047 
00048 int8_t SBUS::catchSerial(void){
00049     buf_sbus[cnt_sbus] = rawserial.getc();
00050   if(buf_sbus[0] == 0x0f)   cnt_sbus ++;
00051   if(cnt_sbus >=25){  
00052     if(buf_sbus[24] == 0x00){
00053       return(1);     //calculatePwm実行
00054     }else{
00055       cnt_sbus = 0;
00056     }
00057   }
00058     return(0);
00059 }
00060 
00061 void SBUS::unpackSerialdata(void){
00062 /*  for (uint8_t i=0; i<25; i++){
00063         pc.printf("%x ", buf_sbus[i]);
00064         }pc.printf("\n"); 
00065 */
00066     // clear channels[]
00067     for (uint8_t i=0; i<18; i++) {channels[i] = 0;}
00068 
00069     // reset counters
00070     byte_in_sbus = 1;
00071     bit_in_sbus = 0;
00072     ch = 0;
00073     bit_in_channel = 0;
00074 
00075     // process actual sbus data
00076     for (uint8_t i=0; i<176; i++) {
00077         if (buf_sbus[byte_in_sbus] & (1<<bit_in_sbus)) {
00078             channels[ch] |= (1<<bit_in_channel);
00079         }
00080         bit_in_sbus++;
00081         bit_in_channel++;
00082 
00083         if (bit_in_sbus == 8) {
00084             bit_in_sbus =0;
00085             byte_in_sbus++;
00086         }
00087         if (bit_in_channel == 11) {
00088             bit_in_channel =0;
00089             ch++;
00090         }
00091     }
00092     // DigiChannel 1
00093     if (buf_sbus[23] & (1<<0)) {
00094         channels[16] = 1;
00095     }else{
00096         channels[16] = 0;
00097     }
00098     // DigiChannel 2
00099     if (buf_sbus[23] & (1<<1)) {
00100         channels[17] = 1;
00101     }else{
00102         channels[17] = 0;
00103     }
00104     // Failsafe
00105     failsafe_status = SBUS_SIGNAL_OK;
00106     if (buf_sbus[23] & (1<<2)) {
00107         failsafe_status = SBUS_SIGNAL_LOST;
00108     }
00109     if (buf_sbus[23] & (1<<3)) {
00110         failsafe_status = SBUS_SIGNAL_FAILSAFE;
00111     }
00112     //channels[i] = channels[i]>>1;
00113     for (uint8_t i=0; i<25; i++) {buf_sbus[i] = 0;}
00114     cnt_sbus = 0;
00115     flg_ch_update = true;
00116     
00117     return;
00118 }
00119 
00120 void SBUS::inputPwm(void){
00121   for(uint8_t i=0; i<8; i++){
00122     manualpwm[i] = (channels[i]>>1) + 1000;
00123   }
00124 }