航空研究会 / Mbed 2 deprecated Autoflight2018_24_c

自動着陸の右旋回を修正

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of Autoflight2018_24 by 航空研究会

sbus/sbus.cpp@20:9393b0cfa44d, 2018-09-19 (annotated)

Committer:
taknokolat
Date:
Wed Sep 19 00:05:30 2018 +0000
Revision:
20:9393b0cfa44d
Parent:
0:17f575135219 
a

Who changed what in which revision?

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