Diff: servo.cpp

Revision:

118:97ffe77b6f38

Parent:

117:8cedc59454c4

Child:

119:43ac44c68ff0

diff -r 8cedc59454c4 -r 97ffe77b6f38 servo.cpp
--- a/servo.cpp	Wed Mar 30 04:47:36 2022 +0000
+++ b/servo.cpp	Wed Mar 30 05:30:13 2022 +0000
@@ -52,7 +52,7 @@
         break;
     }
     
-    drobj = rc[3] + updateValues.dr_command;
+    drobj = rc[3];
     
     
     float objgain =  15.0f*M_PI/180.0f;
@@ -66,12 +66,19 @@
         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);
-    //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);
+    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);
+
+    }
     
     yawratePID.setGain(2.0f,0.0f,0.0f);
     pitchPID.setProcessValue(rpy.y);
@@ -89,10 +96,13 @@
     if(dr<-1.0f){dr = -1.0f;}
     if(dr>1.0f){dr = 1.0f;}
 
+    dTl = rc[2]+drobj*0.2f+updateValues.dT_command;
+    dTr = rc[2]-drobj*0.2f+updateValues.dT_command;
+
     scaledServoOut[0]=-de;
     scaledServoOut[1]=dr;
-    scaledMotorOut[0]=rc[2]+drobj*0.2f+updateValues.dT_command;
-    scaledMotorOut[1]=rc[2]-drobj*0.2f+updateValues.dT_command;
+    scaledMotorOut[0]= dTl;
+    scaledMotorOut[1]= dTr;
     
     float LP_servo = 0.2;
     for(int i = 0; i < sizeof(servoOut)/sizeof(servoOut[0]); i++)
@@ -127,4 +137,5 @@
     motor1.pulsewidth_us(motorOut[0]);
     motor2.pulsewidth_us(motorOut[1]);
     send2center();
+    send2pc();
 }
\ No newline at end of file