Diff: servo.cpp

Revision:

116:248f2d8cc11e

Parent:

115:ceb9ef04bb26

Child:

117:8cedc59454c4

diff -r ceb9ef04bb26 -r 248f2d8cc11e servo.cpp
--- a/servo.cpp	Thu Mar 24 08:51:44 2022 +0000
+++ b/servo.cpp	Mon Mar 28 09:11:55 2022 +0000
@@ -65,27 +65,14 @@
     {
         pitchobj = -objgain;
     }
-    pc.printf("pitchobj: %f\r\n", pitchobj);
-    if(rc[7] < -0.3f){
-        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();
-        pitchratePID.setGain(0.9540f,0.0f,0.0f);
-    }
+    //pc.printf("pitchobj: %f\r\n", pitchobj);
+    
+    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]);  
+    pitchPID.setGain(6.36f, 10.6f*0.5f,0.0);
+    pitchratePID.setGain(0.9540f,0.0f,0.0f);
+    
     yawratePID.setGain(2.0f,0.0f,0.0f);
     pitchPID.setProcessValue(rpy.y);
     pitchratePID.setProcessValue((gyro.y-gyroBias(2,1)));
@@ -96,7 +83,7 @@
     de = pitchPID.compute()+pitchratePID.compute();
     if(de<-1.0f){de = -1.0f;}
     if(de>1.0f){de = 1.0f;}
-    pc.printf("%f %f %f %f %f\r\n",pitchobj,deobj,de,drobj,dr);
+    //pc.printf("%f %f %f %f %f\r\n",pitchobj,deobj,de,drobj,dr);
     
     dr = yawratePID.compute()+drobj;
     if(dr<-1.0f){dr = -1.0f;}