Diff: main.cpp

Revision:

42:74d72339a8a8

Parent:

41:3b8c250ae79c

Child:

43:4413ee61bc39

--- a/main.cpp	Wed Sep 26 04:13:36 2018 +0000
+++ b/main.cpp	Wed Sep 26 04:35:47 2018 +0000
@@ -51,7 +51,8 @@
 #define LEFT_PITCH_SHORT -5.0
 #define RIGHTLOOPROLL_APPROACH -17.0
 #define LEFTLOOPROLL_APPROACH 16.0
-
+#define RIGHTLOOPPITCH_APPROACH -15.0
+#define LEFTLOOPPITCH_APPROACH -13.0
 
 #define rightloopROLL2 -10.0
 
@@ -157,7 +158,7 @@
 OperationMode operation_mode = StartUp;
 BombingMode bombing_mode = Takeoff;
 
-static int16_t autopwm[8] = {1455,1450,1176,1628,1512,1452};
+static int16_t autopwm[8] = {1455,1450,1176,1628,1417,1452};
 /*
 //1号機
 static int16_t trimpwm[6] = {1580,1600,1176,1404,1440,1448};
@@ -168,8 +169,8 @@
 
 //2号機
 
-static int16_t trimpwm[6] = {1455,1450,1176,1628,1512,1452};
-int16_t maxpwm[6] = {1672,1786,1848,1964,1820,1860};
+static int16_t trimpwm[6] = {1455,1450,1176,1628,1417,1452};
+int16_t maxpwm[6] = {1672,1786,1848,1964,1712,1860};
 int16_t minpwm[6] = {1057,1115,1176,1292,1180,1180};
 const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]};
 
@@ -248,7 +249,8 @@
                int *g_leftloopRUD, int *g_leftloopshortRUD,
                int *g_glideRUD,
                float *g_rightloopROLL_approach,float *g_leftloopROLL_approach,
-               int *g_rightloopRUD_approach,int *g_leftloopRUD_approach
+               int *g_rightloopRUD_approach,int *g_leftloopRUD_approach,
+               float *g_rightloopPITCH_approach,float *g_leftloopPITCH_approach
                );
 //switch2割り込み
 void ResetTrim();
@@ -328,7 +330,8 @@
                &g_leftloopRUD, &g_leftloopshortRUD,
                &g_glideloopRUD,
                &g_rightloopROLL_approach,&g_leftloopROLL_approach,
-               &g_rightloopRUD_approach,&g_leftloopRUD_approach
+               &g_rightloopRUD_approach,&g_leftloopRUD_approach,
+               &g_rightloopPITCH_approach,&g_leftloopPITCH_approach
                );    
     
         
@@ -642,7 +645,8 @@
                int *g_leftloopRUD, int *g_leftloopshortRUD,
                int *g_glideloopRUD,
                float *g_rightloopROLL_approach,float *g_leftloopROLL_approach,
-               int *g_rightloopRUD_approach,int *g_leftloopRUD_approach
+               int *g_rightloopRUD_approach,int *g_leftloopRUD_approach,
+               float *g_rightloopPITCH_approach,float *g_leftloopPITCH_approach
                ){
 
     pc.printf("SDsetup start.\r\n");    
@@ -682,7 +686,9 @@
         "RIGHTLOOP_ROLL_APPROACH",        
         "LEFTLOOP_ROLL_APPROACH",        
         "RIGHTLOOP_RUDDER_APPROACH",        
-        "LEFTLOOP_RUDDER_APPROACH"            
+        "LEFTLOOP_RUDDER_APPROACH",
+        "RIGHTLOOP_PITCH_APPROACH",
+        "LEFTLOOP_PITCH_APPROACH",            
     };
 
     fp = fopen("/sd/option.txt","r");
@@ -817,6 +823,14 @@
         else{                                         *g_leftloopRUD= LEFTLOOPRUD_APPROACH;
                                                       SDerrorcount++;
         }
+        if(GetParameter(fp,paramNames[32],parameter)) *g_rightloopPITCH_approach = atof(parameter);
+        else{                                         *g_rightloopPITCH_approach = RIGHTLOOPPITCH_APPROACH;
+                                                      SDerrorcount++;
+        }
+        if(GetParameter(fp,paramNames[33],parameter)) *g_leftloopPITCH_approach = atof(parameter);
+        else{                                         *g_leftloopPITCH_approach = LEFTLOOPPITCH_APPROACH;
+                                                      SDerrorcount++;
+        }
         
         fclose(fp);
 
@@ -848,7 +862,8 @@
         *g_leftloopROLL_approach = LEFTLOOPROLL_APPROACH;
         *g_rightloopRUD_approach = RIGHTLOOPRUD_APPROACH;
         *g_leftloopRUD_approach = LEFTLOOPRUD_APPROACH;
-        
+        *g_rightloopPITCH_approach = RIGHTLOOPPITCH_APPROACH;
+        *g_leftloopPITCH_approach = LEFTLOOPPITCH_APPROACH;
         
         SDerrorcount = -1;
     }
@@ -872,6 +887,8 @@
     pc.printf("GLIDELOOPRUD = %d\r\n",*g_glideloopRUD);
     pc.printf("RIGHTLOOP_ROLL_APPROACH = %f, LEFTLOOP_ROLL_APPROACH= %f\r\n",*g_rightloopROLL_approach,*g_leftloopROLL_approach);
     pc.printf("RIGHTLOOP_RUD_APPROACH = %d, LEFTLOOP_RUD_APPROACH = %d\r\n",*g_rightloopRUD_approach,*g_leftloopRUD_approach);
+    pc.printf("RIGHTLOOP_PITCH_APPROACH = %f, LEFTLOOP_PITCH_APPROACH = %f\r\n",*g_rightloopPITCH_approach,*g_leftloopPITCH_approach);
+    
     return SDerrorcount;
 }                  
 
@@ -1374,7 +1391,7 @@
 //チキラー投下
 void Chicken_Drop(){
     if(CheckSW_Up(Ch7)){
-        autopwm[DROP] = 1911;
+        autopwm[DROP] = 1712;
         pc.printf("Bombed!\r\n");
         //RerurnChickenServo1.attach(&ReturnChickenServo1, 3);
         //operation_mode = Approach;
@@ -1686,10 +1703,10 @@
     //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);
+    //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