a
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_38 by
Diff: main.cpp
- Revision:
- 31:dba3216c2755
- Parent:
- 30:624cb32e13a3
- Child:
- 32:9d0646372abe
--- a/main.cpp Sat Sep 22 06:59:37 2018 +0000 +++ b/main.cpp Sat Sep 22 09:46:42 2018 +0000 @@ -135,7 +135,7 @@ //InterruptIn switch2(PC_14); MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン,シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine -//HCSR04 usensor(PB_9,PB_8); //trig,echo 9,8 +HCSR04 usensor(PB_9,PB_8); //trig,echo 9,8 PID pid_AIL(g_kpAIL,g_kiAIL,g_kdAIL); PID pid_ELE(g_kpELE,g_kiELE,g_kdELE); @@ -181,7 +181,7 @@ float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0; unsigned int g_distance; -//Ticker USsensor; +Ticker USsensor; static char g_buf[16]; char g_landingcommand='Z'; float g_SerialTargetYAW; @@ -332,7 +332,7 @@ Init_sensors(); //switch2.rise(ResetTrim); - //USsensor.attach(&UpdateDist, 0.05); + USsensor.attach(&UpdateDist, 0.05); NVIC_SetPriority(USART1_IRQn,0); NVIC_SetPriority(EXTI0_IRQn,1); @@ -396,7 +396,7 @@ //NVIC_EnableIRQ(USART2_IRQn); #if DEBUG_PRINT_INLOOP //Sbusprintf(); - //DebugPrint(); + DebugPrint(); #endif } @@ -1109,10 +1109,10 @@ //超音波割り込み -/*void UpdateDist(){ +void UpdateDist(){ g_distance = usensor.get_dist_cm(); usensor.start(); -}*/ +} //8の字旋回 void UpdateTargetAngle_Moebius(float targetAngle[3]){ @@ -1149,6 +1149,7 @@ static int THRcount = 0; static int t_start = 0; static bool flg_tstart = false; + static bool flg_ground = false; int t_diff = 0; static int groundcount = 0; @@ -1182,8 +1183,8 @@ } if(g_distance<180 && g_distance > 0) { - groundcount++; - } + groundcount++; + } NVIC_EnableIRQ(EXTI9_5_IRQn); }else{ t_diff = 0; @@ -1191,23 +1192,27 @@ led2 = 0; } - if(t_diff > 17){ - autopwm[THR] = SetTHRinRatio(0.5); - }else{ NVIC_DisableIRQ(EXTI9_5_IRQn); - if(g_distance<150 && g_distance>0 ){ - NVIC_EnableIRQ(EXTI9_5_IRQn); + if(t_diff > 17) autopwm[THR] = SetTHRinRatio(0.5); + + else if(g_distance<150 && g_distance>0 ){ + NVIC_DisableIRQ(EXTI9_5_IRQn); THRcount++; - if(THRcount>5){ - autopwm[THR] = SetTHRinRatio(0.6); - //pc.printf("throttle ON\r\n"); + if(THRcount>5) flg_ground = true; + } + else THRcount = 0; + NVIC_EnableIRQ(EXTI9_5_IRQn); + + if(flg_ground == true) autopwm[THR] = SetTHRinRatio(0.6); + else autopwm[THR] = minpwm[THR]; + + NVIC_DisableIRQ(USART1_IRQn); + if(!CheckSW_Up(Ch7)){ + flg_ground = false; } - }else{ - autopwm[THR] = 1180; - THRcount = 0; - } - } + NVIC_EnableIRQ(USART1_IRQn); } + //離陸-投下-着陸一連 void Take_off_and_landing(float targetAngle[3]){ /* @@ -1599,8 +1604,10 @@ //for(uint8_t i=0; i<2; i++) pc.printf("%3.2f\t",nowAngle[i]); //ロール、ピッチのみ 小数点以下2ケタ //pc.printf("%d\t",autopwm[AIL_L]); // pc.printf("%d\t",autopwm[RUD]); //pc.printf("%d",g_distance); - + NVIC_DisableIRQ(EXTI9_5_IRQn); + pc.printf("g_distance = %d",g_distance); + NVIC_EnableIRQ(EXTI9_5_IRQn); //pc.printf("Mode: %c: ",g_buf[0]); //if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW); - //pc.printf("\r\n"); + pc.printf("\r\n"); } \ No newline at end of file