Revision 127:839ae718713b, committed 2022-07-08

Comitter:

NaotoMorita

Date:

Fri Jul 08 08:25:42 2022 +0000

Parent:

126:79e396942eb1

Commit message:

new

Changed in this revision

--- a/datatransfer.cpp	Fri May 20 13:37:39 2022 +0000
+++ b/datatransfer.cpp	Fri Jul 08 08:25:42 2022 +0000
@@ -67,6 +67,9 @@
     sp.dTl = dTl;
     sp.dTr = dTr;
     sp.dr = dr;
+    sp.roll = rpy.x/M_PI*180.0f;
+    sp.pitch = rpy.y/M_PI*180.0f;
+    sp.yaw = rpy.z/M_PI*180.0f;
     //sp.data[0] = vi.x;
     //sp.data[1] = vi.y;
     //sp.data[2] = vi.z;

--- a/global.cpp	Fri May 20 13:37:39 2022 +0000
+++ b/global.cpp	Fri Jul 08 08:25:42 2022 +0000
@@ -2,7 +2,7 @@
 
 // var
 
-bool hilFlag = false;
+bool hilFlag = true;
 
 // communication
 UsaPack pc(USBTX, USBRX, 115200);

--- a/global.hpp	Fri May 20 13:37:39 2022 +0000
+++ b/global.hpp	Fri Jul 08 08:25:42 2022 +0000
@@ -93,7 +93,10 @@
     float dTl;
     float dTr;
     float dr;
-    //float data[3];
+    float roll;
+    float pitch;
+    float yaw;
+    
 };
 
 

--- a/run.cpp	Fri May 20 13:37:39 2022 +0000
+++ b/run.cpp	Fri Jul 08 08:25:42 2022 +0000
@@ -61,9 +61,7 @@
         }else{
             getHilIMUval();
         }
-        
         calcOpticalVel();
-        
         //ekfの更新
         eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
         eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);

--- 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];