確認用
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_29 by
Diff: main.cpp
- Revision:
- 19:0955311b0db6
- Parent:
- 18:cce82f3374fc
- Child:
- 20:9393b0cfa44d
diff -r cce82f3374fc -r 0955311b0db6 main.cpp --- a/main.cpp Mon Sep 17 12:01:13 2018 +0000 +++ b/main.cpp Mon Sep 17 13:11:53 2018 +0000 @@ -62,6 +62,7 @@ #define AIL_L_correctionleftloop -0 #define AIL_L_correctionleftloopshort 0 + #define RIGHTLOOP_RUD 1250 #define RIGHTLOOPSHORT_RUD 1250 #define LEFTLOOP_RUD 1500 @@ -168,6 +169,8 @@ int16_t g_AIL_L_Ratio_rightloop = 0.5; +int zeroTHR=1;//着陸時のスロットルが0かの判断用 + static float nowAngle[3] = {0,0,0}; const float trimAngle[3] = {0.0, 0.0, 0.0}; @@ -244,10 +247,13 @@ //自動操縦 void UpdateTargetAngle_GoStraight(float targetAngle[3]); +void UpdateTargetAngle_GoStraight_zero(float targetAngle[3]); //着陸時にスロットルが0の時の直進 void UpdateTargetAngle_Rightloop(float targetAngle[3]); void UpdateTargetAngle_Rightloop_short(float targetAngle[3]); +void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]); //着陸時にスロットルが0の時の右旋回 void UpdateTargetAngle_Leftloop(float targetAngle[3]); void UpdateTargetAngle_Leftloop_short(float targetAngle[3]); +void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]); //着陸時にスロットルが0の時の左旋回 void UpdateTargetAngle_Moebius(float targetAngle[3]); void UpdateTargetAngle_Glide(float targetAngle[3]); void UpdateTargetAngle_Takeoff(float targetAngle[3]); @@ -1275,6 +1281,10 @@ switch(g_landingcommand){ case 'R': //右旋回セヨ + if(zeroTHR==0){ + UpdateTargetAngle_Rightloop_zero(targetAngle); + } + else{ targetAngle[ROLL] = g_rightloopROLL; targetAngle[PITCH] = g_rightloopPITCH ; autopwm[RUD]=g_rightloopRUD; //RUD固定 @@ -1285,8 +1295,12 @@ autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop; break; - + } case 'L': //左旋回セヨ + if(zeroTHR==0){ + UpdateTargetAngle_Leftloop_zero(targetAngle); + } + else{ targetAngle[ROLL] = g_leftloopROLL; targetAngle[PITCH] = g_leftloopPITCH; autopwm[RUD]=g_leftloopRUD; @@ -1294,13 +1308,17 @@ 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; - + } break; case 'G': //直進セヨ + if(zeroTHR==0){ + UpdateTargetAngle_GoStraight_zero(targetAngle); + } + else{ targetAngle[ROLL] = g_gostraightROLL; targetAngle[PITCH] = g_gostraightPITCH; - + } break; @@ -1320,6 +1338,7 @@ targetAngle[ROLL] = 0.0; targetAngle[PITCH] = -3.0; autopwm[THR] = minpwm[THR]; + zeroTHR=0; break; } @@ -1360,6 +1379,7 @@ //直進 void UpdateTargetAngle_GoStraight(float targetAngle[3]){ + autopwm[RUD] = trimpwm[RUD]; targetAngle[ROLL] = g_gostraightROLL; targetAngle[PITCH] = g_gostraightPITCH; autopwm[THR] = SetTHRinRatio(g_loopTHR); @@ -1367,6 +1387,16 @@ //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); } +//直進(着陸時スロットル0のとき) +void UpdateTargetAngle_GoStraight_zero(float targetAngle[3]){ + + autopwm[RUD] = trimpwm[RUD]; + targetAngle[ROLL] = g_gostraightROLL; + targetAngle[PITCH] = g_gostraightPITCH; + autopwm[THR] = minpwm[THR]; + + //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); +} //右旋回 void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回 @@ -1406,7 +1436,7 @@ targetAngle[ROLL] = g_rightloopROLLshort; targetAngle[PITCH] = g_rightloopPITCHshort; autopwm[RUD]=g_rightloopshortRUD; - autopwm[THR] = oldTHR; + autopwm[THR] = SetTHRinRatio(0.5); if(autopwm[AIL_R]>trimpwm[AIL_R]){ autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; } @@ -1421,7 +1451,7 @@ targetAngle[ROLL] = g_leftloopROLL; targetAngle[PITCH] = g_leftloopPITCH; autopwm[RUD]=g_leftloopRUD; - autopwm[THR] = oldTHR; + autopwm[THR] = SetTHRinRatio(0.5); if(autopwm[AIL_R]>trimpwm[AIL_R]){ autopwm[AIL_L]=trimpwm[AIL_L]+g_AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; } @@ -1453,7 +1483,7 @@ targetAngle[ROLL] = g_leftloopROLLshort; targetAngle[PITCH] = g_leftloopPITCHshort; autopwm[RUD]=g_leftloopRUD; - autopwm[THR] = oldTHR; + autopwm[THR] = SetTHRinRatio(0.5); if(autopwm[AIL_R]>trimpwm[AIL_R]){ autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; }