航空研究会 / Mbed 2 deprecated AutoFlight2017_now_copy

s

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of AutoFlight2017_now_copy by Bot Furukawa

sbus/sbus.cpp@0:92024886c0be, 2017-08-01 (annotated)

Committer:
TUATBM
Date:
Tue Aug 01 12:27:13 2017 +0000
Revision:
0:92024886c0be
??????????????????????; ????

Who changed what in which revision?

UserRevisionLine numberNew 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 }