a
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_4 by
sbus/sbus.cpp@0:17f575135219, 2018-09-07 (annotated)
- Committer:
- HARUKIDELTA
- Date:
- Fri Sep 07 04:11:48 2018 +0000
- Revision:
- 0:17f575135219
SBUS_READ
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
HARUKIDELTA | 0:17f575135219 | 1 | #include "mbed.h" |
HARUKIDELTA | 0:17f575135219 | 2 | #include "sbus.h" |
HARUKIDELTA | 0:17f575135219 | 3 | |
HARUKIDELTA | 0:17f575135219 | 4 | |
HARUKIDELTA | 0:17f575135219 | 5 | SBUS::SBUS(PinName tx, PinName rx) |
HARUKIDELTA | 0:17f575135219 | 6 | : |
HARUKIDELTA | 0:17f575135219 | 7 | rawserial_p(new RawSerial(tx,rx)), |
HARUKIDELTA | 0:17f575135219 | 8 | rawserial(*rawserial_p) |
HARUKIDELTA | 0:17f575135219 | 9 | {} |
HARUKIDELTA | 0:17f575135219 | 10 | |
HARUKIDELTA | 0:17f575135219 | 11 | SBUS::~SBUS() |
HARUKIDELTA | 0:17f575135219 | 12 | { |
HARUKIDELTA | 0:17f575135219 | 13 | if(rawserial_p != NULL) delete rawserial_p; |
HARUKIDELTA | 0:17f575135219 | 14 | } |
HARUKIDELTA | 0:17f575135219 | 15 | |
HARUKIDELTA | 0:17f575135219 | 16 | void SBUS::initialize(void){ |
HARUKIDELTA | 0:17f575135219 | 17 | flg_ch_update = false; |
HARUKIDELTA | 0:17f575135219 | 18 | failsafe_status = SBUS_SIGNAL_FAILSAFE; |
HARUKIDELTA | 0:17f575135219 | 19 | cnt_sbus = 0; |
HARUKIDELTA | 0:17f575135219 | 20 | lastfunc = NULL; |
HARUKIDELTA | 0:17f575135219 | 21 | for (uint8_t i = 0; i < 25; i++) buf_sbus[i] = 0; |
HARUKIDELTA | 0:17f575135219 | 22 | |
HARUKIDELTA | 0:17f575135219 | 23 | rawserial.baud(100000); |
HARUKIDELTA | 0:17f575135219 | 24 | rawserial.format(8, Serial::Even, 2); |
HARUKIDELTA | 0:17f575135219 | 25 | } |
HARUKIDELTA | 0:17f575135219 | 26 | |
HARUKIDELTA | 0:17f575135219 | 27 | void SBUS::startInterrupt(void){ |
HARUKIDELTA | 0:17f575135219 | 28 | rawserial.attach(this, &SBUS::func_interrupt, RawSerial::RxIrq); //fromSBUStoPWMの前に&が必要? |
HARUKIDELTA | 0:17f575135219 | 29 | } |
HARUKIDELTA | 0:17f575135219 | 30 | |
HARUKIDELTA | 0:17f575135219 | 31 | //SBUS::stopInterrupt(void){ |
HARUKIDELTA | 0:17f575135219 | 32 | //} |
HARUKIDELTA | 0:17f575135219 | 33 | |
HARUKIDELTA | 0:17f575135219 | 34 | void SBUS::setLastfuncPoint(void (*func_p)(void)){ |
HARUKIDELTA | 0:17f575135219 | 35 | lastfunc = func_p; |
HARUKIDELTA | 0:17f575135219 | 36 | } |
HARUKIDELTA | 0:17f575135219 | 37 | |
HARUKIDELTA | 0:17f575135219 | 38 | //シリアル割り込みで実行する関数 |
HARUKIDELTA | 0:17f575135219 | 39 | void SBUS::func_interrupt(void){ |
HARUKIDELTA | 0:17f575135219 | 40 | if(catchSerial()){ |
HARUKIDELTA | 0:17f575135219 | 41 | unpackSerialdata(); |
HARUKIDELTA | 0:17f575135219 | 42 | inputPwm(); |
HARUKIDELTA | 0:17f575135219 | 43 | if(lastfunc != NULL) (*lastfunc)(); //setLastfuncPointで設定した関数を最後に実行 |
HARUKIDELTA | 0:17f575135219 | 44 | } |
HARUKIDELTA | 0:17f575135219 | 45 | return; |
HARUKIDELTA | 0:17f575135219 | 46 | } |
HARUKIDELTA | 0:17f575135219 | 47 | |
HARUKIDELTA | 0:17f575135219 | 48 | int8_t SBUS::catchSerial(void){ |
HARUKIDELTA | 0:17f575135219 | 49 | buf_sbus[cnt_sbus] = rawserial.getc(); |
HARUKIDELTA | 0:17f575135219 | 50 | if(buf_sbus[0] == 0x0f) cnt_sbus ++; |
HARUKIDELTA | 0:17f575135219 | 51 | if(cnt_sbus >=25){ |
HARUKIDELTA | 0:17f575135219 | 52 | if(buf_sbus[24] == 0x00){ |
HARUKIDELTA | 0:17f575135219 | 53 | return(1); //calculatePwm実行 |
HARUKIDELTA | 0:17f575135219 | 54 | }else{ |
HARUKIDELTA | 0:17f575135219 | 55 | cnt_sbus = 0; |
HARUKIDELTA | 0:17f575135219 | 56 | } |
HARUKIDELTA | 0:17f575135219 | 57 | } |
HARUKIDELTA | 0:17f575135219 | 58 | return(0); |
HARUKIDELTA | 0:17f575135219 | 59 | } |
HARUKIDELTA | 0:17f575135219 | 60 | |
HARUKIDELTA | 0:17f575135219 | 61 | void SBUS::unpackSerialdata(void){ |
HARUKIDELTA | 0:17f575135219 | 62 | /* for (uint8_t i=0; i<25; i++){ |
HARUKIDELTA | 0:17f575135219 | 63 | pc.printf("%x ", buf_sbus[i]); |
HARUKIDELTA | 0:17f575135219 | 64 | }pc.printf("\n"); |
HARUKIDELTA | 0:17f575135219 | 65 | */ |
HARUKIDELTA | 0:17f575135219 | 66 | // clear channels[] |
HARUKIDELTA | 0:17f575135219 | 67 | for (uint8_t i=0; i<18; i++) {channels[i] = 0;} |
HARUKIDELTA | 0:17f575135219 | 68 | |
HARUKIDELTA | 0:17f575135219 | 69 | // reset counters |
HARUKIDELTA | 0:17f575135219 | 70 | byte_in_sbus = 1; |
HARUKIDELTA | 0:17f575135219 | 71 | bit_in_sbus = 0; |
HARUKIDELTA | 0:17f575135219 | 72 | ch = 0; |
HARUKIDELTA | 0:17f575135219 | 73 | bit_in_channel = 0; |
HARUKIDELTA | 0:17f575135219 | 74 | |
HARUKIDELTA | 0:17f575135219 | 75 | // process actual sbus data |
HARUKIDELTA | 0:17f575135219 | 76 | for (uint8_t i=0; i<176; i++) { |
HARUKIDELTA | 0:17f575135219 | 77 | if (buf_sbus[byte_in_sbus] & (1<<bit_in_sbus)) { |
HARUKIDELTA | 0:17f575135219 | 78 | channels[ch] |= (1<<bit_in_channel); |
HARUKIDELTA | 0:17f575135219 | 79 | } |
HARUKIDELTA | 0:17f575135219 | 80 | bit_in_sbus++; |
HARUKIDELTA | 0:17f575135219 | 81 | bit_in_channel++; |
HARUKIDELTA | 0:17f575135219 | 82 | |
HARUKIDELTA | 0:17f575135219 | 83 | if (bit_in_sbus == 8) { |
HARUKIDELTA | 0:17f575135219 | 84 | bit_in_sbus =0; |
HARUKIDELTA | 0:17f575135219 | 85 | byte_in_sbus++; |
HARUKIDELTA | 0:17f575135219 | 86 | } |
HARUKIDELTA | 0:17f575135219 | 87 | if (bit_in_channel == 11) { |
HARUKIDELTA | 0:17f575135219 | 88 | bit_in_channel =0; |
HARUKIDELTA | 0:17f575135219 | 89 | ch++; |
HARUKIDELTA | 0:17f575135219 | 90 | } |
HARUKIDELTA | 0:17f575135219 | 91 | } |
HARUKIDELTA | 0:17f575135219 | 92 | // DigiChannel 1 |
HARUKIDELTA | 0:17f575135219 | 93 | if (buf_sbus[23] & (1<<0)) { |
HARUKIDELTA | 0:17f575135219 | 94 | channels[16] = 1; |
HARUKIDELTA | 0:17f575135219 | 95 | }else{ |
HARUKIDELTA | 0:17f575135219 | 96 | channels[16] = 0; |
HARUKIDELTA | 0:17f575135219 | 97 | } |
HARUKIDELTA | 0:17f575135219 | 98 | // DigiChannel 2 |
HARUKIDELTA | 0:17f575135219 | 99 | if (buf_sbus[23] & (1<<1)) { |
HARUKIDELTA | 0:17f575135219 | 100 | channels[17] = 1; |
HARUKIDELTA | 0:17f575135219 | 101 | }else{ |
HARUKIDELTA | 0:17f575135219 | 102 | channels[17] = 0; |
HARUKIDELTA | 0:17f575135219 | 103 | } |
HARUKIDELTA | 0:17f575135219 | 104 | // Failsafe |
HARUKIDELTA | 0:17f575135219 | 105 | failsafe_status = SBUS_SIGNAL_OK; |
HARUKIDELTA | 0:17f575135219 | 106 | if (buf_sbus[23] & (1<<2)) { |
HARUKIDELTA | 0:17f575135219 | 107 | failsafe_status = SBUS_SIGNAL_LOST; |
HARUKIDELTA | 0:17f575135219 | 108 | } |
HARUKIDELTA | 0:17f575135219 | 109 | if (buf_sbus[23] & (1<<3)) { |
HARUKIDELTA | 0:17f575135219 | 110 | failsafe_status = SBUS_SIGNAL_FAILSAFE; |
HARUKIDELTA | 0:17f575135219 | 111 | } |
HARUKIDELTA | 0:17f575135219 | 112 | //channels[i] = channels[i]>>1; |
HARUKIDELTA | 0:17f575135219 | 113 | for (uint8_t i=0; i<25; i++) {buf_sbus[i] = 0;} |
HARUKIDELTA | 0:17f575135219 | 114 | cnt_sbus = 0; |
HARUKIDELTA | 0:17f575135219 | 115 | flg_ch_update = true; |
HARUKIDELTA | 0:17f575135219 | 116 | |
HARUKIDELTA | 0:17f575135219 | 117 | return; |
HARUKIDELTA | 0:17f575135219 | 118 | } |
HARUKIDELTA | 0:17f575135219 | 119 | |
HARUKIDELTA | 0:17f575135219 | 120 | void SBUS::inputPwm(void){ |
HARUKIDELTA | 0:17f575135219 | 121 | for(uint8_t i=0; i<8; i++){ |
HARUKIDELTA | 0:17f575135219 | 122 | manualpwm[i] = (channels[i]>>1) + 1000; |
HARUKIDELTA | 0:17f575135219 | 123 | } |
HARUKIDELTA | 0:17f575135219 | 124 | } |