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