Diff: servo.cpp

Revision:

115:ceb9ef04bb26

Parent:

114:ba221936d53a

Child:

116:248f2d8cc11e

diff -r ba221936d53a -r ceb9ef04bb26 servo.cpp
--- a/servo.cpp	Wed Mar 23 11:55:07 2022 +0000
+++ b/servo.cpp	Thu Mar 24 08:51:44 2022 +0000
@@ -31,8 +31,10 @@
     }
     
     // 自身の位置に応じてエレベータ舵角を決定する
-    float derc =  rc[1];
-    float darc =  rc[0]-rc[1];
+    float derc =  -rc[1];
+    //pc.printf("derc: %f\r\n", derc);
+    float darc =  -rc[0] + rc[1];
+    //pc.printf("darc: %f\r\n", darc);
     deobj = 0.0f;
     switch(pos_tail)
     {
@@ -54,10 +56,31 @@
     
     
     float objgain =  15.0f*M_PI/180.0f;
-    float pitchobj = -objgain * (deobj+updateValues.de_command);
+    float pitchobj = objgain * (deobj + updateValues.de_command);
+    if (pitchobj > objgain)
+    {
+        pitchobj = objgain;
+    }
+    if (pitchobj < -objgain)
+    {
+        pitchobj = -objgain;
+    }
+    pc.printf("pitchobj: %f\r\n", pitchobj);
     if(rc[7] < -0.3f){
-        pitchPID.setGain(6.36f, 10.6f,0.0);
-        pitchratePID.setGain(0.9540f,0.0f,0.0f);
+        Matrix vihat = eskf.getVihat();
+        float vihat_norm = vihat(1, 1) * vihat(1, 1) + vihat(2, 1) * vihat(2, 1) + vihat(3, 1) * vihat(3, 1);
+        pc.printf("rc[2]: %f\r\n", rc[2]);
+        if (vihat_norm > 9.0f || rc[2] > -0.8f)
+        {
+            pitchPID.setGain(6.36f, 10.6f*0.5f,0.0);
+            pitchratePID.setGain(0.9540f,0.0f,0.0f);
+        }
+        else
+        {
+            pitchPID.setGain(6.36f, 0.0f,0.0);
+            pitchPID.resetIntError();
+            pitchratePID.setGain(0.9540f,0.0f,0.0f);
+        }
     }else{
         pitchPID.setGain(6.36f, 0.0f,0.0);
         pitchPID.resetIntError();