![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
割り込みテスト用(28は間違い)
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_27_test by
Diff: main.cpp
- Revision:
- 10:652071c20bf6
- Parent:
- 9:f6367b7fd7be
- Child:
- 11:c0b9ad25d3db
diff -r f6367b7fd7be -r 652071c20bf6 main.cpp --- a/main.cpp Sat Sep 15 06:14:03 2018 +0000 +++ b/main.cpp Sat Sep 15 15:34:20 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); @@ -167,9 +167,9 @@ 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; +char g_landingcommand='Z'; float g_SerialTargetYAW; Timer t; @@ -265,6 +265,8 @@ loop(); + + if(!CheckSW_Up(Ch7)){ led3=0; }else{ @@ -298,7 +300,7 @@ Init_sensors(); //switch2.rise(ResetTrim); pc.attach(getSF_Serial, Serial::RxIrq); - USsensor.attach(&UpdateDist, 0.05); + //USsensor.attach(&UpdateDist, 0.05); NVIC_SetPriority(USART1_IRQn,0); NVIC_SetPriority(EXTI0_IRQn,1); @@ -341,11 +343,16 @@ static float targetAngle[3] = {0.0, 0.0, 0.0}, controlValue[2] = {0.0, 0.0}; SensingMPU(); + NVIC_DisableIRQ(USART2_IRQn); UpdateTargetAngle(targetAngle); //Rotate(targetAngle, 30.0); CalculateControlValue(targetAngle, controlValue); + NVIC_DisableIRQ(USART1_IRQn); UpdateAutoPWM(controlValue); - wait_ms(25); + NVIC_EnableIRQ(USART1_IRQn); + NVIC_EnableIRQ(USART2_IRQn); + wait_ms(20); + pc.printf("%c",g_landingcommand); #if DEBUG_PRINT_INLOOP DebugPrint(); #endif @@ -796,6 +803,7 @@ static int16_t pwm[6]; static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4],trimpwm[5]}; if(sbus.flg_ch_update == true){ + NVIC_DisableIRQ(USART1_IRQn); switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替 case Manual: for(uint8_t i=0;i<6;i++){ @@ -814,8 +822,8 @@ pwm[RUD] = autopwm[RUD]; pwm[DROP] = autopwm[DROP]; pwm[AIL_L] = autopwm[AIL_L]; - pc.printf("%d ,",pwm[AIL_R]);//R - pc.printf("%d ,\r\n",pwm[AIL_L]);//L + //pc.printf("%d ,",pwm[AIL_R]);//R + //pc.printf("%d ,\r\n",pwm[AIL_L]);//L //pc.printf("update_auto\r\n"); break; @@ -825,6 +833,7 @@ pwm[i] = sbus.manualpwm[i]; } //pc.printf("update_manual\r\n"); break; + NVIC_EnableIRQ(USART1_IRQn); } for(uint8_t i=0;i<6;i++){ if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i]; @@ -841,12 +850,14 @@ //pwmをサーボに出力。 void Output_PWM(int16_t pwm[5]) { + NVIC_DisableIRQ(USART1_IRQn); servo1.pulsewidth_us(pwm[0]); servo2.pulsewidth_us(pwm[1]); servo3.pulsewidth_us(pwm[2]); servo4.pulsewidth_us(pwm[3]); servo5.pulsewidth_us(pwm[4]); servo6.pulsewidth_us(pwm[5]); + NVIC_EnableIRQ(USART1_IRQn); } @@ -868,8 +879,8 @@ NVIC_DisableIRQ(USART1_IRQn); NVIC_DisableIRQ(USART2_IRQn); NVIC_DisableIRQ(TIM5_IRQn); - NVIC_DisableIRQ(EXTI0_IRQn); - NVIC_DisableIRQ(EXTI9_5_IRQn); + NVIC_DisableIRQ(EXTI0_IRQn);//MPU割り込み禁止 + NVIC_DisableIRQ(EXTI9_5_IRQn);//超音波割り込み禁止 mpu6050.getRollPitchYaw_Skipper(rpy); @@ -911,6 +922,8 @@ static char SFbuf[16]; static int bufcounter=0; + pc.printf("%d",bufcounter); + SFbuf[bufcounter]=pc.getc(); if(SFbuf[0]=='S'&&bufcounter<5) bufcounter++; @@ -919,9 +932,9 @@ if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]); bufcounter = 0; memset(SFbuf, 0, strlen(SFbuf)); - pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); + //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); } - else if(bufcounter>=5 && g_buf[4]!='F'){ + else if(bufcounter>=5 ){ pc.printf("Communication Falsed.\r\n"); bufcounter = 0; } @@ -937,10 +950,10 @@ //超音波割り込み -void UpdateDist(){ +/*void UpdateDist(){ g_distance = usensor.get_dist_cm(); usensor.start(); -} +}*/ //8の字旋回 void UpdateTargetAngle_Moebius(float targetAngle[3]){ @@ -984,8 +997,7 @@ targetAngle[PITCH] = g_glideloopPITCH; // autopwm[THR]=oldTHR; - //シリアル通信受信の割り込みイベント登録 - pc.attach(ISR_Serial_Rx, Serial::RxIrq); + //時間計測開始設定 @@ -1003,11 +1015,14 @@ if(flg_tstart){ t_diff = t.read() - t_start; //一定高度or15秒でled点灯 + NVIC_DisableIRQ(EXTI9_5_IRQn); if((groundcount>5 && g_distance>0) || t_diff > 15){ led2 = 1; //pc.printf("Call [Stop!] calling!\r\n"); } + if(g_distance<180 && g_distance > 0) groundcount++; + NVIC_EnableIRQ(EXTI9_5_IRQn); }else{ t_diff = 0; groundcount = 0; @@ -1017,7 +1032,9 @@ 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); THRcount++; if(THRcount>5){ autopwm[THR] = SetTHRinRatio(0.6); @@ -1031,6 +1048,7 @@ } //離陸-投下-着陸一連 void Take_off_and_landing(float targetAngle[3]){ + /* if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff; switch(bombing_mode){ @@ -1055,10 +1073,10 @@ bombing_mode = Approach; } break; - /* - case Chicken: - break; - */ + + //case Chicken: + //break; + case Transition: static int ApproachCount = 0; targetAngle[YAW]=180.0; @@ -1069,13 +1087,17 @@ break; case Approach: + */ + autopwm[THR] = SetTHRinRatio(g_loopTHR); //0.7;スロットルの割合 UpdateTargetAngle_Approach(targetAngle); + /* break; default: bombing_mode = Takeoff; break; } + */ } //離陸モード @@ -1131,60 +1153,83 @@ void ReturnChickenServo2(){ autopwm[DROP] = 1392; pc.printf("second reverse\r\n"); -} - -//着陸モード(PCからの指令に従う) -void UpdateTargetAngle_Approach(float targetAngle[3]){ - pc.putc(g_buf[0]); - switch(g_buf[0]){ - case 'R': //右旋回セヨ - UpdateTargetAngle_Rightloop(targetAngle); - break; - - case 'L': //左旋回セヨ - UpdateTargetAngle_Leftloop(targetAngle); - break; - - case 'G': //直進セヨ - UpdateTargetAngle_GoStraight(targetAngle); - break; + } + + //着陸モード(PCからの指令に従う) + void UpdateTargetAngle_Approach(float targetAngle[3]){ + + NVIC_DisableIRQ(USART2_IRQn); + + switch(g_landingcommand){ + case 'R': //右旋回セヨ + targetAngle[ROLL] = g_rightloopROLL; + targetAngle[PITCH] = g_rightloopPITCH ; + autopwm[RUD]=rightloopRUD; //RUD固定 + if(autopwm[AIL_R]>trimpwm[AIL_R]){ //エルロン上がりやすさ調節 + autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; + } + else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; + autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop; - case 'Y': //指定ノヨー方向ニ移動セヨ - Rotate(targetAngle, g_SerialTargetYAW); - break; + break; + + case 'L': //左旋回セヨ + targetAngle[ROLL] = g_leftloopROLL; + targetAngle[PITCH] = g_leftloopPITCH; + autopwm[RUD]=leftloopRUD; + if(autopwm[AIL_R]>trimpwm[AIL_R]){ + autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; + } + else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; - case 'B': //ブザーヲ鳴ラセ - //buzzer = 1; - break; + break; + + case 'G': //直進セヨ + targetAngle[ROLL] = g_gostraightROLL; + targetAngle[PITCH] = g_gostraightPITCH; + + + break; + + case 'Y': //指定ノヨー方向ニ移動セヨ + Rotate(targetAngle, g_SerialTargetYAW); + break; + + case 'B': //ブザーヲ鳴ラセ + //buzzer = 1; + break; + + case 'D': //物資ヲ落トセ + Chicken_Drop(); + break; + + case 'C': //停止セヨ + targetAngle[ROLL] = 0.0; + targetAngle[PITCH] = -3.0; + autopwm[THR] = minpwm[THR]; + break; - case 'D': //物資ヲ落トセ - Chicken_Drop(); - break; - - case 'C': //停止セヨ - targetAngle[ROLL] = 0.0; - targetAngle[PITCH] = -3.0; - autopwm[THR] = minpwm[THR]; - break; + } + NVIC_EnableIRQ(USART2_IRQn); +} + +void checkHeight(float targetAngle[3]){ + + static int targetHeight = 200; + NVIC_DisableIRQ(EXTI9_5_IRQn); + if(g_distance < targetHeight + ALLOWHEIGHT){ + UpdateTargetAngle_NoseUP(targetAngle); + if(CheckSW_Up(Ch7)) led2 = 1; + } + else if(g_distance > targetHeight - ALLOWHEIGHT){ + UpdateTargetAngle_NoseDOWN(targetAngle); + if(CheckSW_Up(Ch7)) led2 = 1; + } + else led2=0; + NVIC_EnableIRQ(EXTI9_5_IRQn); } -} - -void checkHeight(float targetAngle[3]){ - static int targetHeight = 200; - - if(g_distance < targetHeight + ALLOWHEIGHT){ - UpdateTargetAngle_NoseUP(targetAngle); - if(CheckSW_Up(Ch7)) led2 = 1; - } - else if(g_distance > targetHeight - ALLOWHEIGHT){ - UpdateTargetAngle_NoseDOWN(targetAngle); - if(CheckSW_Up(Ch7)) led2 = 1; - } - else led2=0; -} - -void UpdateTargetAngle_NoseUP(float targetAngle[3]){ + void UpdateTargetAngle_NoseUP(float targetAngle[3]){ //targetAngle[PITCH] += 2.0f; //if(nowAngle[PITCH]<targetAngle[PITCH]) autopwm[THR] = SetTHRinRatio(0.6); @@ -1303,17 +1348,7 @@ //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); } -void ISR_Serial_Rx() -{ - // シリアルの受信処理 - char data = pc.getc(); - - if(data=='C'){ - autopwm[THR]=minpwm[2]; - wait(60.0); - } - -} + //デバッグ用 void DebugPrint(){ @@ -1329,10 +1364,10 @@ //pc.printf("\r\n"); //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //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\t",autopwm[AIL_L]); // pc.printf("%d\t",autopwm[RUD]); //pc.printf("%d",g_distance); //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