勇輝 関本
/
Estrela_v10
4項目一応成功
Fork of Estrela_v01 by
main.cpp@6:13bb74a57a33, 2016-08-21 (annotated)
- Committer:
- motoseki
- Date:
- Sun Aug 21 09:13:30 2016 +0000
- Revision:
- 6:13bb74a57a33
- Parent:
- 5:c53ca479e96c
- Child:
- 7:13d98c4067c4
??????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TUATBM | 0:4013a9855dc8 | 1 | #include "mbed.h" |
TUATBM | 0:4013a9855dc8 | 2 | //#include "rtos.h" |
TUATBM | 0:4013a9855dc8 | 3 | #include "MPU9250.h" |
TUATBM | 0:4013a9855dc8 | 4 | #include "SkipperSv2.h" |
TUATBM | 0:4013a9855dc8 | 5 | #include "Estrela.h" |
TUATBM | 0:4013a9855dc8 | 6 | //#include "stm32f4xx_hal_iwdg.h" |
TUATBM | 0:4013a9855dc8 | 7 | |
TUATBM | 0:4013a9855dc8 | 8 | |
TUATBM | 0:4013a9855dc8 | 9 | #define UPD_MANUAL 0 |
TUATBM | 0:4013a9855dc8 | 10 | #define UPD_AUTO 1 |
motoseki | 6:13bb74a57a33 | 11 | #define UPD_LANDING 2 |
TUATBM | 0:4013a9855dc8 | 12 | |
TUATBM | 0:4013a9855dc8 | 13 | #define SBUS_SIGNAL_OK 0x00 |
TUATBM | 0:4013a9855dc8 | 14 | #define SBUS_SIGNAL_LOST 0x01 |
TUATBM | 0:4013a9855dc8 | 15 | #define SBUS_SIGNAL_FAILSAFE 0x03 |
TUATBM | 0:4013a9855dc8 | 16 | |
TUATBM | 0:4013a9855dc8 | 17 | //Serial pc(TX, RX); |
TUATBM | 0:4013a9855dc8 | 18 | RawSerial sbus_(PA_9, PA_10); //SBUS |
TUATBM | 0:4013a9855dc8 | 19 | |
TUATBM | 0:4013a9855dc8 | 20 | PwmOut servo1(PC_6); // TIM3_CH1 |
TUATBM | 0:4013a9855dc8 | 21 | PwmOut servo2(PC_7); // TIM3_CH2 //PC_7 |
TUATBM | 0:4013a9855dc8 | 22 | PwmOut servo3(PB_0); // TIM3_CH3 |
TUATBM | 0:4013a9855dc8 | 23 | PwmOut servo4(PB_1); // TIM3_CH4 |
TUATBM | 0:4013a9855dc8 | 24 | PwmOut servo5(PB_6); // TIM4_CH1 |
TUATBM | 0:4013a9855dc8 | 25 | PwmOut servo6(PB_7); // TIM4_CH2 |
TUATBM | 0:4013a9855dc8 | 26 | PwmOut servo7(PB_8); // TIM4_CH3 //PB_8 |
TUATBM | 0:4013a9855dc8 | 27 | PwmOut servo8(PB_9); // TIM4_CH4 |
TUATBM | 0:4013a9855dc8 | 28 | |
TUATBM | 0:4013a9855dc8 | 29 | RawSerial pc(PA_2, PA_3); |
TUATBM | 0:4013a9855dc8 | 30 | |
TUATBM | 0:4013a9855dc8 | 31 | DigitalOut led1(PA_0); //緑 |
TUATBM | 0:4013a9855dc8 | 32 | DigitalOut led2(PA_1); //赤 |
TUATBM | 0:4013a9855dc8 | 33 | DigitalOut led3(PB_4); |
TUATBM | 0:4013a9855dc8 | 34 | DigitalOut led4(PB_5); |
TUATBM | 0:4013a9855dc8 | 35 | |
TUATBM | 0:4013a9855dc8 | 36 | volatile uint8_t buf_sbus[25]; |
TUATBM | 0:4013a9855dc8 | 37 | volatile int cnt_sbus = 0; |
TUATBM | 0:4013a9855dc8 | 38 | static int16_t channels[18]; |
TUATBM | 0:4013a9855dc8 | 39 | uint8_t failsafe_status = SBUS_SIGNAL_FAILSAFE; |
TUATBM | 0:4013a9855dc8 | 40 | static bool flg_ch_update = false; |
TUATBM | 3:8c01b4f33389 | 41 | uint8_t i,j,k; |
TUATBM | 0:4013a9855dc8 | 42 | uint8_t update_mode = 0; |
TUATBM | 0:4013a9855dc8 | 43 | static uint16_t pwm[8]; |
motoseki | 6:13bb74a57a33 | 44 | static uint16_t oldTHL; |
TUATBM | 0:4013a9855dc8 | 45 | |
TUATBM | 0:4013a9855dc8 | 46 | //9軸センサー関連 |
TUATBM | 0:4013a9855dc8 | 47 | float sum = 0; |
TUATBM | 0:4013a9855dc8 | 48 | uint32_t sumCount = 0; |
TUATBM | 0:4013a9855dc8 | 49 | MPU9250 mpu9250; |
TUATBM | 0:4013a9855dc8 | 50 | Timer t; |
TUATBM | 0:4013a9855dc8 | 51 | |
TUATBM | 0:4013a9855dc8 | 52 | /* |
TUATBM | 0:4013a9855dc8 | 53 | float g_BefGyro[3] = {0,0,0}; |
TUATBM | 0:4013a9855dc8 | 54 | double g_HpfGyro[3] = {0,0,0}; |
TUATBM | 0:4013a9855dc8 | 55 | double g_HpfGyro1[3] = {0,0,0}; |
TUATBM | 0:4013a9855dc8 | 56 | double g_LpfAccel[3] = {0,0,0}; |
TUATBM | 0:4013a9855dc8 | 57 | */ |
TUATBM | 0:4013a9855dc8 | 58 | |
TUATBM | 0:4013a9855dc8 | 59 | static int jj = 0; |
TUATBM | 0:4013a9855dc8 | 60 | |
TUATBM | 0:4013a9855dc8 | 61 | //IWDG_HandleTypeDef hiwdg; |
TUATBM | 0:4013a9855dc8 | 62 | |
TUATBM | 0:4013a9855dc8 | 63 | void disp_clock(){ |
TUATBM | 0:4013a9855dc8 | 64 | pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000); |
TUATBM | 0:4013a9855dc8 | 65 | pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000); |
TUATBM | 0:4013a9855dc8 | 66 | pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000); |
TUATBM | 0:4013a9855dc8 | 67 | pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000); |
TUATBM | 0:4013a9855dc8 | 68 | pc.printf("\r\n"); |
TUATBM | 0:4013a9855dc8 | 69 | } |
TUATBM | 0:4013a9855dc8 | 70 | |
TUATBM | 0:4013a9855dc8 | 71 | //サーボ初期化関数 |
TUATBM | 0:4013a9855dc8 | 72 | void init_Servo() |
TUATBM | 0:4013a9855dc8 | 73 | { |
TUATBM | 0:4013a9855dc8 | 74 | servo1.period_ms(14); |
TUATBM | 0:4013a9855dc8 | 75 | servo1.pulsewidth_us(1500); |
TUATBM | 0:4013a9855dc8 | 76 | |
TUATBM | 0:4013a9855dc8 | 77 | servo2.period_ms(14); |
TUATBM | 0:4013a9855dc8 | 78 | servo2.pulsewidth_us(1500); |
TUATBM | 0:4013a9855dc8 | 79 | |
TUATBM | 0:4013a9855dc8 | 80 | servo3.period_ms(14); |
TUATBM | 0:4013a9855dc8 | 81 | servo3.pulsewidth_us(1000); |
TUATBM | 0:4013a9855dc8 | 82 | |
TUATBM | 0:4013a9855dc8 | 83 | servo4.period_ms(14); |
TUATBM | 0:4013a9855dc8 | 84 | servo4.pulsewidth_us(1500); |
TUATBM | 0:4013a9855dc8 | 85 | |
TUATBM | 0:4013a9855dc8 | 86 | servo5.period_ms(14); |
TUATBM | 0:4013a9855dc8 | 87 | servo5.pulsewidth_us(1500); |
TUATBM | 0:4013a9855dc8 | 88 | |
TUATBM | 0:4013a9855dc8 | 89 | servo6.period_ms(14); |
TUATBM | 0:4013a9855dc8 | 90 | servo6.pulsewidth_us(1500); |
TUATBM | 0:4013a9855dc8 | 91 | |
TUATBM | 0:4013a9855dc8 | 92 | servo7.period_ms(14); |
TUATBM | 0:4013a9855dc8 | 93 | servo7.pulsewidth_us(1500); |
TUATBM | 0:4013a9855dc8 | 94 | |
TUATBM | 0:4013a9855dc8 | 95 | servo8.period_ms(14); |
TUATBM | 0:4013a9855dc8 | 96 | servo8.pulsewidth_us(1500); |
TUATBM | 0:4013a9855dc8 | 97 | |
TUATBM | 0:4013a9855dc8 | 98 | for (j=0; j<8; j++) {pwm[j] = 1500;} |
TUATBM | 0:4013a9855dc8 | 99 | pc.printf("servo initialized\r\n"); |
TUATBM | 0:4013a9855dc8 | 100 | } |
TUATBM | 0:4013a9855dc8 | 101 | |
TUATBM | 0:4013a9855dc8 | 102 | //---setall_servo--- |
TUATBM | 0:4013a9855dc8 | 103 | //pwmをサーボに出力する関数。 |
TUATBM | 0:4013a9855dc8 | 104 | void setall_servo() |
TUATBM | 0:4013a9855dc8 | 105 | { |
TUATBM | 1:fc5988ebfa53 | 106 | |
TUATBM | 1:fc5988ebfa53 | 107 | |
TUATBM | 0:4013a9855dc8 | 108 | //if(failsafe_status == SBUS_SIGNAL_OK){ |
TUATBM | 0:4013a9855dc8 | 109 | servo1.pulsewidth_us(pwm[0]); |
TUATBM | 0:4013a9855dc8 | 110 | servo2.pulsewidth_us(pwm[1]); |
TUATBM | 0:4013a9855dc8 | 111 | servo3.pulsewidth_us(pwm[2]); |
TUATBM | 0:4013a9855dc8 | 112 | servo4.pulsewidth_us(pwm[3]); |
TUATBM | 0:4013a9855dc8 | 113 | servo5.pulsewidth_us(pwm[4]); |
TUATBM | 0:4013a9855dc8 | 114 | servo6.pulsewidth_us(pwm[5]); |
TUATBM | 0:4013a9855dc8 | 115 | servo7.pulsewidth_us(pwm[6]); |
TUATBM | 1:fc5988ebfa53 | 116 | servo8.pulsewidth_us(pwm[7]); |
TUATBM | 0:4013a9855dc8 | 117 | /* }else{ |
TUATBM | 0:4013a9855dc8 | 118 | servo1.pulsewidth_us(1000); |
TUATBM | 0:4013a9855dc8 | 119 | servo2.pulsewidth_us(1000); |
TUATBM | 0:4013a9855dc8 | 120 | servo3.pulsewidth_us(1000); |
TUATBM | 0:4013a9855dc8 | 121 | servo4.pulsewidth_us(1000); |
TUATBM | 0:4013a9855dc8 | 122 | servo5.pulsewidth_us(1000); |
TUATBM | 0:4013a9855dc8 | 123 | servo6.pulsewidth_us(1000); |
TUATBM | 0:4013a9855dc8 | 124 | servo7.pulsewidth_us(1000); |
TUATBM | 0:4013a9855dc8 | 125 | servo8.pulsewidth_us(1000); |
TUATBM | 0:4013a9855dc8 | 126 | }*/ |
TUATBM | 1:fc5988ebfa53 | 127 | /* |
TUATBM | 0:4013a9855dc8 | 128 | for(j=0;j<8;j++){ |
motoseki | 4:ba610b05751d | 129 | pc.printf("ch%d=%d ", j+1, pwm[j]); |
TUATBM | 0:4013a9855dc8 | 130 | } |
TUATBM | 0:4013a9855dc8 | 131 | //pc.printf("time = %d", frq.read_ms()); |
TUATBM | 0:4013a9855dc8 | 132 | pc.printf("\r\n"); |
TUATBM | 1:fc5988ebfa53 | 133 | */ |
TUATBM | 0:4013a9855dc8 | 134 | //frq.reset(); |
TUATBM | 0:4013a9855dc8 | 135 | } |
TUATBM | 0:4013a9855dc8 | 136 | |
TUATBM | 0:4013a9855dc8 | 137 | //---update_pwm--- |
TUATBM | 0:4013a9855dc8 | 138 | //udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード) |
TUATBM | 0:4013a9855dc8 | 139 | //各stabiGylo関数で算出したpwmを各変数に代入する(自動モード) |
TUATBM | 0:4013a9855dc8 | 140 | void update_pwm() |
TUATBM | 0:4013a9855dc8 | 141 | { |
TUATBM | 0:4013a9855dc8 | 142 | if(flg_ch_update == true){ |
motoseki | 6:13bb74a57a33 | 143 | switch(update_mode){ //マニュアルモード,自動モード,自動着陸もモードを切替 |
TUATBM | 0:4013a9855dc8 | 144 | case UPD_MANUAL: //マニュアルモード |
TUATBM | 0:4013a9855dc8 | 145 | for(j=0;j<8;j++){ |
TUATBM | 0:4013a9855dc8 | 146 | pwm[j] = (channels[j]>>1) + 1000; |
TUATBM | 0:4013a9855dc8 | 147 | } |
motoseki | 6:13bb74a57a33 | 148 | oldTHL = pwm[2]; |
TUATBM | 0:4013a9855dc8 | 149 | jj=0; |
motoseki | 4:ba610b05751d | 150 | //pc.printf("update_manual\r\n"); |
TUATBM | 0:4013a9855dc8 | 151 | break; |
TUATBM | 0:4013a9855dc8 | 152 | |
TUATBM | 0:4013a9855dc8 | 153 | case UPD_AUTO: //自動モード |
TUATBM | 0:4013a9855dc8 | 154 | pwm[0] = (channels[0]>>1) + 1000; |
TUATBM | 0:4013a9855dc8 | 155 | pwm[1] = Auto_ELE; |
TUATBM | 0:4013a9855dc8 | 156 | pwm[2] = Auto_THR; |
TUATBM | 0:4013a9855dc8 | 157 | pwm[3] = Auto_RUD; |
TUATBM | 0:4013a9855dc8 | 158 | pwm[4] = (channels[4]>>1) + 1000; |
TUATBM | 0:4013a9855dc8 | 159 | pwm[5] = (channels[5]>>1) + 1000; |
TUATBM | 0:4013a9855dc8 | 160 | pwm[6] = (channels[6]>>1) + 1000; |
TUATBM | 0:4013a9855dc8 | 161 | pwm[7] = (channels[7]>>1) + 1000; |
TUATBM | 0:4013a9855dc8 | 162 | |
TUATBM | 0:4013a9855dc8 | 163 | //pc.printf("update_auto\r\n"); |
TUATBM | 0:4013a9855dc8 | 164 | break; |
TUATBM | 0:4013a9855dc8 | 165 | |
motoseki | 6:13bb74a57a33 | 166 | case UPD_LANDING: //自動着陸モード |
motoseki | 6:13bb74a57a33 | 167 | pwm[0] = (channels[0]>>1) + 1000; |
motoseki | 6:13bb74a57a33 | 168 | pwm[1] = Auto_ELE; |
motoseki | 6:13bb74a57a33 | 169 | pwm[2] = (channels[2]>>1) + 1000; |
motoseki | 6:13bb74a57a33 | 170 | pwm[3] = (channels[3]>>1) + 1000; |
motoseki | 6:13bb74a57a33 | 171 | pwm[4] = (channels[4]>>1) + 1000; |
motoseki | 6:13bb74a57a33 | 172 | pwm[5] = (channels[5]>>1) + 1000; |
motoseki | 6:13bb74a57a33 | 173 | pwm[6] = (channels[6]>>1) + 1000; |
motoseki | 6:13bb74a57a33 | 174 | pwm[7] = (channels[7]>>1) + 1000; |
motoseki | 6:13bb74a57a33 | 175 | |
TUATBM | 0:4013a9855dc8 | 176 | default: //デフォはマニュアルモード |
TUATBM | 0:4013a9855dc8 | 177 | for(j=0;j<8;j++){ |
TUATBM | 0:4013a9855dc8 | 178 | pwm[j] = (channels[j]>>1) + 1000; |
TUATBM | 0:4013a9855dc8 | 179 | } |
TUATBM | 0:4013a9855dc8 | 180 | |
TUATBM | 0:4013a9855dc8 | 181 | jj=0; |
TUATBM | 0:4013a9855dc8 | 182 | //pc.printf("update_manual\r\n"); |
TUATBM | 0:4013a9855dc8 | 183 | break; |
TUATBM | 0:4013a9855dc8 | 184 | } |
TUATBM | 0:4013a9855dc8 | 185 | } |
motoseki | 6:13bb74a57a33 | 186 | flg_ch_update = false; |
motoseki | 6:13bb74a57a33 | 187 | ///oldTHL = pwm[2]; |
TUATBM | 0:4013a9855dc8 | 188 | setall_servo(); |
TUATBM | 0:4013a9855dc8 | 189 | } |
TUATBM | 0:4013a9855dc8 | 190 | |
TUATBM | 0:4013a9855dc8 | 191 | /*void update_auto() |
TUATBM | 0:4013a9855dc8 | 192 | { |
TUATBM | 0:4013a9855dc8 | 193 | pwm[0] = (channels[0]>>1) + 1000; |
TUATBM | 0:4013a9855dc8 | 194 | pwm[1] = Auto_ELE; |
TUATBM | 0:4013a9855dc8 | 195 | pwm[2] = Auto_THR; |
TUATBM | 0:4013a9855dc8 | 196 | pwm[3] = Auto_RUD; |
TUATBM | 0:4013a9855dc8 | 197 | pwm[4] = (channels[4]>>1) + 1000; |
TUATBM | 0:4013a9855dc8 | 198 | pwm[5] = (channels[5]>>1) + 1000; |
TUATBM | 0:4013a9855dc8 | 199 | pwm[6] = (channels[6]>>1) + 1000; |
TUATBM | 0:4013a9855dc8 | 200 | pwm[7] = (channels[7]>>1) + 1000; |
TUATBM | 0:4013a9855dc8 | 201 | |
TUATBM | 0:4013a9855dc8 | 202 | setall_servo(); |
TUATBM | 0:4013a9855dc8 | 203 | //pc.printf("update_auto\r\n"); |
TUATBM | 0:4013a9855dc8 | 204 | }*/ |
TUATBM | 0:4013a9855dc8 | 205 | |
TUATBM | 0:4013a9855dc8 | 206 | //---update_Input--- |
TUATBM | 0:4013a9855dc8 | 207 | //catch_sbusで取得したSbusの生データから各チャンネルのpwmを抽出する。 |
TUATBM | 0:4013a9855dc8 | 208 | void update_Input() |
TUATBM | 0:4013a9855dc8 | 209 | { /* for (i=0; i<25; i++){ |
TUATBM | 0:4013a9855dc8 | 210 | pc.printf("%x ", buf_sbus[i]); |
TUATBM | 0:4013a9855dc8 | 211 | }pc.printf("\n"); |
TUATBM | 0:4013a9855dc8 | 212 | */ |
TUATBM | 0:4013a9855dc8 | 213 | // clear channels[] |
TUATBM | 0:4013a9855dc8 | 214 | for (i=0; i<18; i++) {channels[i] = 0;} |
TUATBM | 0:4013a9855dc8 | 215 | |
TUATBM | 0:4013a9855dc8 | 216 | // reset counters |
TUATBM | 0:4013a9855dc8 | 217 | uint8_t byte_in_sbus = 1; |
TUATBM | 0:4013a9855dc8 | 218 | uint8_t bit_in_sbus = 0; |
TUATBM | 0:4013a9855dc8 | 219 | uint8_t ch = 0; |
TUATBM | 0:4013a9855dc8 | 220 | uint8_t bit_in_channel = 0; |
TUATBM | 0:4013a9855dc8 | 221 | |
TUATBM | 0:4013a9855dc8 | 222 | // process actual sbus data |
TUATBM | 0:4013a9855dc8 | 223 | for (i=0; i<176; i++) { |
TUATBM | 0:4013a9855dc8 | 224 | if (buf_sbus[byte_in_sbus] & (1<<bit_in_sbus)) { |
TUATBM | 0:4013a9855dc8 | 225 | channels[ch] |= (1<<bit_in_channel); |
TUATBM | 0:4013a9855dc8 | 226 | } |
TUATBM | 0:4013a9855dc8 | 227 | bit_in_sbus++; |
TUATBM | 0:4013a9855dc8 | 228 | bit_in_channel++; |
TUATBM | 0:4013a9855dc8 | 229 | |
TUATBM | 0:4013a9855dc8 | 230 | if (bit_in_sbus == 8) { |
TUATBM | 0:4013a9855dc8 | 231 | bit_in_sbus =0; |
TUATBM | 0:4013a9855dc8 | 232 | byte_in_sbus++; |
TUATBM | 0:4013a9855dc8 | 233 | } |
TUATBM | 0:4013a9855dc8 | 234 | if (bit_in_channel == 11) { |
TUATBM | 0:4013a9855dc8 | 235 | bit_in_channel =0; |
TUATBM | 0:4013a9855dc8 | 236 | ch++; |
TUATBM | 0:4013a9855dc8 | 237 | } |
TUATBM | 0:4013a9855dc8 | 238 | } |
TUATBM | 0:4013a9855dc8 | 239 | // DigiChannel 1 |
TUATBM | 0:4013a9855dc8 | 240 | if (buf_sbus[23] & (1<<0)) { |
TUATBM | 0:4013a9855dc8 | 241 | channels[16] = 1; |
TUATBM | 0:4013a9855dc8 | 242 | }else{ |
TUATBM | 0:4013a9855dc8 | 243 | channels[16] = 0; |
TUATBM | 0:4013a9855dc8 | 244 | } |
TUATBM | 0:4013a9855dc8 | 245 | // DigiChannel 2 |
TUATBM | 0:4013a9855dc8 | 246 | if (buf_sbus[23] & (1<<1)) { |
TUATBM | 0:4013a9855dc8 | 247 | channels[17] = 1; |
TUATBM | 0:4013a9855dc8 | 248 | }else{ |
TUATBM | 0:4013a9855dc8 | 249 | channels[17] = 0; |
TUATBM | 0:4013a9855dc8 | 250 | } |
TUATBM | 0:4013a9855dc8 | 251 | // Failsafe |
TUATBM | 0:4013a9855dc8 | 252 | failsafe_status = SBUS_SIGNAL_OK; |
TUATBM | 0:4013a9855dc8 | 253 | if (buf_sbus[23] & (1<<2)) { |
TUATBM | 0:4013a9855dc8 | 254 | failsafe_status = SBUS_SIGNAL_LOST; |
TUATBM | 0:4013a9855dc8 | 255 | } |
TUATBM | 0:4013a9855dc8 | 256 | if (buf_sbus[23] & (1<<3)) { |
TUATBM | 0:4013a9855dc8 | 257 | failsafe_status = SBUS_SIGNAL_FAILSAFE; |
TUATBM | 0:4013a9855dc8 | 258 | } |
TUATBM | 0:4013a9855dc8 | 259 | //channels[i] = channels[i]>>1; |
TUATBM | 0:4013a9855dc8 | 260 | for (i=0; i<25; i++) {buf_sbus[i] = 0;} |
TUATBM | 0:4013a9855dc8 | 261 | cnt_sbus = 0; |
TUATBM | 0:4013a9855dc8 | 262 | flg_ch_update = true; |
TUATBM | 0:4013a9855dc8 | 263 | |
TUATBM | 0:4013a9855dc8 | 264 | update_pwm(); |
TUATBM | 0:4013a9855dc8 | 265 | |
TUATBM | 0:4013a9855dc8 | 266 | |
TUATBM | 0:4013a9855dc8 | 267 | } |
TUATBM | 0:4013a9855dc8 | 268 | |
TUATBM | 0:4013a9855dc8 | 269 | //---catch_sbus--- |
TUATBM | 0:4013a9855dc8 | 270 | //受信機から送られてくるSBus信号(シリアル)を取得する関数 |
TUATBM | 0:4013a9855dc8 | 271 | void catch_sbus() |
TUATBM | 0:4013a9855dc8 | 272 | { |
TUATBM | 0:4013a9855dc8 | 273 | buf_sbus[cnt_sbus] = sbus_.getc(); |
TUATBM | 0:4013a9855dc8 | 274 | if(buf_sbus[0] == 0x0f) |
TUATBM | 0:4013a9855dc8 | 275 | { |
TUATBM | 0:4013a9855dc8 | 276 | cnt_sbus ++; |
TUATBM | 0:4013a9855dc8 | 277 | } |
TUATBM | 0:4013a9855dc8 | 278 | if(cnt_sbus >=25){ |
TUATBM | 0:4013a9855dc8 | 279 | if(buf_sbus[24] == 0x00){ |
TUATBM | 0:4013a9855dc8 | 280 | update_Input(); //update_Input実行 |
TUATBM | 0:4013a9855dc8 | 281 | }else{ |
TUATBM | 0:4013a9855dc8 | 282 | cnt_sbus = 0; |
TUATBM | 0:4013a9855dc8 | 283 | } |
TUATBM | 0:4013a9855dc8 | 284 | } |
TUATBM | 0:4013a9855dc8 | 285 | return; |
TUATBM | 0:4013a9855dc8 | 286 | } |
TUATBM | 0:4013a9855dc8 | 287 | |
TUATBM | 0:4013a9855dc8 | 288 | //---init_sbus--- |
TUATBM | 0:4013a9855dc8 | 289 | //Sbusを初期化する関数 |
TUATBM | 0:4013a9855dc8 | 290 | void init_sbus(){ |
TUATBM | 0:4013a9855dc8 | 291 | // Set Baudrate |
TUATBM | 0:4013a9855dc8 | 292 | sbus_.baud(100000); |
TUATBM | 0:4013a9855dc8 | 293 | // Set Datalenght & Frame |
TUATBM | 0:4013a9855dc8 | 294 | sbus_.format(8, Serial::Even, 2); |
TUATBM | 0:4013a9855dc8 | 295 | //start sbus irq |
TUATBM | 2:b88f73d7073c | 296 | sbus_.attach(&catch_sbus, RawSerial::RxIrq); |
TUATBM | 0:4013a9855dc8 | 297 | } |
TUATBM | 0:4013a9855dc8 | 298 | |
TUATBM | 0:4013a9855dc8 | 299 | //---CheckSW_up--- |
TUATBM | 0:4013a9855dc8 | 300 | //プロポのスイッチがonかoffか返す関数 |
TUATBM | 0:4013a9855dc8 | 301 | //ch 1~8 |
TUATBM | 0:4013a9855dc8 | 302 | bool CheckSW_up(int8_t ch) |
TUATBM | 0:4013a9855dc8 | 303 | { |
TUATBM | 0:4013a9855dc8 | 304 | uint16_t checkpwm; |
TUATBM | 0:4013a9855dc8 | 305 | checkpwm = (channels[ch-1]>>1) + 1000; |
TUATBM | 0:4013a9855dc8 | 306 | //pc.printf("ch%d=%d ", ch, checkpwm); |
TUATBM | 0:4013a9855dc8 | 307 | if(SWITCH_CHECK > checkpwm){ |
TUATBM | 0:4013a9855dc8 | 308 | return true; |
TUATBM | 0:4013a9855dc8 | 309 | }else{ |
TUATBM | 0:4013a9855dc8 | 310 | return false; |
TUATBM | 0:4013a9855dc8 | 311 | } |
TUATBM | 0:4013a9855dc8 | 312 | } |
TUATBM | 0:4013a9855dc8 | 313 | |
TUATBM | 0:4013a9855dc8 | 314 | |
motoseki | 6:13bb74a57a33 | 315 | |
TUATBM | 0:4013a9855dc8 | 316 | //---sensing--- |
TUATBM | 0:4013a9855dc8 | 317 | //センサーの値を読み込み、各種フィルタをかける(認知) |
TUATBM | 0:4013a9855dc8 | 318 | void sensing() |
TUATBM | 0:4013a9855dc8 | 319 | { |
TUATBM | 0:4013a9855dc8 | 320 | // If intPin goes high, all data registers have new data |
TUATBM | 0:4013a9855dc8 | 321 | if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt |
TUATBM | 0:4013a9855dc8 | 322 | |
TUATBM | 0:4013a9855dc8 | 323 | mpu9250.readAccelData(accelCount); // Read the x/y/z adc values |
TUATBM | 0:4013a9855dc8 | 324 | // Now we'll calculate the accleration value into actual g's |
TUATBM | 0:4013a9855dc8 | 325 | ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set |
TUATBM | 0:4013a9855dc8 | 326 | ay = (float)accelCount[1]*aRes - accelBias[1]; |
TUATBM | 0:4013a9855dc8 | 327 | az = (float)accelCount[2]*aRes - accelBias[2]; |
TUATBM | 0:4013a9855dc8 | 328 | |
TUATBM | 0:4013a9855dc8 | 329 | mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values |
TUATBM | 0:4013a9855dc8 | 330 | // Calculate the gyro value into actual degrees per second |
TUATBM | 0:4013a9855dc8 | 331 | gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set |
TUATBM | 0:4013a9855dc8 | 332 | gy = (float)gyroCount[1]*gRes - gyroBias[1]; |
TUATBM | 0:4013a9855dc8 | 333 | gz = (float)gyroCount[2]*gRes - gyroBias[2]; |
TUATBM | 0:4013a9855dc8 | 334 | |
TUATBM | 0:4013a9855dc8 | 335 | mpu9250.readMagData(magCount); // Read the x/y/z adc values |
TUATBM | 0:4013a9855dc8 | 336 | // Calculate the magnetometer values in milliGauss |
TUATBM | 0:4013a9855dc8 | 337 | // Include factory calibration per data sheet and user environmental corrections |
TUATBM | 0:4013a9855dc8 | 338 | mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set |
TUATBM | 0:4013a9855dc8 | 339 | my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]; |
TUATBM | 0:4013a9855dc8 | 340 | mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; |
TUATBM | 0:4013a9855dc8 | 341 | } |
TUATBM | 0:4013a9855dc8 | 342 | |
TUATBM | 0:4013a9855dc8 | 343 | Now = t.read_us(); |
TUATBM | 0:4013a9855dc8 | 344 | deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update |
TUATBM | 0:4013a9855dc8 | 345 | lastUpdate = Now; |
TUATBM | 0:4013a9855dc8 | 346 | |
TUATBM | 0:4013a9855dc8 | 347 | sum += deltat; |
TUATBM | 0:4013a9855dc8 | 348 | sumCount++; |
TUATBM | 0:4013a9855dc8 | 349 | |
TUATBM | 0:4013a9855dc8 | 350 | // if(lastUpdate - firstUpdate > 10000000.0f) { |
TUATBM | 0:4013a9855dc8 | 351 | // beta = 0.04; // decrease filter gain after stabilized |
TUATBM | 0:4013a9855dc8 | 352 | // zeta = 0.015; // increasey bias drift gain after stabilized |
TUATBM | 0:4013a9855dc8 | 353 | // } |
TUATBM | 0:4013a9855dc8 | 354 | |
TUATBM | 0:4013a9855dc8 | 355 | // Pass gyro rate as rad/s |
TUATBM | 0:4013a9855dc8 | 356 | mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); |
TUATBM | 0:4013a9855dc8 | 357 | mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); |
TUATBM | 0:4013a9855dc8 | 358 | |
TUATBM | 2:b88f73d7073c | 359 | /* Auto_Loop()で50ms待ってるから要らないと思う |
TUATBM | 0:4013a9855dc8 | 360 | // Serial print and/or display at 0.5 s rate independent of data rates |
TUATBM | 0:4013a9855dc8 | 361 | delt_t = t.read_ms() - count; |
TUATBM | 0:4013a9855dc8 | 362 | if (delt_t > 500) { // update LCD once per half-second independent of read rate |
TUATBM | 2:b88f73d7073c | 363 | */ |
TUATBM | 0:4013a9855dc8 | 364 | |
TUATBM | 1:fc5988ebfa53 | 365 | //pc.printf("ax = %f", 1000*ax); |
TUATBM | 1:fc5988ebfa53 | 366 | //pc.printf(" ay = %f", 1000*ay); |
TUATBM | 1:fc5988ebfa53 | 367 | //pc.printf(" az = %f mg\n\r", 1000*az); |
TUATBM | 0:4013a9855dc8 | 368 | |
TUATBM | 0:4013a9855dc8 | 369 | //pc.printf("gx = %f", gx); |
TUATBM | 0:4013a9855dc8 | 370 | //pc.printf(" gy = %f", gy); |
TUATBM | 0:4013a9855dc8 | 371 | //pc.printf(" gz = %f deg/s\n\r", gz); |
TUATBM | 0:4013a9855dc8 | 372 | |
TUATBM | 3:8c01b4f33389 | 373 | //pc.printf("mx = %f", mx); |
TUATBM | 3:8c01b4f33389 | 374 | //pc.printf(" my = %f", my); |
TUATBM | 3:8c01b4f33389 | 375 | //pc.printf(" mz = %f mG\n\r", mz); |
TUATBM | 0:4013a9855dc8 | 376 | |
TUATBM | 0:4013a9855dc8 | 377 | tempCount = mpu9250.readTempData(); // Read the adc values |
TUATBM | 0:4013a9855dc8 | 378 | temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade |
TUATBM | 0:4013a9855dc8 | 379 | //pc.printf(" temperature = %f C\n\r", temperature); |
TUATBM | 0:4013a9855dc8 | 380 | |
TUATBM | 0:4013a9855dc8 | 381 | //pc.printf("q0 = %f\n\r", q[0]); |
TUATBM | 0:4013a9855dc8 | 382 | //pc.printf("q1 = %f\n\r", q[1]); |
TUATBM | 0:4013a9855dc8 | 383 | //pc.printf("q2 = %f\n\r", q[2]); |
TUATBM | 0:4013a9855dc8 | 384 | //pc.printf("q3 = %f\n\r", q[3]); |
TUATBM | 0:4013a9855dc8 | 385 | |
TUATBM | 0:4013a9855dc8 | 386 | |
TUATBM | 0:4013a9855dc8 | 387 | // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. |
TUATBM | 0:4013a9855dc8 | 388 | // In this coordinate system, the positive z-axis is down toward Earth. |
TUATBM | 0:4013a9855dc8 | 389 | // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise. |
TUATBM | 0:4013a9855dc8 | 390 | // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. |
TUATBM | 0:4013a9855dc8 | 391 | // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. |
TUATBM | 0:4013a9855dc8 | 392 | // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. |
TUATBM | 0:4013a9855dc8 | 393 | // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be |
TUATBM | 0:4013a9855dc8 | 394 | // applied in the correct order which for this configuration is yaw, pitch, and then roll. |
TUATBM | 0:4013a9855dc8 | 395 | // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. |
TUATBM | 0:4013a9855dc8 | 396 | yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); |
motoseki | 4:ba610b05751d | 397 | roll = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); |
motoseki | 4:ba610b05751d | 398 | pitch = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); |
TUATBM | 0:4013a9855dc8 | 399 | pitch *= 180.0f / PI; |
TUATBM | 0:4013a9855dc8 | 400 | yaw *= 180.0f / PI; |
TUATBM | 0:4013a9855dc8 | 401 | yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 |
TUATBM | 0:4013a9855dc8 | 402 | roll *= 180.0f / PI; |
TUATBM | 0:4013a9855dc8 | 403 | |
motoseki | 6:13bb74a57a33 | 404 | //pc.printf("$%d %d %d;",(int)(yaw*100),(int)(pitch*100),(int)(roll*100)); |
motoseki | 4:ba610b05751d | 405 | //pc.printf("Yaw: %f Pitch:%f Roll:%f\n\r", yaw, pitch, roll); |
TUATBM | 0:4013a9855dc8 | 406 | //pc.printf("average rate = %f\n\r", (float) sumCount/sum); |
TUATBM | 0:4013a9855dc8 | 407 | |
TUATBM | 0:4013a9855dc8 | 408 | count = t.read_ms(); |
TUATBM | 0:4013a9855dc8 | 409 | sum = 0; |
TUATBM | 0:4013a9855dc8 | 410 | sumCount = 0; |
TUATBM | 2:b88f73d7073c | 411 | //} |
TUATBM | 0:4013a9855dc8 | 412 | } |
motoseki | 4:ba610b05751d | 413 | |
motoseki | 6:13bb74a57a33 | 414 | |
motoseki | 6:13bb74a57a33 | 415 | //---CalibrateCompass--- |
motoseki | 6:13bb74a57a33 | 416 | //グラウンドチェック時にスイッチ7だけをonにしておくことで実行 リポ抜いたらやり直ししなくてはならない |
motoseki | 6:13bb74a57a33 | 417 | void CalibrateCompass(void) |
motoseki | 6:13bb74a57a33 | 418 | { |
motoseki | 6:13bb74a57a33 | 419 | led1 = 1; |
motoseki | 6:13bb74a57a33 | 420 | sensing(); |
motoseki | 6:13bb74a57a33 | 421 | uint8_t ii; |
motoseki | 6:13bb74a57a33 | 422 | float mxMAX=mx, mxMIN=mx; |
motoseki | 6:13bb74a57a33 | 423 | float myMAX=my, myMIN=my; |
motoseki | 6:13bb74a57a33 | 424 | magbias[0] = 0; //初期化 |
motoseki | 6:13bb74a57a33 | 425 | magbias[1] = 0; |
motoseki | 6:13bb74a57a33 | 426 | //magbias[2] = 0; |
motoseki | 6:13bb74a57a33 | 427 | ii=0; |
motoseki | 6:13bb74a57a33 | 428 | while(1){ //キャリブレーション実行 led2が点滅時に機体を一回転させる xy方向のみ |
motoseki | 6:13bb74a57a33 | 429 | sensing(); |
motoseki | 6:13bb74a57a33 | 430 | if(mxMAX < mx) mxMAX = mx; |
motoseki | 6:13bb74a57a33 | 431 | if(mxMIN > mx) mxMIN = mx; |
motoseki | 6:13bb74a57a33 | 432 | if(myMAX < my) myMAX = my; |
motoseki | 6:13bb74a57a33 | 433 | if(myMIN > my) myMIN = my; |
motoseki | 6:13bb74a57a33 | 434 | led2 = !led2; |
motoseki | 6:13bb74a57a33 | 435 | ++ii; |
motoseki | 6:13bb74a57a33 | 436 | wait(50); |
motoseki | 6:13bb74a57a33 | 437 | |
motoseki | 6:13bb74a57a33 | 438 | if(!CheckSW_up(7)) break; //スイッチ7を下げて終了 |
motoseki | 6:13bb74a57a33 | 439 | } |
motoseki | 6:13bb74a57a33 | 440 | led2 = 0; |
motoseki | 6:13bb74a57a33 | 441 | if(ii<20){ //チャンネル7をすぐ切った場合 元に戻す |
motoseki | 6:13bb74a57a33 | 442 | magbias[0] = MAGBIASX; |
motoseki | 6:13bb74a57a33 | 443 | magbias[1] = MAGBIASY; |
motoseki | 6:13bb74a57a33 | 444 | }else{ //しっかりと値を入れた場合 |
motoseki | 6:13bb74a57a33 | 445 | magbias[0] = (mxMAX+mxMIN)/2; |
motoseki | 6:13bb74a57a33 | 446 | magbias[1] = (myMAX+myMIN)/2; |
motoseki | 6:13bb74a57a33 | 447 | } |
motoseki | 6:13bb74a57a33 | 448 | led1 = 0; |
motoseki | 6:13bb74a57a33 | 449 | t.reset(); //タイマーをリセットして終了 |
motoseki | 6:13bb74a57a33 | 450 | } |
motoseki | 6:13bb74a57a33 | 451 | |
TUATBM | 0:4013a9855dc8 | 452 | |
TUATBM | 0:4013a9855dc8 | 453 | //---StabiGyro--- |
TUATBM | 0:4013a9855dc8 | 454 | //水平旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断) |
motoseki | 4:ba610b05751d | 455 | //右旋回 |
TUATBM | 0:4013a9855dc8 | 456 | void StabiGyro() |
TUATBM | 0:4013a9855dc8 | 457 | { |
motoseki | 6:13bb74a57a33 | 458 | if(jj<=20){ //旋回し始め |
motoseki | 6:13bb74a57a33 | 459 | Auto_RUD = RUD_N + -105 +int((-30-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); //roll -> gy 3.5 |
motoseki | 6:13bb74a57a33 | 460 | Auto_ELE = ELE_N + -75 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN); //pitch -> -gx |
motoseki | 6:13bb74a57a33 | 461 | Auto_THR = oldTHL + 50; |
TUATBM | 0:4013a9855dc8 | 462 | //pc.printf("first%d\r\n",jj); |
TUATBM | 0:4013a9855dc8 | 463 | jj++; |
TUATBM | 0:4013a9855dc8 | 464 | }else{ //旋回中 |
motoseki | 6:13bb74a57a33 | 465 | Auto_RUD = RUD_N + -80 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); |
motoseki | 6:13bb74a57a33 | 466 | Auto_ELE = ELE_N + -90 + int( (-18 - pitch)*ELEVATOR_GN - gx*ELE_DGN); |
motoseki | 6:13bb74a57a33 | 467 | Auto_THR = oldTHL; |
TUATBM | 0:4013a9855dc8 | 468 | } |
TUATBM | 0:4013a9855dc8 | 469 | |
TUATBM | 0:4013a9855dc8 | 470 | //pwmの値が最小値と最大値の間に入ってなかったら、強制的に最大値or最小値にする |
motoseki | 4:ba610b05751d | 471 | if(Auto_RUD > RUD_MAX){ |
motoseki | 4:ba610b05751d | 472 | Auto_RUD = RUD_MAX; |
motoseki | 4:ba610b05751d | 473 | }else if(Auto_RUD < RUD_MIN){ |
motoseki | 4:ba610b05751d | 474 | Auto_RUD = RUD_MIN; |
TUATBM | 0:4013a9855dc8 | 475 | } |
motoseki | 4:ba610b05751d | 476 | if(Auto_ELE > ELE_MAX){ |
motoseki | 4:ba610b05751d | 477 | Auto_ELE = ELE_MAX; |
motoseki | 4:ba610b05751d | 478 | }else if(Auto_ELE < ELE_MIN){ |
motoseki | 4:ba610b05751d | 479 | Auto_ELE = ELE_MIN; |
TUATBM | 0:4013a9855dc8 | 480 | } |
TUATBM | 0:4013a9855dc8 | 481 | |
TUATBM | 0:4013a9855dc8 | 482 | //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE); |
TUATBM | 0:4013a9855dc8 | 483 | //Thread::wait(G_MS); |
TUATBM | 0:4013a9855dc8 | 484 | } |
TUATBM | 0:4013a9855dc8 | 485 | |
TUATBM | 0:4013a9855dc8 | 486 | //---StabiGyro_Mobius--- |
TUATBM | 0:4013a9855dc8 | 487 | //8の字旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断) |
TUATBM | 0:4013a9855dc8 | 488 | void StabiGyro_Mobius() |
TUATBM | 0:4013a9855dc8 | 489 | { |
TUATBM | 0:4013a9855dc8 | 490 | if(jj<=25){ //右旋回導入 |
motoseki | 6:13bb74a57a33 | 491 | Auto_RUD = RUD_N + int((-25-roll)*RUDDER_GN - gy*RUD_DGN); |
motoseki | 4:ba610b05751d | 492 | Auto_ELE = ELE_N + int( (LAND_ANGLE + 3 - pitch)*ELEVATOR_GN - gx*ELE_DGN); |
motoseki | 4:ba610b05751d | 493 | Auto_THR = THR_N + 50; |
TUATBM | 0:4013a9855dc8 | 494 | //pc.printf("first%d\r\n",jj); |
TUATBM | 0:4013a9855dc8 | 495 | jj++; |
TUATBM | 0:4013a9855dc8 | 496 | }else if((25<jj) && (jj<=112)){ //右旋回途中 |
motoseki | 6:13bb74a57a33 | 497 | Auto_RUD = RUD_N + int((-20-roll)*RUDDER_GN - gy*RUD_DGN); |
motoseki | 4:ba610b05751d | 498 | Auto_ELE = ELE_N + int( (LAND_ANGLE - 1 - pitch)*ELEVATOR_GN - gx*ELE_DGN); |
TUATBM | 0:4013a9855dc8 | 499 | Auto_THR = THR_N; |
TUATBM | 0:4013a9855dc8 | 500 | jj++; |
TUATBM | 0:4013a9855dc8 | 501 | }else if((112<jj) && (jj<=117)){ //旋回遷移1 |
motoseki | 4:ba610b05751d | 502 | Auto_RUD = RUD_N + int((-roll)*RUDDER_GN - gy*RUD_DGN); |
motoseki | 4:ba610b05751d | 503 | Auto_ELE = ELE_N + int( (LAND_ANGLE + 2 - pitch)*ELEVATOR_GN - gx*ELE_DGN); |
motoseki | 4:ba610b05751d | 504 | Auto_THR = THR_N - 100; |
TUATBM | 0:4013a9855dc8 | 505 | //pc.printf("first%d\r\n",jj); |
TUATBM | 0:4013a9855dc8 | 506 | jj++; |
TUATBM | 0:4013a9855dc8 | 507 | }else if((117<jj) && (jj<=132)){ //旋回遷移2 |
motoseki | 6:13bb74a57a33 | 508 | Auto_RUD = RUD_N + int((25-roll)*RUDDER_GN - gy*RUD_DGN); |
motoseki | 4:ba610b05751d | 509 | Auto_ELE = ELE_N + int( (LAND_ANGLE + 6 - pitch)*ELEVATOR_GN - gx*ELE_DGN); |
motoseki | 4:ba610b05751d | 510 | Auto_THR = THR_N - 20; |
TUATBM | 0:4013a9855dc8 | 511 | jj++; |
TUATBM | 0:4013a9855dc8 | 512 | }else{ //左旋回途中 |
motoseki | 6:13bb74a57a33 | 513 | Auto_RUD = RUD_N + int((20-roll)*RUDDER_GN - gy*RUD_DGN); |
motoseki | 4:ba610b05751d | 514 | Auto_ELE = ELE_N + int( (LAND_ANGLE - pitch)*ELEVATOR_GN - gx*ELE_DGN); |
motoseki | 4:ba610b05751d | 515 | Auto_THR = THR_N + 30; |
TUATBM | 0:4013a9855dc8 | 516 | } |
TUATBM | 0:4013a9855dc8 | 517 | |
motoseki | 4:ba610b05751d | 518 | if(Auto_RUD > RUD_MAX){ |
motoseki | 4:ba610b05751d | 519 | Auto_RUD = RUD_MAX; |
motoseki | 4:ba610b05751d | 520 | }else if(Auto_RUD < RUD_MIN){ |
motoseki | 4:ba610b05751d | 521 | Auto_RUD = RUD_MIN; |
TUATBM | 0:4013a9855dc8 | 522 | } |
motoseki | 4:ba610b05751d | 523 | if(Auto_ELE > ELE_MAX){ |
motoseki | 4:ba610b05751d | 524 | Auto_ELE = ELE_MAX; |
motoseki | 4:ba610b05751d | 525 | }else if(Auto_ELE < ELE_MIN){ |
motoseki | 4:ba610b05751d | 526 | Auto_ELE = ELE_MIN; |
TUATBM | 0:4013a9855dc8 | 527 | } |
TUATBM | 0:4013a9855dc8 | 528 | |
TUATBM | 0:4013a9855dc8 | 529 | //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE); |
TUATBM | 0:4013a9855dc8 | 530 | //Thread::wait(G_MS); |
TUATBM | 0:4013a9855dc8 | 531 | } |
TUATBM | 0:4013a9855dc8 | 532 | |
TUATBM | 0:4013a9855dc8 | 533 | //---StabiGyro_Climb--- |
TUATBM | 0:4013a9855dc8 | 534 | //上昇旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断) |
TUATBM | 0:4013a9855dc8 | 535 | void StabiGyro_Climb() |
TUATBM | 0:4013a9855dc8 | 536 | { |
TUATBM | 0:4013a9855dc8 | 537 | if(jj<=25){ //右旋回導入 |
motoseki | 6:13bb74a57a33 | 538 | Auto_RUD = RUD_N + int((-25-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); //roll -> gy |
motoseki | 5:c53ca479e96c | 539 | Auto_ELE = ELE_N + int( (LAND_ANGLE + 3 - pitch)*ELEVATOR_GN - gx*ELE_DGN); //pitch -> -gx |
TUATBM | 0:4013a9855dc8 | 540 | Auto_THR = THR_N + 50; |
TUATBM | 0:4013a9855dc8 | 541 | //pc.printf("first%d\r\n",jj); |
TUATBM | 0:4013a9855dc8 | 542 | jj++; |
TUATBM | 0:4013a9855dc8 | 543 | }else if((25<jj) && (jj<=224)){ //右旋回途中 |
motoseki | 6:13bb74a57a33 | 544 | Auto_RUD = RUD_N + int((-20-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); |
motoseki | 5:c53ca479e96c | 545 | Auto_ELE = ELE_N + int( (LAND_ANGLE - pitch)*ELEVATOR_GN - gx*ELE_DGN); |
TUATBM | 0:4013a9855dc8 | 546 | Auto_THR = THR_N; |
TUATBM | 0:4013a9855dc8 | 547 | jj++; |
TUATBM | 0:4013a9855dc8 | 548 | /*}else if((224<jj) && (jj<=234)){ //上昇遷移 |
TUATBM | 0:4013a9855dc8 | 549 | Auto_RUD = RUD_N + 160 - int((g_HpfGyro[0]+25)*0.75); |
TUATBM | 0:4013a9855dc8 | 550 | Auto_ELE = ELE_N + 20 + int((g_HpfGyro[1]-30)*0.8); |
TUATBM | 0:4013a9855dc8 | 551 | Auto_THR = THR_N + 100; |
TUATBM | 0:4013a9855dc8 | 552 | //pc.printf("first%d\r\n",jj); |
TUATBM | 0:4013a9855dc8 | 553 | jj++;*/ |
TUATBM | 0:4013a9855dc8 | 554 | }else if((224<jj) && (jj<=314)){ //上昇中 |
motoseki | 5:c53ca479e96c | 555 | Auto_RUD = RUD_N + int((-10-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); |
motoseki | 5:c53ca479e96c | 556 | Auto_ELE = ELE_N + int( (LAND_ANGLE + 8 - pitch)*ELEVATOR_GN - gx*ELE_DGN); |
motoseki | 5:c53ca479e96c | 557 | Auto_THR = THR_N + 200; |
TUATBM | 0:4013a9855dc8 | 558 | jj++; |
TUATBM | 0:4013a9855dc8 | 559 | }else{ //水平旋回 |
motoseki | 5:c53ca479e96c | 560 | Auto_RUD = RUD_N + int((-15-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); //roll -> gy |
motoseki | 5:c53ca479e96c | 561 | Auto_ELE = ELE_N + int( (LAND_ANGLE + 3 - pitch)*ELEVATOR_GN - gx*ELE_DGN); //pitch -> -gx |
motoseki | 5:c53ca479e96c | 562 | Auto_THR = THR_N; |
TUATBM | 0:4013a9855dc8 | 563 | } |
TUATBM | 0:4013a9855dc8 | 564 | |
motoseki | 4:ba610b05751d | 565 | if(Auto_RUD > RUD_MAX){ |
motoseki | 4:ba610b05751d | 566 | Auto_RUD = RUD_MAX; |
motoseki | 4:ba610b05751d | 567 | }else if(Auto_RUD < RUD_MIN){ |
motoseki | 4:ba610b05751d | 568 | Auto_RUD = RUD_MIN; |
TUATBM | 0:4013a9855dc8 | 569 | } |
motoseki | 4:ba610b05751d | 570 | if(Auto_ELE > ELE_MAX){ |
motoseki | 4:ba610b05751d | 571 | Auto_ELE = ELE_MAX; |
motoseki | 4:ba610b05751d | 572 | }else if(Auto_ELE < ELE_MIN){ |
motoseki | 4:ba610b05751d | 573 | Auto_ELE = ELE_MIN; |
TUATBM | 0:4013a9855dc8 | 574 | } |
TUATBM | 0:4013a9855dc8 | 575 | |
TUATBM | 0:4013a9855dc8 | 576 | //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE); |
TUATBM | 0:4013a9855dc8 | 577 | //Thread::wait(G_MS); |
TUATBM | 0:4013a9855dc8 | 578 | } |
TUATBM | 0:4013a9855dc8 | 579 | |
TUATBM | 0:4013a9855dc8 | 580 | //---StabiGyro_Glide--- |
TUATBM | 0:4013a9855dc8 | 581 | //自動滑空のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断) |
TUATBM | 0:4013a9855dc8 | 582 | void StabiGyro_Glide() |
TUATBM | 0:4013a9855dc8 | 583 | { |
TUATBM | 0:4013a9855dc8 | 584 | //if(jj<=25){ |
TUATBM | 0:4013a9855dc8 | 585 | // Auto_RUD = RUD_N - 30 - int((g_HpfGyro[0]+130)*0.75); |
TUATBM | 0:4013a9855dc8 | 586 | // Auto_ELE = ELE_N + 30+ int((g_HpfGyro[1]-30)*0.8); |
TUATBM | 0:4013a9855dc8 | 587 | // Auto_THR = THR_N + 50; |
TUATBM | 0:4013a9855dc8 | 588 | //pc.printf("first%d\r\n",jj); |
TUATBM | 0:4013a9855dc8 | 589 | // jj++; |
TUATBM | 0:4013a9855dc8 | 590 | //}else{ |
motoseki | 5:c53ca479e96c | 591 | Auto_RUD = RUD_N + int((-10-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); |
motoseki | 5:c53ca479e96c | 592 | Auto_ELE = ELE_N + int( (LAND_ANGLE + 3 - pitch)*ELEVATOR_GN - gx*ELE_DGN); |
TUATBM | 0:4013a9855dc8 | 593 | Auto_THR = 1000; //スロットルoff |
TUATBM | 0:4013a9855dc8 | 594 | |
motoseki | 4:ba610b05751d | 595 | if(Auto_RUD > RUD_MAX){ |
motoseki | 4:ba610b05751d | 596 | Auto_RUD = RUD_MAX; |
motoseki | 4:ba610b05751d | 597 | }else if(Auto_RUD < RUD_MIN){ |
motoseki | 4:ba610b05751d | 598 | Auto_RUD = RUD_MIN; |
TUATBM | 0:4013a9855dc8 | 599 | } |
motoseki | 4:ba610b05751d | 600 | if(Auto_ELE > ELE_MAX){ |
motoseki | 4:ba610b05751d | 601 | Auto_ELE = ELE_MAX; |
motoseki | 4:ba610b05751d | 602 | }else if(Auto_ELE < ELE_MIN){ |
motoseki | 4:ba610b05751d | 603 | Auto_ELE = ELE_MIN; |
TUATBM | 0:4013a9855dc8 | 604 | } |
TUATBM | 0:4013a9855dc8 | 605 | |
TUATBM | 0:4013a9855dc8 | 606 | //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE); |
TUATBM | 0:4013a9855dc8 | 607 | //Thread::wait(G_MS); |
TUATBM | 0:4013a9855dc8 | 608 | } |
TUATBM | 0:4013a9855dc8 | 609 | |
TUATBM | 0:4013a9855dc8 | 610 | void Auto_Loop() |
TUATBM | 0:4013a9855dc8 | 611 | { |
motoseki | 4:ba610b05751d | 612 | //while(true){ |
TUATBM | 3:8c01b4f33389 | 613 | //for(k=0;k<100;++k){ |
motoseki | 4:ba610b05751d | 614 | //sensing(); //センサーの値を読み込む(認知) |
TUATBM | 0:4013a9855dc8 | 615 | StabiGyro(); //水平旋回のためのラダー、エレベーターのpwmを計算(判断) |
TUATBM | 0:4013a9855dc8 | 616 | update_mode = UPD_AUTO; //オートモード |
TUATBM | 0:4013a9855dc8 | 617 | //StabiAccel(); |
TUATBM | 0:4013a9855dc8 | 618 | //Auto_ELE+=145; |
TUATBM | 0:4013a9855dc8 | 619 | //Auto_RUD-=80; |
TUATBM | 0:4013a9855dc8 | 620 | //Servos.Auto2Servo(); |
TUATBM | 0:4013a9855dc8 | 621 | //Thread::wait(50); |
motoseki | 4:ba610b05751d | 622 | /* |
motoseki | 4:ba610b05751d | 623 | if(!CheckSW_up(7)){ |
motoseki | 4:ba610b05751d | 624 | pc.printf("go to manual\n"); |
motoseki | 4:ba610b05751d | 625 | break; |
motoseki | 4:ba610b05751d | 626 | }*/ |
motoseki | 4:ba610b05751d | 627 | //wait_ms(50); |
motoseki | 4:ba610b05751d | 628 | //} |
motoseki | 4:ba610b05751d | 629 | //k=0; //for debug |
motoseki | 4:ba610b05751d | 630 | //pwm[6]=1000; |
TUATBM | 0:4013a9855dc8 | 631 | } |
TUATBM | 0:4013a9855dc8 | 632 | |
TUATBM | 0:4013a9855dc8 | 633 | void Auto_Mobius() |
TUATBM | 0:4013a9855dc8 | 634 | { |
TUATBM | 0:4013a9855dc8 | 635 | sensing(); |
TUATBM | 0:4013a9855dc8 | 636 | StabiGyro_Mobius(); |
TUATBM | 0:4013a9855dc8 | 637 | update_mode = UPD_AUTO; |
TUATBM | 0:4013a9855dc8 | 638 | wait_ms(50); |
TUATBM | 0:4013a9855dc8 | 639 | } |
TUATBM | 0:4013a9855dc8 | 640 | |
TUATBM | 0:4013a9855dc8 | 641 | void Auto_Climb() |
TUATBM | 0:4013a9855dc8 | 642 | { |
TUATBM | 0:4013a9855dc8 | 643 | sensing(); |
TUATBM | 0:4013a9855dc8 | 644 | StabiGyro_Climb(); |
TUATBM | 0:4013a9855dc8 | 645 | update_mode = UPD_AUTO; |
TUATBM | 0:4013a9855dc8 | 646 | wait_ms(50); |
TUATBM | 0:4013a9855dc8 | 647 | } |
TUATBM | 0:4013a9855dc8 | 648 | |
TUATBM | 0:4013a9855dc8 | 649 | void Auto_Glide() |
TUATBM | 0:4013a9855dc8 | 650 | { |
TUATBM | 0:4013a9855dc8 | 651 | sensing(); |
TUATBM | 0:4013a9855dc8 | 652 | StabiGyro_Glide(); |
TUATBM | 0:4013a9855dc8 | 653 | update_mode = UPD_AUTO; |
TUATBM | 0:4013a9855dc8 | 654 | wait_ms(50); |
TUATBM | 0:4013a9855dc8 | 655 | } |
TUATBM | 0:4013a9855dc8 | 656 | |
TUATBM | 0:4013a9855dc8 | 657 | void Auto_Landing() |
TUATBM | 0:4013a9855dc8 | 658 | { |
TUATBM | 0:4013a9855dc8 | 659 | update_mode = UPD_MANUAL; |
TUATBM | 0:4013a9855dc8 | 660 | } |
TUATBM | 0:4013a9855dc8 | 661 | |
TUATBM | 0:4013a9855dc8 | 662 | int main() |
TUATBM | 0:4013a9855dc8 | 663 | { |
TUATBM | 0:4013a9855dc8 | 664 | //SBus用バッファ初期化 |
TUATBM | 0:4013a9855dc8 | 665 | int i; |
TUATBM | 0:4013a9855dc8 | 666 | for (i=0; i<25; i++) {buf_sbus[i] = 0;} |
TUATBM | 0:4013a9855dc8 | 667 | |
TUATBM | 0:4013a9855dc8 | 668 | //pc.baud(115200); //パソコン通信用シリアルのボードレート |
TUATBM | 0:4013a9855dc8 | 669 | |
TUATBM | 0:4013a9855dc8 | 670 | //frq.start(); |
TUATBM | 0:4013a9855dc8 | 671 | init_Servo(); |
TUATBM | 0:4013a9855dc8 | 672 | init_sbus(); |
TUATBM | 0:4013a9855dc8 | 673 | disp_clock(); |
TUATBM | 0:4013a9855dc8 | 674 | |
TUATBM | 0:4013a9855dc8 | 675 | //---9軸センサー(MPU9250)初期化--- |
TUATBM | 0:4013a9855dc8 | 676 | |
TUATBM | 0:4013a9855dc8 | 677 | //Set up I2C |
TUATBM | 0:4013a9855dc8 | 678 | i2c.frequency(400000); // use fast (400 kHz) I2C |
TUATBM | 0:4013a9855dc8 | 679 | pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock); |
TUATBM | 0:4013a9855dc8 | 680 | t.start(); |
TUATBM | 0:4013a9855dc8 | 681 | |
TUATBM | 0:4013a9855dc8 | 682 | // Read the WHO_AM_I register, this is a good test of communication |
TUATBM | 0:4013a9855dc8 | 683 | uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 |
TUATBM | 0:4013a9855dc8 | 684 | pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r"); |
TUATBM | 0:4013a9855dc8 | 685 | |
TUATBM | 0:4013a9855dc8 | 686 | if (whoami == 0x71){ // WHO_AM_I should always be 0x68 |
TUATBM | 0:4013a9855dc8 | 687 | pc.printf("MPU9250 is online...\n\r"); |
TUATBM | 1:fc5988ebfa53 | 688 | //wait(1); |
TUATBM | 0:4013a9855dc8 | 689 | |
TUATBM | 0:4013a9855dc8 | 690 | mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration |
TUATBM | 0:4013a9855dc8 | 691 | mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers |
TUATBM | 0:4013a9855dc8 | 692 | //pc.printf("x gyro bias = %f\n\r", gyroBias[0]); |
TUATBM | 0:4013a9855dc8 | 693 | //pc.printf("y gyro bias = %f\n\r", gyroBias[1]); |
TUATBM | 0:4013a9855dc8 | 694 | //pc.printf("z gyro bias = %f\n\r", gyroBias[2]); |
TUATBM | 0:4013a9855dc8 | 695 | //pc.printf("x accel bias = %f\n\r", accelBias[0]); |
TUATBM | 0:4013a9855dc8 | 696 | //pc.printf("y accel bias = %f\n\r", accelBias[1]); |
TUATBM | 0:4013a9855dc8 | 697 | //pc.printf("z accel bias = %f\n\r", accelBias[2]); |
TUATBM | 1:fc5988ebfa53 | 698 | wait(1); |
TUATBM | 0:4013a9855dc8 | 699 | |
TUATBM | 0:4013a9855dc8 | 700 | mpu9250.initMPU9250(); |
TUATBM | 0:4013a9855dc8 | 701 | pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature |
TUATBM | 0:4013a9855dc8 | 702 | mpu9250.initAK8963(magCalibration); |
TUATBM | 0:4013a9855dc8 | 703 | pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer |
TUATBM | 0:4013a9855dc8 | 704 | pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale)); |
TUATBM | 0:4013a9855dc8 | 705 | pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale)); |
TUATBM | 0:4013a9855dc8 | 706 | if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r"); |
TUATBM | 0:4013a9855dc8 | 707 | if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r"); |
TUATBM | 0:4013a9855dc8 | 708 | if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r"); |
TUATBM | 0:4013a9855dc8 | 709 | if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r"); |
TUATBM | 0:4013a9855dc8 | 710 | wait(2); |
TUATBM | 0:4013a9855dc8 | 711 | |
TUATBM | 0:4013a9855dc8 | 712 | }else{ |
TUATBM | 0:4013a9855dc8 | 713 | pc.printf("Could not connect to MPU9250: \n\r"); |
TUATBM | 0:4013a9855dc8 | 714 | pc.printf("%#x \n", whoami); |
TUATBM | 0:4013a9855dc8 | 715 | while(1) ; // Loop forever if communication doesn't happen |
TUATBM | 0:4013a9855dc8 | 716 | } |
TUATBM | 0:4013a9855dc8 | 717 | |
TUATBM | 0:4013a9855dc8 | 718 | mpu9250.getAres(); // Get accelerometer sensitivity |
TUATBM | 0:4013a9855dc8 | 719 | mpu9250.getGres(); // Get gyro sensitivity |
TUATBM | 0:4013a9855dc8 | 720 | mpu9250.getMres(); // Get magnetometer sensitivity |
TUATBM | 0:4013a9855dc8 | 721 | //pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes); |
TUATBM | 0:4013a9855dc8 | 722 | //pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes); |
TUATBM | 0:4013a9855dc8 | 723 | //pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes); |
TUATBM | 3:8c01b4f33389 | 724 | magbias[0] = MAGBIASX; //+470.; // User environmental x-axis correction in milliGauss, should be automatically calculated |
TUATBM | 3:8c01b4f33389 | 725 | magbias[1] = MAGBIASY; //+120.; // User environmental y-axis correction in milliGauss |
TUATBM | 3:8c01b4f33389 | 726 | magbias[2] = MAGBIASZ; //+125.; // User environmental z-axis correction in milliGauss |
TUATBM | 0:4013a9855dc8 | 727 | |
TUATBM | 3:8c01b4f33389 | 728 | t.start(); |
TUATBM | 0:4013a9855dc8 | 729 | pc.printf("mpu9250 initialized\r\n"); |
TUATBM | 0:4013a9855dc8 | 730 | |
TUATBM | 0:4013a9855dc8 | 731 | //---9軸センサー(MPU9250)初期化終わり--- |
TUATBM | 0:4013a9855dc8 | 732 | |
TUATBM | 0:4013a9855dc8 | 733 | led1 = 0; |
TUATBM | 0:4013a9855dc8 | 734 | led2 = 0; |
TUATBM | 1:fc5988ebfa53 | 735 | led3 = 0; |
TUATBM | 1:fc5988ebfa53 | 736 | led4 = 0; |
TUATBM | 0:4013a9855dc8 | 737 | |
TUATBM | 0:4013a9855dc8 | 738 | pc.printf("All initialized\r\n"); |
TUATBM | 0:4013a9855dc8 | 739 | |
TUATBM | 3:8c01b4f33389 | 740 | |
TUATBM | 0:4013a9855dc8 | 741 | while (true) { //無限ループ |
TUATBM | 0:4013a9855dc8 | 742 | //for(i=0;i<8;i++){ |
TUATBM | 0:4013a9855dc8 | 743 | //pc.printf("%x ", buf_sbus[i]); |
TUATBM | 0:4013a9855dc8 | 744 | //pc.printf("ch%d=%d ", i+1, channels[i]); |
TUATBM | 0:4013a9855dc8 | 745 | //} |
TUATBM | 0:4013a9855dc8 | 746 | //pc.printf("\n"); |
motoseki | 4:ba610b05751d | 747 | sensing(); |
motoseki | 4:ba610b05751d | 748 | wait_ms(50); |
TUATBM | 0:4013a9855dc8 | 749 | |
TUATBM | 0:4013a9855dc8 | 750 | switch(OperationMode){ //変数OperationModeの値で場合分け |
TUATBM | 0:4013a9855dc8 | 751 | case GROUNDCHECK: //グラウンドチェック(スイッチが2つ初期値にちゃんとなってたら水平旋回モードに移行) |
TUATBM | 0:4013a9855dc8 | 752 | if(!CheckSW_up(7)&&!CheckSW_up(8)){ //チャンネル7,8が両方offなら |
TUATBM | 0:4013a9855dc8 | 753 | OperationMode=AUTOLOOP; //変数OperationModeにAUTOLOOP(定数なので2(SkipperS.hで定義))を代入 |
TUATBM | 0:4013a9855dc8 | 754 | pc.printf("go to autoloop\r\n"); |
motoseki | 6:13bb74a57a33 | 755 | }//else if(CheckSW_up(7)&&!CheckSW_up(8)){ //チャンネル7だけonなら |
motoseki | 6:13bb74a57a33 | 756 | //CalibrateCompass(); |
motoseki | 6:13bb74a57a33 | 757 | //} |
TUATBM | 0:4013a9855dc8 | 758 | //変数update_modeはSBus(=プロポ=操縦者)からのpwmをそのまま垂れ流すか、自動制御で計算したpwmをサーボに流すかを切り替えるための変数 |
TUATBM | 0:4013a9855dc8 | 759 | update_mode = UPD_MANUAL; //マニュアルモード(そのまま垂れ流す) |
motoseki | 4:ba610b05751d | 760 | // pc.printf("groundcheck mode\r\n"); |
TUATBM | 0:4013a9855dc8 | 761 | |
TUATBM | 0:4013a9855dc8 | 762 | break; |
TUATBM | 0:4013a9855dc8 | 763 | case AUTOLOOP: //水平旋回モード |
motoseki | 4:ba610b05751d | 764 | //t.reset(); |
TUATBM | 0:4013a9855dc8 | 765 | if(CheckSW_up(7)){ //チャンネル7がonなら |
motoseki | 4:ba610b05751d | 766 | //t.reset(); |
motoseki | 4:ba610b05751d | 767 | Auto_Loop(); //関数Auto_Loop実行 |
TUATBM | 0:4013a9855dc8 | 768 | led1 = 0; |
TUATBM | 0:4013a9855dc8 | 769 | led2 = 1; |
motoseki | 4:ba610b05751d | 770 | //Auto_Loop(); |
TUATBM | 3:8c01b4f33389 | 771 | //pc.printf("Auto Loop Now\r\n"); |
TUATBM | 0:4013a9855dc8 | 772 | //autoloop.set_priority(osPriorityAboveNormal); |
TUATBM | 0:4013a9855dc8 | 773 | /* |
TUATBM | 0:4013a9855dc8 | 774 | while(true){ |
TUATBM | 0:4013a9855dc8 | 775 | if(!CheckSW_up(7)){ |
TUATBM | 0:4013a9855dc8 | 776 | autoloop.terminate(); |
TUATBM | 0:4013a9855dc8 | 777 | break; |
TUATBM | 0:4013a9855dc8 | 778 | } |
TUATBM | 0:4013a9855dc8 | 779 | } |
TUATBM | 0:4013a9855dc8 | 780 | */ |
TUATBM | 0:4013a9855dc8 | 781 | }else{ |
TUATBM | 0:4013a9855dc8 | 782 | update_mode = UPD_MANUAL; |
motoseki | 4:ba610b05751d | 783 | //pc.printf("manual Now\r\n"); |
TUATBM | 0:4013a9855dc8 | 784 | led1 = 1; |
TUATBM | 0:4013a9855dc8 | 785 | led2 = 0; |
TUATBM | 3:8c01b4f33389 | 786 | |
motoseki | 4:ba610b05751d | 787 | /*debug*/ |
motoseki | 4:ba610b05751d | 788 | //k++; |
motoseki | 4:ba610b05751d | 789 | //wait(50); |
motoseki | 4:ba610b05751d | 790 | //if(k>100) pwm[6]=2000; |
TUATBM | 0:4013a9855dc8 | 791 | } |
TUATBM | 0:4013a9855dc8 | 792 | |
TUATBM | 0:4013a9855dc8 | 793 | if(!CheckSW_up(7)&&CheckSW_up(8)){ //チャンネル7がoff、8がonなら |
TUATBM | 0:4013a9855dc8 | 794 | OperationMode=AUTOMOBIUS; |
TUATBM | 0:4013a9855dc8 | 795 | pc.printf("go to auto8\r\n"); |
TUATBM | 0:4013a9855dc8 | 796 | } |
TUATBM | 0:4013a9855dc8 | 797 | break; |
TUATBM | 0:4013a9855dc8 | 798 | case AUTOMOBIUS: //8の字旋回モード |
TUATBM | 0:4013a9855dc8 | 799 | if(CheckSW_up(7)){ |
TUATBM | 0:4013a9855dc8 | 800 | Auto_Mobius(); |
TUATBM | 0:4013a9855dc8 | 801 | led1 = 0; |
TUATBM | 0:4013a9855dc8 | 802 | led2 = 1; |
TUATBM | 0:4013a9855dc8 | 803 | pc.printf("Auto Mobius Now\r\n"); |
TUATBM | 0:4013a9855dc8 | 804 | }else{ |
TUATBM | 0:4013a9855dc8 | 805 | update_mode = UPD_MANUAL; |
TUATBM | 0:4013a9855dc8 | 806 | pc.printf("manual Now\r\n"); |
TUATBM | 0:4013a9855dc8 | 807 | led1 = 1; |
TUATBM | 0:4013a9855dc8 | 808 | led2 = 1; |
TUATBM | 0:4013a9855dc8 | 809 | } |
TUATBM | 0:4013a9855dc8 | 810 | |
TUATBM | 0:4013a9855dc8 | 811 | if(!CheckSW_up(7)&&!CheckSW_up(8)){ |
TUATBM | 0:4013a9855dc8 | 812 | OperationMode=AUTOCLIMB; |
TUATBM | 0:4013a9855dc8 | 813 | pc.printf("go to autoclimb\r\n"); |
TUATBM | 0:4013a9855dc8 | 814 | } |
TUATBM | 0:4013a9855dc8 | 815 | break; |
TUATBM | 0:4013a9855dc8 | 816 | case AUTOCLIMB: //上昇旋回モード |
TUATBM | 0:4013a9855dc8 | 817 | if(CheckSW_up(7)){ |
TUATBM | 0:4013a9855dc8 | 818 | Auto_Climb(); |
TUATBM | 0:4013a9855dc8 | 819 | led1 = 0; |
TUATBM | 0:4013a9855dc8 | 820 | led2 = 1; |
TUATBM | 0:4013a9855dc8 | 821 | pc.printf("Auto Climb Now\r\n"); |
TUATBM | 0:4013a9855dc8 | 822 | }else{ |
TUATBM | 0:4013a9855dc8 | 823 | update_mode = UPD_MANUAL; |
TUATBM | 0:4013a9855dc8 | 824 | pc.printf("manual Now\r\n"); |
TUATBM | 0:4013a9855dc8 | 825 | led1 = 1; |
TUATBM | 0:4013a9855dc8 | 826 | led2 = 0; |
TUATBM | 0:4013a9855dc8 | 827 | } |
TUATBM | 0:4013a9855dc8 | 828 | |
TUATBM | 0:4013a9855dc8 | 829 | if(!CheckSW_up(7)&&CheckSW_up(8)){ |
TUATBM | 0:4013a9855dc8 | 830 | OperationMode=AUTOGLIDE; |
TUATBM | 0:4013a9855dc8 | 831 | pc.printf("go to manual\r\n"); |
TUATBM | 0:4013a9855dc8 | 832 | } |
TUATBM | 0:4013a9855dc8 | 833 | break; |
TUATBM | 0:4013a9855dc8 | 834 | |
TUATBM | 0:4013a9855dc8 | 835 | case AUTOGLIDE: //自動滑空モード |
TUATBM | 0:4013a9855dc8 | 836 | if(CheckSW_up(7)){ |
TUATBM | 0:4013a9855dc8 | 837 | Auto_Glide(); |
TUATBM | 0:4013a9855dc8 | 838 | led1 = 0; |
TUATBM | 0:4013a9855dc8 | 839 | led2 = 1; |
TUATBM | 0:4013a9855dc8 | 840 | pc.printf("Auto Glide Now\r\n"); |
TUATBM | 0:4013a9855dc8 | 841 | }else{ |
TUATBM | 0:4013a9855dc8 | 842 | update_mode = UPD_MANUAL; |
TUATBM | 0:4013a9855dc8 | 843 | pc.printf("manual Now\r\n"); |
TUATBM | 0:4013a9855dc8 | 844 | led1 = 1; |
TUATBM | 0:4013a9855dc8 | 845 | led2 = 1; |
TUATBM | 0:4013a9855dc8 | 846 | } |
TUATBM | 0:4013a9855dc8 | 847 | |
TUATBM | 0:4013a9855dc8 | 848 | if(!CheckSW_up(7)&&!CheckSW_up(8)){ |
motoseki | 6:13bb74a57a33 | 849 | OperationMode=AUTOLANDING; |
TUATBM | 0:4013a9855dc8 | 850 | pc.printf("go to autoloop\r\n"); |
TUATBM | 0:4013a9855dc8 | 851 | } |
TUATBM | 0:4013a9855dc8 | 852 | break; |
motoseki | 6:13bb74a57a33 | 853 | |
TUATBM | 0:4013a9855dc8 | 854 | case AUTOLANDING: |
motoseki | 6:13bb74a57a33 | 855 | led3 = 1; //赤外線 |
TUATBM | 0:4013a9855dc8 | 856 | if(CheckSW_up(7)){ |
TUATBM | 0:4013a9855dc8 | 857 | Auto_Landing(); |
TUATBM | 0:4013a9855dc8 | 858 | led1 = 0; |
TUATBM | 0:4013a9855dc8 | 859 | led2 = 1; |
TUATBM | 0:4013a9855dc8 | 860 | pc.printf("Auto Landing Now\r\n"); |
TUATBM | 0:4013a9855dc8 | 861 | }else{ |
TUATBM | 0:4013a9855dc8 | 862 | update_mode = UPD_MANUAL; |
TUATBM | 0:4013a9855dc8 | 863 | pc.printf("manual Now\r\n"); |
TUATBM | 0:4013a9855dc8 | 864 | led1 = 1; |
TUATBM | 0:4013a9855dc8 | 865 | led2 = 0; |
TUATBM | 0:4013a9855dc8 | 866 | } |
TUATBM | 0:4013a9855dc8 | 867 | |
TUATBM | 0:4013a9855dc8 | 868 | if(!CheckSW_up(7)&&CheckSW_up(8)){ |
motoseki | 6:13bb74a57a33 | 869 | //OperationMode=MANUALMODE; |
motoseki | 6:13bb74a57a33 | 870 | //pc.printf("go to manualmade\r\n"); |
TUATBM | 0:4013a9855dc8 | 871 | } |
TUATBM | 0:4013a9855dc8 | 872 | break; |
motoseki | 6:13bb74a57a33 | 873 | /* |
TUATBM | 0:4013a9855dc8 | 874 | case MANUALMODE: |
TUATBM | 0:4013a9855dc8 | 875 | update_mode = UPD_MANUAL; |
TUATBM | 0:4013a9855dc8 | 876 | led1 = 1; |
TUATBM | 0:4013a9855dc8 | 877 | led2 = 1; |
TUATBM | 0:4013a9855dc8 | 878 | |
TUATBM | 0:4013a9855dc8 | 879 | if(!CheckSW_up(7)&&!CheckSW_up(8)){ |
TUATBM | 0:4013a9855dc8 | 880 | OperationMode=AUTOLOOP; |
TUATBM | 0:4013a9855dc8 | 881 | pc.printf("go to autoloop\r\n"); |
TUATBM | 0:4013a9855dc8 | 882 | } |
TUATBM | 0:4013a9855dc8 | 883 | break; |
TUATBM | 0:4013a9855dc8 | 884 | */ |
TUATBM | 0:4013a9855dc8 | 885 | } |
TUATBM | 0:4013a9855dc8 | 886 | |
TUATBM | 0:4013a9855dc8 | 887 | |
TUATBM | 0:4013a9855dc8 | 888 | |
TUATBM | 0:4013a9855dc8 | 889 | |
TUATBM | 0:4013a9855dc8 | 890 | /* |
TUATBM | 0:4013a9855dc8 | 891 | if(CheckSW_up(7)){ |
TUATBM | 0:4013a9855dc8 | 892 | pc.printf("true\r\n"); |
TUATBM | 0:4013a9855dc8 | 893 | Auto_loop(); |
TUATBM | 0:4013a9855dc8 | 894 | |
TUATBM | 0:4013a9855dc8 | 895 | }else{ |
TUATBM | 0:4013a9855dc8 | 896 | pc.printf("false\r\n"); |
TUATBM | 0:4013a9855dc8 | 897 | update_manual(); |
TUATBM | 0:4013a9855dc8 | 898 | }*/ |
TUATBM | 0:4013a9855dc8 | 899 | } |
TUATBM | 0:4013a9855dc8 | 900 | } |