Diff: main.cpp

Revision:

3:9dbb6d18a090

Parent:

2:23daa5fa28b4

--- a/main.cpp	Sat Sep 08 05:32:55 2018 +0000
+++ b/main.cpp	Thu Sep 13 10:41:14 2018 +0000
@@ -35,7 +35,7 @@
 #define GAIN_CONTROLVALUE_TO_PWM 3.0
 
 #define RIGHT_ROLL -12.0
-#define RIGHT_PITCH -5.0
+#define RIGHT_PITCH 5.0
 #define LEFT_ROLL 12.0
 #define LEFT_PITCH -5.0
 #define STRAIGHT_ROLL 4.0
@@ -43,7 +43,7 @@
 #define TAKEOFF_THR 0.8
 #define LOOP_THR 0.6
 
-#define g_rightloopRUD 1500 
+//#define g_rightloopRUD 1500 
 
 #define RIGHT_ROLL_SHORT -12.0
 #define RIGHT_PITCH_SHORT -5.0
@@ -53,6 +53,19 @@
 #define GLIDE_ROLL -12.0
 #define GLIDE_PITCH -3.0
 
+#define rightloopRUD 1250 
+#define AIL_L_correctionrightloop 0
+#define rightloopshortRUD 1250 
+#define AIL_L_correctionrightloopshort 0
+#define leftloopRUD  1500
+#define AIL_L_correctionleftloop -0
+#define leftloopshortRUD 1500 
+#define AIL_L_correctionleftloopshort 0
+#define glideloopRUD 1300
+
+#define AIL_L_RatioRising 0.5
+#define AIL_L_RatioDescent 2
+
 //コンパスキャリブレーション
 //SkipperS2基板
 /*
@@ -86,7 +99,7 @@
 const float trim[4] = {Trim_Falfalla[0],Trim_Falfalla[1],Trim_Falfalla[2],Trim_Falfalla[3]};
 const float expMax[4] = {ExpMax_Falfalla[0],ExpMax_Falfalla[1],ExpMax_Falfalla[2],ExpMax_Falfalla[3]};
 const float expMin[4] = {ExpMin_Falfalla[0],ExpMin_Falfalla[1],ExpMin_Falfalla[2],ExpMin_Falfalla[3]};
-const int16_t reverce[4] = {ExpMin_Falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]};
+const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]};
 
 SBUS sbus(PA_9, PA_10); //SBUS
 
@@ -722,6 +735,9 @@
     //controlValue[ROLL] = pid_RUD.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt);
     controlValue[ROLL] = pid_AIL.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt);       //エルロンでロール制御   
     controlValue[PITCH] = pid_ELE.calcPID(nowAngle[PITCH], targetAngle[PITCH], dt);
+    
+    pc.printf("%f,%f",controlValue[ROLL],controlValue[PITCH]);
+    
 }
 
 void UpdateAutoPWM(float controlValue[3]){    
@@ -730,7 +746,7 @@
     addpwm[ROLL] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL];           //センサ：右回転正（8月13日時点；左回転が正！）     レバー:右回転正
     
     autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH];                 //rewrite
-    autopwm[AIL_R] = trimpwm[AIL_R] + reverce[AIL_R] * addpwm[AIL_R];
+    autopwm[AIL_R] = trimpwm[AIL_R] - reverce[AIL_R] * addpwm[ROLL];
     //autopwm[THR] = oldTHR;
 
     autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]);
@@ -789,8 +805,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;
@@ -1186,12 +1202,15 @@
 //右旋回
 void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回
 
-    autopwm[RUD]=g_rightloopRUD;
+    autopwm[RUD]=rightloopRUD;
     targetAngle[ROLL] = g_rightloopROLL;
     targetAngle[PITCH] = g_rightloopPITCH ;
     autopwm[THR] = oldTHR;//SetTHRinRatio(g_loopTHR);
-    autopwm[AIL_L]=trimpwm[AIL_L]+(autopwm[AIL_R]-trimpwm[AIL_R])*0.5;//*inverse;//g_AIL_L_Ratio_rightloop;
-    pc.printf("",)
+    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_L]=trimpwm[AIL_L]+(autopwm[AIL_R]-trimpwm[AIL_R])*0.5;//*inverse;//g_AIL_L_Ratio_rightloop;
     //checkHeight(targetAngle);
     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
 }
@@ -1251,7 +1270,7 @@
     //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[ELE]);  pc.printf("%d\t",autopwm[RUD]);
+    pc.printf("%d\t",autopwm[AIL_R]);  //pc.printf("%d\t",autopwm[RUD]);
     //pc.printf("%d",g_distance);
 
     //pc.printf("Mode: %c: ",g_buf[0]);