a
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_4 by
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