航空研究会 / Mbed 2 deprecated sbus_test_2018

a

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of AutoFlight2018_Control by 航空研究会

sbus/sbus.cpp@1:09a162f4f6ce, 2018-09-07 (annotated)

Committer:
TUATBM
Date:
Fri Sep 07 03:42:49 2018 +0000
Revision:
1:09a162f4f6ce
Parent:
0:813f5cd20cf1 
a

Who changed what in which revision?

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