Diff: servo.cpp

Revision:

127:839ae718713b

Parent:

122:2e77adcf0c0d

diff -r 79e396942eb1 -r 839ae718713b servo.cpp
--- a/servo.cpp	Fri May 20 13:37:39 2022 +0000
+++ b/servo.cpp	Fri Jul 08 08:25:42 2022 +0000
@@ -57,33 +57,19 @@
     
     float objgain =  15.0f*M_PI/180.0f;
     float pitchobj = objgain * (deobj + updateValues.de_command);
-    if (pitchobj > objgain)
-    {
-        pitchobj = objgain;
-    }
-    if (pitchobj < -objgain)
-    {
-        pitchobj = -objgain;
-    }
     //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);
-    if (hilFlag == false){
-        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);
-
-        }
+    if (vihat_norm > 9.0f || rc[2] > -0.8f)
+    {
+        pitchPID.setGain(6.36f, 10.6f*0.1f,0.0f);
+        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);
+
     }
     
     yawratePID.setGain(2.0f,0.0f,0.0f);
@@ -110,7 +96,7 @@
     scaledMotorOut[0]= dTl;
     scaledMotorOut[1]= dTr;
     
-    float LP_servo = 0.2;
+    float LP_servo = 1.0;
     for(int i = 0; i < sizeof(servoOut)/sizeof(servoOut[0]); i++)
     {
         servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i];