フェイルセーフ完成版

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of Autoflight2018_53 by 航空研究会

Revision:
9:f6367b7fd7be
Parent:
8:66bba39b95a9
Child:
10:652071c20bf6
diff -r 66bba39b95a9 -r f6367b7fd7be main.cpp
--- a/main.cpp	Fri Sep 14 05:56:59 2018 +0000
+++ b/main.cpp	Sat Sep 15 06:14:03 2018 +0000
@@ -20,9 +20,9 @@
 #define DEBUG_SEMIAUTO 0
 #define DEBUG_PRINT_INLOOP 1
 
-#define KP_ELE 2.0
+#define KP_ELE 15.0  //2.0
 #define KI_ELE 0.0
-#define KD_ELE 0.0
+#define KD_ELE 0.0  //0/0
 #define KP_RUD 3.0
 #define KI_RUD 0.0
 #define KD_RUD 0.0
@@ -35,7 +35,7 @@
 #define GAIN_CONTROLVALUE_TO_PWM 3.0
 
 #define RIGHT_ROLL -12.0
-#define RIGHT_PITCH 5.0
+#define RIGHT_PITCH -10.0  //5.0
 #define LEFT_ROLL 12.0
 #define LEFT_PITCH -5.0
 #define STRAIGHT_ROLL 4.0
@@ -50,8 +50,9 @@
 #define LEFT_ROLL_SHORT 12.0
 #define LEFT_PITCH_SHORT -5.0
 
-#define rightloopRUD 1250
-#define AIL_L_correctionrightloop -200
+#define rightloopRUD 1300  //1250
+#define AIL_R_correctionrightloop 0
+#define AIL_L_correctionrightloop -400
 #define rightloopshortRUD 1250 
 #define AIL_L_correctionrightloopshort 0
 #define leftloopRUD  1500
@@ -344,7 +345,7 @@
     //Rotate(targetAngle, 30.0);
     CalculateControlValue(targetAngle, controlValue);
     UpdateAutoPWM(controlValue);
-    wait_ms(30);
+    wait_ms(25);
 #if DEBUG_PRINT_INLOOP
     DebugPrint();
 #endif
@@ -1215,12 +1216,13 @@
     targetAngle[ROLL] = g_rightloopROLL;
     targetAngle[PITCH] = g_rightloopPITCH ;
     autopwm[RUD]=rightloopRUD;              //RUD固定
-    autopwm[THR] = oldTHR;                  //手動スロットル記憶
+    autopwm[THR] = SetTHRinRatio(0.5);                  //手動スロットル記憶
     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;
+    
     //checkHeight(targetAngle);
     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
 }