フェイルセーフ完成版

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of Autoflight2018_53 by 航空研究会

Revision:
16:7fca5b938da6
Parent:
15:43427b0241d9
Child:
17:55249ea37dff
diff -r 43427b0241d9 -r 7fca5b938da6 main.cpp
--- a/main.cpp	Mon Sep 17 06:40:41 2018 +0000
+++ b/main.cpp	Mon Sep 17 08:58:08 2018 +0000
@@ -834,7 +834,12 @@
     pc.printf("rightloopROLLshort = %f, rightloopPITCHshort = %f\r\n", *g_rightloopROLLshort, *g_rightloopPITCHshort);
     pc.printf("leftloopROLLshort = %f, g_leftloopPITCHshort =  %f\r\n", *g_leftloopROLLshort, *g_leftloopPITCHshort);
     pc.printf("glideROLL = %f, glidePITCH = %f\r\n", *g_glideloopROLL, *g_glideloopPITCH);
-    
+    pc.printf("kpAIL = %f,kiAIL = %f,kdAIL = %f\r\n ",*g_kpAIL,*g_kiAIL,*g_kdAIL);
+    pc.printf("RIGHTLOOPRUD = %d,RIGHTLOOPSHORTRUD = %d\r\n",*g_rightloopRUD,*g_rightloopshortRUD);
+    pc.printf("LEFTTLOOPRUD = %d,LEFTLOOPSHORTRUD = %d\r\n",*g_leftloopRUD,*g_leftloopshortRUD);
+    pc.printf("GLIDELOOPRUD = %d\r\n",*g_glideloopRUD);
+    pc.printf("AIL_L_CORRECTION_RIGHTLOOP = %d, AIL_L_CORRECTION_RIGHTLOOPSHORT = %d\r\n",*g_AIL_L_correctionrightloop,*g_AIL_L_correctionrightloopshort);
+    pc.printf("AIL_L_CORRECTION_LEFTLOOP = %d, AIL_L_CORRECTION_LEFTLOOPSHORT = %d\r\n",*g_AIL_L_correctionleftloop,*g_AIL_L_correctionleftloopshort);
     return SDerrorcount;
 }                  
 
@@ -1099,6 +1104,7 @@
     targetAngle[ROLL] = g_glideloopROLL;
     targetAngle[PITCH] = g_glideloopPITCH;
     
+    autopwm[RUD]=g_glideloopRUD;
    // autopwm[THR]=oldTHR;
 
     
@@ -1363,7 +1369,7 @@
 
     targetAngle[ROLL] = g_rightloopROLL;
     targetAngle[PITCH] = g_rightloopPITCH ;
-    autopwm[RUD]=rightloopRUD;              //RUD固定
+    autopwm[RUD]=g_rightloopRUD;              //RUD固定
     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;
@@ -1380,7 +1386,7 @@
 
     targetAngle[ROLL] = g_rightloopROLL;
     targetAngle[PITCH] = g_rightloopPITCH ;
-    autopwm[RUD]=rightloopRUD;
+    autopwm[RUD]=g_rightloopRUD;
     autopwm[THR] = minpwm[THR];//SetTHRinRatio(g_loopTHR);
     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;
@@ -1395,7 +1401,7 @@
 
     targetAngle[ROLL] = g_rightloopROLLshort;
     targetAngle[PITCH] = g_rightloopPITCHshort;
-    autopwm[RUD]=rightloopshortRUD;
+    autopwm[RUD]=g_rightloopshortRUD;
     autopwm[THR] = oldTHR;
     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;
@@ -1410,7 +1416,7 @@
     
     targetAngle[ROLL] = g_leftloopROLL;
     targetAngle[PITCH] = g_leftloopPITCH;
-    autopwm[RUD]=leftloopRUD;
+    autopwm[RUD]=g_leftloopRUD;
     autopwm[THR] = oldTHR;
     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;
@@ -1426,7 +1432,7 @@
     
     targetAngle[ROLL] = g_leftloopROLL;
     targetAngle[PITCH] = g_leftloopPITCH;
-    autopwm[RUD]=leftloopRUD;
+    autopwm[RUD]=g_leftloopRUD;
     autopwm[THR] = minpwm[THR];
     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;
@@ -1442,7 +1448,7 @@
     
     targetAngle[ROLL] = g_leftloopROLLshort;
     targetAngle[PITCH] = g_leftloopPITCHshort;
-    autopwm[RUD]=leftloopRUD;
+    autopwm[RUD]=g_leftloopRUD;
     autopwm[THR] = oldTHR;
     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;