確認用
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_29 by
sbus/sbus.cpp
- Committer:
- HARUKIDELTA
- Date:
- 2018-09-07
- Revision:
- 0:17f575135219
File content as of revision 0:17f575135219:
#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; } }