勇輝 関本 / Mbed 2 deprecated Estrela_v10

４項目一応成功

Dependencies:   mbed

Fork of Estrela_v01 by Bot Furukawa

main.cpp@9:182021b1df79, 2016-08-22 (annotated)

Committer:
motoseki
Date:
Mon Aug 22 09:03:15 2016 +0000
Revision:
9:182021b1df79
Parent:
8:887d448db359 
??????

Who changed what in which revision?

UserRevisionLine numberNew 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); //赤
motoseki 9:182021b1df79 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 7:13d98c4067c4 458 if(jj<=20){ //旋回し始め 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 9:182021b1df79 465 Auto_RUD = RUD_N + -70 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
motoseki 9:182021b1df79 466 Auto_ELE = ELE_N + -100 + int((-25 - 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 9:182021b1df79 491 Auto_RUD = RUD_N + -105 + int((-30-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); //roll -> gy 3.5
motoseki 9:182021b1df79 492 Auto_ELE = ELE_N + -75 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN); //pitch -> -gx
motoseki 9:182021b1df79 493 Auto_THR = oldTHL + 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 9:182021b1df79 497 Auto_RUD = RUD_N + -80 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
motoseki 9:182021b1df79 498 Auto_ELE = ELE_N + -110 + int((-28 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
motoseki 9:182021b1df79 499 Auto_THR = oldTHL;
TUATBM 0:4013a9855dc8 500 jj++;
TUATBM 0:4013a9855dc8 501 }else if((112<jj) && (jj<=117)){ //旋回遷移1
motoseki 9:182021b1df79 502 Auto_RUD = RUD_N + int((0 -roll)*RUDDER_GN - (gy-100)*RUD_DGN);
motoseki 9:182021b1df79 503 Auto_ELE = ELE_N + -30 + int((-10 - 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 9:182021b1df79 508 Auto_RUD = RUD_N + 105 + int((30-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
motoseki 9:182021b1df79 509 Auto_ELE = ELE_N + -75 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
motoseki 9:182021b1df79 510 Auto_THR = oldTHL - 20;
TUATBM 0:4013a9855dc8 511 jj++;
TUATBM 0:4013a9855dc8 512 }else{ //左旋回途中
motoseki 9:182021b1df79 513 Auto_RUD = RUD_N + 80 + int((23-roll)*RUDDER_GN - gy*RUD_DGN);
motoseki 9:182021b1df79 514 Auto_ELE = ELE_N + -110 + int((-28 - 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 9:182021b1df79 538 Auto_RUD = RUD_N + -105 + int((-30-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); //roll -> gy 3.5
motoseki 9:182021b1df79 539 Auto_ELE = ELE_N + -75 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN); //pitch -> -gx
motoseki 9:182021b1df79 540 Auto_THR = oldTHL + 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 9:182021b1df79 544 Auto_RUD = RUD_N + -80 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
motoseki 9:182021b1df79 545 Auto_ELE = ELE_N + -110 + int((-28 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
motoseki 9:182021b1df79 546 Auto_THR = oldTHL;
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 9:182021b1df79 555 Auto_RUD = RUD_N + -60 + int((-20-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
motoseki 9:182021b1df79 556 Auto_ELE = ELE_N + -140 + int((-30 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
motoseki 9:182021b1df79 557 Auto_THR = oldTHL + 220;
TUATBM 0:4013a9855dc8 558 jj++;
TUATBM 0:4013a9855dc8 559 }else{ //水平旋回
motoseki 9:182021b1df79 560 Auto_RUD = RUD_N + -80 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
motoseki 9:182021b1df79 561 Auto_ELE = ELE_N + -110 + int((-28 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
motoseki 9:182021b1df79 562 Auto_THR = oldTHL;
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 9:182021b1df79 591 Auto_RUD = RUD_N + -60 + int((-18-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
motoseki 9:182021b1df79 592 Auto_ELE = ELE_N + -110 + int((-24 - 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
motoseki 9:182021b1df79 610 void StabiGyro_Landing()
motoseki 9:182021b1df79 611 {
motoseki 9:182021b1df79 612 Auto_ELE = ELE_N + -80 + int((-20 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
motoseki 9:182021b1df79 613 //Auto_THR =
motoseki 9:182021b1df79 614 }
motoseki 9:182021b1df79 615
TUATBM 0:4013a9855dc8 616 void Auto_Loop()
TUATBM 0:4013a9855dc8 617 {
motoseki 4:ba610b05751d 618 //while(true){
TUATBM 3:8c01b4f33389 619 //for(k=0;k<100;++k){
motoseki 4:ba610b05751d 620 //sensing(); //センサーの値を読み込む(認知)
TUATBM 0:4013a9855dc8 621 StabiGyro(); //水平旋回のためのラダー、エレベーターのpwmを計算(判断)
TUATBM 0:4013a9855dc8 622 update_mode = UPD_AUTO; //オートモード
TUATBM 0:4013a9855dc8 623 //StabiAccel();
TUATBM 0:4013a9855dc8 624 //Auto_ELE+=145;
TUATBM 0:4013a9855dc8 625 //Auto_RUD-=80;
TUATBM 0:4013a9855dc8 626 //Servos.Auto2Servo();
TUATBM 0:4013a9855dc8 627 //Thread::wait(50);
motoseki 4:ba610b05751d 628 /*
motoseki 4:ba610b05751d 629 if(!CheckSW_up(7)){
motoseki 4:ba610b05751d 630 pc.printf("go to manual\n");
motoseki 4:ba610b05751d 631 break;
motoseki 4:ba610b05751d 632 }*/
motoseki 4:ba610b05751d 633 //wait_ms(50);
motoseki 4:ba610b05751d 634 //}
motoseki 4:ba610b05751d 635 //k=0; //for debug
motoseki 4:ba610b05751d 636 //pwm[6]=1000;
TUATBM 0:4013a9855dc8 637 }
TUATBM 0:4013a9855dc8 638
TUATBM 0:4013a9855dc8 639 void Auto_Mobius()
TUATBM 0:4013a9855dc8 640 {
TUATBM 0:4013a9855dc8 641 sensing();
TUATBM 0:4013a9855dc8 642 StabiGyro_Mobius();
TUATBM 0:4013a9855dc8 643 update_mode = UPD_AUTO;
TUATBM 0:4013a9855dc8 644 wait_ms(50);
TUATBM 0:4013a9855dc8 645 }
TUATBM 0:4013a9855dc8 646
TUATBM 0:4013a9855dc8 647 void Auto_Climb()
TUATBM 0:4013a9855dc8 648 {
TUATBM 0:4013a9855dc8 649 sensing();
TUATBM 0:4013a9855dc8 650 StabiGyro_Climb();
TUATBM 0:4013a9855dc8 651 update_mode = UPD_AUTO;
TUATBM 0:4013a9855dc8 652 wait_ms(50);
TUATBM 0:4013a9855dc8 653 }
TUATBM 0:4013a9855dc8 654
TUATBM 0:4013a9855dc8 655 void Auto_Glide()
TUATBM 0:4013a9855dc8 656 {
TUATBM 0:4013a9855dc8 657 sensing();
TUATBM 0:4013a9855dc8 658 StabiGyro_Glide();
TUATBM 0:4013a9855dc8 659 update_mode = UPD_AUTO;
TUATBM 0:4013a9855dc8 660 wait_ms(50);
TUATBM 0:4013a9855dc8 661 }
TUATBM 0:4013a9855dc8 662
TUATBM 0:4013a9855dc8 663 void Auto_Landing()
motoseki 9:182021b1df79 664 {
motoseki 9:182021b1df79 665 sensing();
motoseki 9:182021b1df79 666 StabiGyro_Landing();
motoseki 9:182021b1df79 667 update_mode = UPD_LANDING;
motoseki 9:182021b1df79 668 wait_ms(50);
TUATBM 0:4013a9855dc8 669 }
TUATBM 0:4013a9855dc8 670
TUATBM 0:4013a9855dc8 671 int main()
TUATBM 0:4013a9855dc8 672 {
TUATBM 0:4013a9855dc8 673 //SBus用バッファ初期化
TUATBM 0:4013a9855dc8 674 int i;
TUATBM 0:4013a9855dc8 675 for (i=0; i<25; i++) {buf_sbus[i] = 0;}
TUATBM 0:4013a9855dc8 676
TUATBM 0:4013a9855dc8 677 //pc.baud(115200); //パソコン通信用シリアルのボードレート
TUATBM 0:4013a9855dc8 678
TUATBM 0:4013a9855dc8 679 //frq.start();
TUATBM 0:4013a9855dc8 680 init_Servo();
TUATBM 0:4013a9855dc8 681 init_sbus();
TUATBM 0:4013a9855dc8 682 disp_clock();
TUATBM 0:4013a9855dc8 683
TUATBM 0:4013a9855dc8 684 //---9軸センサー(MPU9250)初期化---
TUATBM 0:4013a9855dc8 685
TUATBM 0:4013a9855dc8 686 //Set up I2C
TUATBM 0:4013a9855dc8 687 i2c.frequency(400000); // use fast (400 kHz) I2C
TUATBM 0:4013a9855dc8 688 pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
TUATBM 0:4013a9855dc8 689 t.start();
TUATBM 0:4013a9855dc8 690
TUATBM 0:4013a9855dc8 691 // Read the WHO_AM_I register, this is a good test of communication
TUATBM 0:4013a9855dc8 692 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
TUATBM 0:4013a9855dc8 693 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
TUATBM 0:4013a9855dc8 694
TUATBM 0:4013a9855dc8 695 if (whoami == 0x71){ // WHO_AM_I should always be 0x68
TUATBM 0:4013a9855dc8 696 pc.printf("MPU9250 is online...\n\r");
TUATBM 1:fc5988ebfa53 697 //wait(1);
TUATBM 0:4013a9855dc8 698
TUATBM 0:4013a9855dc8 699 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
TUATBM 0:4013a9855dc8 700 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
TUATBM 0:4013a9855dc8 701 //pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
TUATBM 0:4013a9855dc8 702 //pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
TUATBM 0:4013a9855dc8 703 //pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
TUATBM 0:4013a9855dc8 704 //pc.printf("x accel bias = %f\n\r", accelBias[0]);
TUATBM 0:4013a9855dc8 705 //pc.printf("y accel bias = %f\n\r", accelBias[1]);
TUATBM 0:4013a9855dc8 706 //pc.printf("z accel bias = %f\n\r", accelBias[2]);
TUATBM 1:fc5988ebfa53 707 wait(1);
TUATBM 0:4013a9855dc8 708
TUATBM 0:4013a9855dc8 709 mpu9250.initMPU9250();
TUATBM 0:4013a9855dc8 710 pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
TUATBM 0:4013a9855dc8 711 mpu9250.initAK8963(magCalibration);
TUATBM 0:4013a9855dc8 712 pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
TUATBM 0:4013a9855dc8 713 pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
TUATBM 0:4013a9855dc8 714 pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
TUATBM 0:4013a9855dc8 715 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
TUATBM 0:4013a9855dc8 716 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
TUATBM 0:4013a9855dc8 717 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
TUATBM 0:4013a9855dc8 718 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
TUATBM 0:4013a9855dc8 719 wait(2);
TUATBM 0:4013a9855dc8 720
TUATBM 0:4013a9855dc8 721 }else{
TUATBM 0:4013a9855dc8 722 pc.printf("Could not connect to MPU9250: \n\r");
TUATBM 0:4013a9855dc8 723 pc.printf("%#x \n", whoami);
TUATBM 0:4013a9855dc8 724 while(1) ; // Loop forever if communication doesn't happen
TUATBM 0:4013a9855dc8 725 }
TUATBM 0:4013a9855dc8 726
TUATBM 0:4013a9855dc8 727 mpu9250.getAres(); // Get accelerometer sensitivity
TUATBM 0:4013a9855dc8 728 mpu9250.getGres(); // Get gyro sensitivity
TUATBM 0:4013a9855dc8 729 mpu9250.getMres(); // Get magnetometer sensitivity
TUATBM 0:4013a9855dc8 730 //pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
TUATBM 0:4013a9855dc8 731 //pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
TUATBM 0:4013a9855dc8 732 //pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
TUATBM 3:8c01b4f33389 733 magbias[0] = MAGBIASX; //+470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
TUATBM 3:8c01b4f33389 734 magbias[1] = MAGBIASY; //+120.; // User environmental y-axis correction in milliGauss
TUATBM 3:8c01b4f33389 735 magbias[2] = MAGBIASZ; //+125.; // User environmental z-axis correction in milliGauss
TUATBM 0:4013a9855dc8 736
TUATBM 3:8c01b4f33389 737 t.start();
TUATBM 0:4013a9855dc8 738 pc.printf("mpu9250 initialized\r\n");
TUATBM 0:4013a9855dc8 739
TUATBM 0:4013a9855dc8 740 //---9軸センサー(MPU9250)初期化終わり---
TUATBM 0:4013a9855dc8 741
TUATBM 0:4013a9855dc8 742 led1 = 0;
TUATBM 0:4013a9855dc8 743 led2 = 0;
TUATBM 1:fc5988ebfa53 744 led3 = 0;
TUATBM 1:fc5988ebfa53 745 led4 = 0;
TUATBM 0:4013a9855dc8 746
TUATBM 0:4013a9855dc8 747 pc.printf("All initialized\r\n");
TUATBM 0:4013a9855dc8 748
TUATBM 3:8c01b4f33389 749
TUATBM 0:4013a9855dc8 750 while (true) { //無限ループ
TUATBM 0:4013a9855dc8 751 //for(i=0;i<8;i++){
TUATBM 0:4013a9855dc8 752 //pc.printf("%x ", buf_sbus[i]);
TUATBM 0:4013a9855dc8 753 //pc.printf("ch%d=%d ", i+1, channels[i]);
TUATBM 0:4013a9855dc8 754 //}
TUATBM 0:4013a9855dc8 755 //pc.printf("\n");
motoseki 4:ba610b05751d 756 sensing();
motoseki 4:ba610b05751d 757 wait_ms(50);
TUATBM 0:4013a9855dc8 758
TUATBM 0:4013a9855dc8 759 switch(OperationMode){ //変数OperationModeの値で場合分け
TUATBM 0:4013a9855dc8 760 case GROUNDCHECK: //グラウンドチェック(スイッチが2つ初期値にちゃんとなってたら水平旋回モードに移行)
TUATBM 0:4013a9855dc8 761 if(!CheckSW_up(7)&&!CheckSW_up(8)){ //チャンネル7,8が両方offなら
TUATBM 0:4013a9855dc8 762 OperationMode=AUTOLOOP; //変数OperationModeにAUTOLOOP(定数なので2(SkipperS.hで定義))を代入
TUATBM 0:4013a9855dc8 763 pc.printf("go to autoloop\r\n");
motoseki 9:182021b1df79 764 }/*else if(CheckSW_up(7)&&!CheckSW_up(8)){ //チャンネル７だけonなら
motoseki 9:182021b1df79 765 CalibrateCompass();
motoseki 9:182021b1df79 766 }*/
TUATBM 0:4013a9855dc8 767 //変数update_modeはSBus(=プロポ＝操縦者)からのpwmをそのまま垂れ流すか、自動制御で計算したpwmをサーボに流すかを切り替えるための変数
TUATBM 0:4013a9855dc8 768 update_mode = UPD_MANUAL; //マニュアルモード(そのまま垂れ流す)
motoseki 4:ba610b05751d 769 // pc.printf("groundcheck mode\r\n");
TUATBM 0:4013a9855dc8 770
TUATBM 0:4013a9855dc8 771 break;
TUATBM 0:4013a9855dc8 772 case AUTOLOOP: //水平旋回モード
motoseki 4:ba610b05751d 773 //t.reset();
TUATBM 0:4013a9855dc8 774 if(CheckSW_up(7)){ //チャンネル7がonなら
motoseki 4:ba610b05751d 775 //t.reset();
motoseki 4:ba610b05751d 776 Auto_Loop(); //関数Auto_Loop実行
TUATBM 0:4013a9855dc8 777 led1 = 0;
TUATBM 0:4013a9855dc8 778 led2 = 1;
motoseki 4:ba610b05751d 779 //Auto_Loop();
TUATBM 3:8c01b4f33389 780 //pc.printf("Auto Loop Now\r\n");
TUATBM 0:4013a9855dc8 781 //autoloop.set_priority(osPriorityAboveNormal);
TUATBM 0:4013a9855dc8 782 /*
TUATBM 0:4013a9855dc8 783 while(true){
TUATBM 0:4013a9855dc8 784 if(!CheckSW_up(7)){
TUATBM 0:4013a9855dc8 785 autoloop.terminate();
TUATBM 0:4013a9855dc8 786 break;
TUATBM 0:4013a9855dc8 787 }
TUATBM 0:4013a9855dc8 788 }
TUATBM 0:4013a9855dc8 789 */
TUATBM 0:4013a9855dc8 790 }else{
TUATBM 0:4013a9855dc8 791 update_mode = UPD_MANUAL;
motoseki 4:ba610b05751d 792 //pc.printf("manual Now\r\n");
TUATBM 0:4013a9855dc8 793 led1 = 1;
TUATBM 0:4013a9855dc8 794 led2 = 0;
TUATBM 3:8c01b4f33389 795
motoseki 4:ba610b05751d 796 /*debug*/
motoseki 4:ba610b05751d 797 //k++;
motoseki 4:ba610b05751d 798 //wait(50);
motoseki 4:ba610b05751d 799 //if(k>100) pwm[6]=2000;
TUATBM 0:4013a9855dc8 800 }
TUATBM 0:4013a9855dc8 801
TUATBM 0:4013a9855dc8 802 if(!CheckSW_up(7)&&CheckSW_up(8)){ //チャンネル7がoff、8がonなら
TUATBM 0:4013a9855dc8 803 OperationMode=AUTOMOBIUS;
TUATBM 0:4013a9855dc8 804 pc.printf("go to auto8\r\n");
TUATBM 0:4013a9855dc8 805 }
TUATBM 0:4013a9855dc8 806 break;
TUATBM 0:4013a9855dc8 807 case AUTOMOBIUS: //8の字旋回モード
TUATBM 0:4013a9855dc8 808 if(CheckSW_up(7)){
TUATBM 0:4013a9855dc8 809 Auto_Mobius();
TUATBM 0:4013a9855dc8 810 led1 = 0;
TUATBM 0:4013a9855dc8 811 led2 = 1;
TUATBM 0:4013a9855dc8 812 pc.printf("Auto Mobius Now\r\n");
TUATBM 0:4013a9855dc8 813 }else{
TUATBM 0:4013a9855dc8 814 update_mode = UPD_MANUAL;
TUATBM 0:4013a9855dc8 815 pc.printf("manual Now\r\n");
TUATBM 0:4013a9855dc8 816 led1 = 1;
TUATBM 0:4013a9855dc8 817 led2 = 1;
TUATBM 0:4013a9855dc8 818 }
TUATBM 0:4013a9855dc8 819
TUATBM 0:4013a9855dc8 820 if(!CheckSW_up(7)&&!CheckSW_up(8)){
TUATBM 0:4013a9855dc8 821 OperationMode=AUTOCLIMB;
TUATBM 0:4013a9855dc8 822 pc.printf("go to autoclimb\r\n");
TUATBM 0:4013a9855dc8 823 }
TUATBM 0:4013a9855dc8 824 break;
TUATBM 0:4013a9855dc8 825 case AUTOCLIMB: //上昇旋回モード
TUATBM 0:4013a9855dc8 826 if(CheckSW_up(7)){
TUATBM 0:4013a9855dc8 827 Auto_Climb();
TUATBM 0:4013a9855dc8 828 led1 = 0;
TUATBM 0:4013a9855dc8 829 led2 = 1;
TUATBM 0:4013a9855dc8 830 pc.printf("Auto Climb Now\r\n");
TUATBM 0:4013a9855dc8 831 }else{
TUATBM 0:4013a9855dc8 832 update_mode = UPD_MANUAL;
TUATBM 0:4013a9855dc8 833 pc.printf("manual Now\r\n");
TUATBM 0:4013a9855dc8 834 led1 = 1;
TUATBM 0:4013a9855dc8 835 led2 = 0;
TUATBM 0:4013a9855dc8 836 }
TUATBM 0:4013a9855dc8 837
TUATBM 0:4013a9855dc8 838 if(!CheckSW_up(7)&&CheckSW_up(8)){
TUATBM 0:4013a9855dc8 839 OperationMode=AUTOGLIDE;
TUATBM 0:4013a9855dc8 840 pc.printf("go to manual\r\n");
TUATBM 0:4013a9855dc8 841 }
TUATBM 0:4013a9855dc8 842 break;
TUATBM 0:4013a9855dc8 843
TUATBM 0:4013a9855dc8 844 case AUTOGLIDE: //自動滑空モード
TUATBM 0:4013a9855dc8 845 if(CheckSW_up(7)){
TUATBM 0:4013a9855dc8 846 Auto_Glide();
TUATBM 0:4013a9855dc8 847 led1 = 0;
TUATBM 0:4013a9855dc8 848 led2 = 1;
TUATBM 0:4013a9855dc8 849 pc.printf("Auto Glide Now\r\n");
TUATBM 0:4013a9855dc8 850 }else{
TUATBM 0:4013a9855dc8 851 update_mode = UPD_MANUAL;
TUATBM 0:4013a9855dc8 852 pc.printf("manual Now\r\n");
TUATBM 0:4013a9855dc8 853 led1 = 1;
TUATBM 0:4013a9855dc8 854 led2 = 1;
TUATBM 0:4013a9855dc8 855 }
TUATBM 0:4013a9855dc8 856
TUATBM 0:4013a9855dc8 857 if(!CheckSW_up(7)&&!CheckSW_up(8)){
motoseki 6:13bb74a57a33 858 OperationMode=AUTOLANDING;
TUATBM 0:4013a9855dc8 859 pc.printf("go to autoloop\r\n");
TUATBM 0:4013a9855dc8 860 }
TUATBM 0:4013a9855dc8 861 break;
motoseki 6:13bb74a57a33 862
TUATBM 0:4013a9855dc8 863 case AUTOLANDING:
motoseki 6:13bb74a57a33 864 led3 = 1; //赤外線
TUATBM 0:4013a9855dc8 865 if(CheckSW_up(7)){
TUATBM 0:4013a9855dc8 866 Auto_Landing();
TUATBM 0:4013a9855dc8 867 led1 = 0;
TUATBM 0:4013a9855dc8 868 led2 = 1;
TUATBM 0:4013a9855dc8 869 pc.printf("Auto Landing Now\r\n");
TUATBM 0:4013a9855dc8 870 }else{
TUATBM 0:4013a9855dc8 871 update_mode = UPD_MANUAL;
TUATBM 0:4013a9855dc8 872 pc.printf("manual Now\r\n");
TUATBM 0:4013a9855dc8 873 led1 = 1;
TUATBM 0:4013a9855dc8 874 led2 = 0;
TUATBM 0:4013a9855dc8 875 }
TUATBM 0:4013a9855dc8 876
TUATBM 0:4013a9855dc8 877 if(!CheckSW_up(7)&&CheckSW_up(8)){
motoseki 6:13bb74a57a33 878 //OperationMode=MANUALMODE;
motoseki 6:13bb74a57a33 879 //pc.printf("go to manualmade\r\n");
TUATBM 0:4013a9855dc8 880 }
TUATBM 0:4013a9855dc8 881 break;
motoseki 6:13bb74a57a33 882 /*
TUATBM 0:4013a9855dc8 883 case MANUALMODE:
TUATBM 0:4013a9855dc8 884 update_mode = UPD_MANUAL;
TUATBM 0:4013a9855dc8 885 led1 = 1;
TUATBM 0:4013a9855dc8 886 led2 = 1;
TUATBM 0:4013a9855dc8 887
TUATBM 0:4013a9855dc8 888 if(!CheckSW_up(7)&&!CheckSW_up(8)){
TUATBM 0:4013a9855dc8 889 OperationMode=AUTOLOOP;
TUATBM 0:4013a9855dc8 890 pc.printf("go to autoloop\r\n");
TUATBM 0:4013a9855dc8 891 }
TUATBM 0:4013a9855dc8 892 break;
TUATBM 0:4013a9855dc8 893 */
TUATBM 0:4013a9855dc8 894 }
TUATBM 0:4013a9855dc8 895
TUATBM 0:4013a9855dc8 896
TUATBM 0:4013a9855dc8 897
TUATBM 0:4013a9855dc8 898
TUATBM 0:4013a9855dc8 899 /*
TUATBM 0:4013a9855dc8 900 if(CheckSW_up(7)){
TUATBM 0:4013a9855dc8 901 pc.printf("true\r\n");
TUATBM 0:4013a9855dc8 902 Auto_loop();
TUATBM 0:4013a9855dc8 903
TUATBM 0:4013a9855dc8 904 }else{
TUATBM 0:4013a9855dc8 905 pc.printf("false\r\n");
TUATBM 0:4013a9855dc8 906 update_manual();
TUATBM 0:4013a9855dc8 907 }*/
TUATBM 0:4013a9855dc8 908 }
TUATBM 0:4013a9855dc8 909 }