Revision 144:b3a713b4f1c4, committed 2022-06-29

Comitter:

NaotoMorita

Date:

Wed Jun 29 07:57:10 2022 +0000

Parent:

143:53808e4e684c

Commit message:

can fly

Changed in this revision

--- a/Autopilot.lib	Fri Jun 24 05:44:34 2022 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/teams/HAPSRG/code/Autopilot_Eigen/#598522e1bf4f

--- a/autopilot.cpp	Fri Jun 24 05:44:34 2022 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,53 +0,0 @@
-#include "global.hpp"
- 
-void level_flight()
-{
-    Vector3f vdot = calc_vdot();
-    Vector3f pihat = eskf.getPihat();
-    Vector3f vihat = eskf.getVihat();
-    autopilot.update_val(euler, -palt, pihat, vihat, vdot);
-    autopilot.level();
-    autopilot.keep_alt();
-    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
-}
- 
-void point_guide()
-{
-    Vector3f vdot = calc_vdot();
-    Vector3f pihat = eskf.getPihat();
-    Vector3f vihat = eskf.getVihat();
-    autopilot.update_val(euler, -palt, pihat, vihat, vdot);
-    autopilot.guide();
-    autopilot.keep_alt();
-    autopilot.return_val(roll_obj, pitch_obj, dT_obj);  
-}
- 
-void turning()
-{
-    Vector3f vdot = calc_vdot();
-    Vector3f pihat = eskf.getPihat();
-    Vector3f vihat = eskf.getVihat();
-    autopilot.update_val(euler, -palt, pihat, vihat, vdot);
-    autopilot.turn();
-    autopilot.keep_alt();
-    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
-}
-
-void climb()
-{
-    Vector3f vdot = calc_vdot();
-    Vector3f pihat = eskf.getPihat();
-    Vector3f vihat = eskf.getVihat();
-    autopilot.update_val(euler, -palt, pihat, vihat, vdot);
-    autopilot.level();
-    autopilot.climb();
-    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
-}
-
-Vector3f calc_vdot()
-{
-    Vector3f m_vdot = eskf.calcDynAcc(acc);
-    Vector3f vdot;
-    vdot = m_vdot;
-    return vdot;
-}
\ No newline at end of file

--- a/global.cpp	Fri Jun 24 05:44:34 2022 +0000
+++ b/global.cpp	Wed Jun 29 07:57:10 2022 +0000
@@ -18,34 +18,35 @@
 float magres = 0.0f;
 // control
 Timer _t;
-FastPWM servoRight(PE_9);
-FastPWM servoLeft(PE_11);
-FastPWM servoThrust(PE_13);
-PID pitchPID(10.0f,0.0f,0.0f,PID_dt); //rad
-PID pitchratePID(1.0f, 0.0f, 0.0f, PID_dt);//rad/s
-PID rollPID(5.0f,0.0f,0.0f,PID_dt);
+FastPWM motor1(PE_9);
+FastPWM motor2(PE_11);
+FastPWM motor3(PE_13);
+FastPWM motor4(PA_6);
+FastPWM motor5(PA_0);
+FastPWM motor6(PB_10);
+PID pitchPID(0.05f,0.0f,0.0f,PID_dt); //rad
+PID pitchratePID(0.05f, 0.0f, 0.0f, PID_dt);//rad/s
+PID rollPID(0.05f,0.0f,0.0f,PID_dt);
 PID rollratePID(0.05f, 0.0, 0.0, PID_dt);//rad/s
-PID yawratePID(2.0f, 0.0, 0.0, PID_dt);//rad/s
-PID climbratePID(1.0f, 0.0, 0.0, PID_dt);//rad/s
+PID yawratePID(0.3f, 0.0, 0.0, PID_dt);//rad/s
+PID vxPID(0.5f, 0.1f, 0.0f, PID_dt);
+PID vyPID(0.5f, 0.1f, 0.0f, PID_dt);
+PID vzPID(0.05f, 0.0f, 0.0f, PID_dt);
 solaESKF eskf; // ESKF class
-Autopilot autopilot;
-float roll_obj;
-float pitch_obj;
-float dT_obj;
 
 float rc[16];
 int loop_count = 0;
 float att_dt = 0.01f;
 // position
-Matrix3f SensorAlignmentAG;
-Matrix3f SensorAlignmentMAG;
-Vector3f euler;
-Vector3f acc;
-Vector3f mag;
-Vector3f magref(0.0f, 0.0f, 0.0f);
-Vector3f gyro;
-Vector3f vi;
-Vector3f pi;
+Matrix3f SensorAlignmentAG = Matrix3f::Zero();
+Matrix3f SensorAlignmentMAG = Matrix3f::Zero();
+Vector3f euler = Vector3f::Zero(); 
+Vector3f acc = Vector3f::Zero();
+Vector3f mag = Vector3f::Zero();
+Vector3f magref = Vector3f::Zero();
+Vector3f gyro = Vector3f::Zero();
+Vector3f vi = Vector3f::Zero();
+Vector3f pi = Vector3f::Zero();
 float palt;
 float palt0;
 float dynaccnorm2;
@@ -57,14 +58,11 @@
 
 float de = 0.0f;
 float da = 0.0f;
+float dr = 0.0f;
 float dT = 0.0f;
 
-float scaledServoOut[2]= {0.0f,0.0f};
-float scaledMotorOut[1]= {-1.0f};
-float servoOut[2] = {1500.0f,1500.0f};
-float motorOut[1] = {1100.0f};
-
-int calibrationFlag = 0;
+float scaledMotorOut[6]= {-1.0f,-1.0f,-1.0f,-1.0f,-1.0f,-1.0f};
+float motorOut[6] = {1100.0f,1100.0f,1100.0f,1100.0f,1100.0f,1100.0f};
 float agoffset[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
 
 
@@ -75,7 +73,7 @@
 telemetryPack tp;
 
 // HIL
-bool hilFlag = true;
+bool hilFlag = false;
 int16_t hilDataOut = 0;
 
 float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)

--- a/global.hpp	Fri Jun 24 05:44:34 2022 +0000
+++ b/global.hpp	Wed Jun 29 07:57:10 2022 +0000
@@ -13,7 +13,6 @@
 #include "CalibrateMagneto.h"
 #include "solaESKF.hpp"
 #include "GPSUBX_UART.hpp"
-#include "Autopilot.hpp"
 
 // Eigen
 #include <Eigen/Dense.h>
@@ -52,9 +51,7 @@
 
 struct sendPack
 {
-    float da;
-    float de;
-    float dT;
+    float motor[6];
     float euler[3];
     float state[4];
     float variance[3];
@@ -127,20 +124,23 @@
 
 // control
 extern Timer _t;
-extern FastPWM servoRight;
-extern FastPWM servoLeft;
-extern FastPWM servoThrust;
+extern FastPWM motor1;
+extern FastPWM motor2;
+extern FastPWM motor3;
+extern FastPWM motor4;
+extern FastPWM motor5;
+extern FastPWM motor6;
 extern PID pitchPID; //rad
 extern PID pitchratePID;//rad/s
 extern PID rollPID;
 extern PID rollratePID;//rad/s
 extern PID yawratePID;//rad/s
-extern PID climbratePID;//rad/s
+extern PID vxPID;//m/s
+extern PID vyPID;//m/s
+extern PID vzPID;//m/s
+
 extern solaESKF eskf; // EKF class
-extern Autopilot autopilot;
-extern float roll_obj;
-extern float pitch_obj;
-extern float dT_obj;
+
 
 extern float rc[16];
 extern int loop_count;
@@ -168,15 +168,12 @@
 
 extern float de;
 extern float da;
+extern float dr;
 extern float dT;
 
-extern float scaledServoOut[2];
-extern float scaledMotorOut[1];
-extern float servoOut[2];
-extern float motorOut[1];
+extern float scaledMotorOut[6];
+extern float motorOut[6];
 
-
-extern int calibrationFlag;
 extern float agoffset[6];
 
 

--- a/main.cpp	Fri Jun 24 05:44:34 2022 +0000
+++ b/main.cpp	Wed Jun 29 07:57:10 2022 +0000
@@ -3,7 +3,7 @@
 int main()
 {    
     setup();
-    calibrationFlag = userButton.read();  
+    int calibrationFlag = userButton.read();  
     if(calibrationFlag == 0)
     {
         run();

--- a/preflight.cpp	Fri Jun 24 05:44:34 2022 +0000
+++ b/preflight.cpp	Wed Jun 29 07:57:10 2022 +0000
@@ -42,20 +42,25 @@
             }
             // sbusデータの読み込み
             for (int i =0 ; i < 16;i ++){
-                rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
-            }
-            if (rc[4]>-0.3f && rc[6] < -0.3f){
-                preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : autoPilot enabled\r\n");
+                rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368.0f,1680.0f,-1.0f,1.0f) + (1.0f - 0.65f) * rc[i]; // mapped input
             }
-            if(!(gps.gpsFix == 0x02 || gps.gpsFix == 0x03)){
+            if (rc[7] > 0.3f){
                 preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : no gps lock\r\n");
+                motor1.pulsewidth_us(motorPwmMin);
+                motor2.pulsewidth_us(motorPwmMin);
+                motor3.pulsewidth_us(motorPwmMin);
+                motor4.pulsewidth_us(motorPwmMin);
+                motor5.pulsewidth_us(motorPwmMin);
+                motor6.pulsewidth_us(motorPwmMin);
+                twelite.serial.printf("PreFlight Check : switch arming position\r\n");
+                pc.serial.printf("PreFlight Check : switch arming position\r\n");
             }
-            preflightCheck = true;
+            
+            
             if(preflightCheck == true){
                 break;
             }
+            pc.serial.printf("PreFlight Check : failed\r\n");
         }
     }
     twelite.serial.printf("PreFlight Check Completed\r\n");
@@ -68,12 +73,12 @@
     eskf.setPhatPosition(2.0f,2.0f);
     eskf.setPhatVelocity(1.01f,1.01f);
     eskf.setPhatAngleError(0.1f);
-    eskf.setPhatAccBias(0.5f);
+    eskf.setPhatAccBias(0.0001f);
     eskf.setPhatGyroBias(0.0001f);
     eskf.setPhatGravity(0.0000001f);
     
     eskf.setQVelocity(0.1f,0.2f);
     eskf.setQAngleError(0.0000025f);
-    eskf.setQAccBias(0.000001f);
+    eskf.setQAccBias(0.0000001f);
     eskf.setQGyroBias(0.0000001f);
     }
\ No newline at end of file

--- a/run.cpp	Fri Jun 24 05:44:34 2022 +0000
+++ b/run.cpp	Wed Jun 29 07:57:10 2022 +0000
@@ -23,6 +23,13 @@
     Rwhole(7,7) = 5000.0f;
     Rwhole(8,8) = 0.007f;
     
+    MatrixXf Rimu = MatrixXf::Zero(5,5);
+    Rimu(0,0) = 0.1f;
+    Rimu(1,1) = 5000.0f;
+    Rimu(2,2) = 5000.0f;
+    Rimu(3,3) = 5000.0f;
+    Rimu(4,4) = 0.007f;
+    
     
     _t.start();
 
@@ -65,21 +72,19 @@
             getGPSval();
         }
         
-        if(gpsUpdateFlag == true){
-            float heading = std::atan2(-mag(1),mag(0));
-            Vector3f dynacc = eskf.calcDynAcc(acc);
-            dynaccnorm2 = dynacc.squaredNorm();
-            if(dynaccnorm2 > 0.16f){
-                Rwhole(5,5) = 100.0f;
-                Rwhole(6,6) = 100.0f;
-                Rwhole(7,7) = 100.0f;
-            }else{
-                Rwhole(5,5) = 5000.0f;
-                Rwhole(6,6) = 5000.0f;
-                Rwhole(7,7) = 5000.0f;
-            }
-            eskf.updateWhole(pi, palt, vi,acc,heading, Rwhole); 
+        float heading = std::atan2(-mag(1),mag(0));
+        Vector3f dynacc = eskf.calcDynAcc(acc);
+        dynaccnorm2 = dynacc.squaredNorm();
+        if(dynaccnorm2 > 0.16f){
+            Rimu(1,1) = 1.1f;
+            Rimu(2,2) = 1.1f;
+            Rimu(3,3) = 1.1f;
+        }else{
+            Rimu(1,1) = 100.1f;
+            Rimu(2,2) = 100.1f;
+            Rimu(3,3) = 100.1f;
         }
+        eskf.updateIMU(palt,acc,heading, Rimu); 
         
         PIDtick.loop(); 
         

--- a/servo.cpp	Fri Jun 24 05:44:34 2022 +0000
+++ b/servo.cpp	Wed Jun 29 07:57:10 2022 +0000
@@ -8,11 +8,9 @@
         rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368.0f,1680.0f,-1.0f,1.0f) + (1.0f - 0.65f) * rc[i]; // mapped input
     }
 
-    
     //姿勢角の所得
     euler = eskf.computeAngles();
-    
-    Vector3f vihat = eskf.getVihat();
+    euler(1) -= 3.0f*M_PI_F/180.0f;
     Vector3f gyroBias = eskf.getGyroBias();
     //PIDへの状態量のセット
     pitchPID.setProcessValue(euler(1));
@@ -20,96 +18,62 @@
     rollPID.setProcessValue(euler(0));
     rollratePID.setProcessValue(gyro(0)-gyroBias(0));
     yawratePID.setProcessValue(gyro(2)-gyroBias(2));
-    climbratePID.setProcessValue(vihat(2));
+    
+    vxPID.setGain(0.005f,0.0f,0.0f);
+    vyPID.setGain(0.005f,0.0f,0.0f);
+
+    
+    pitchPID.setGain(20.0f,0.0f,0.0f);
+    pitchratePID.setGain(0.18f,0.0f,0.0f);
+    rollPID.setGain(20.0f,0.0f,0.0f);
+    rollratePID.setGain(0.18f,0.0f,0.0f);
     
-    dT = rc[2];
+    Vector3f Vb = eskf.calcVb();
+    Vector3f dynAcc = eskf.calcDynAcc(acc);
+    vxPID.setProcessValue(dynAcc(0));
+    vxPID.setSetPoint(0.0f);
+    vyPID.setProcessValue(dynAcc(1));
+    vyPID.setSetPoint(0.0f);
+    vzPID.setProcessValue(Vb(2));
+    vzPID.setSetPoint(0.0f);
+    
+    //dT = -vzPID.compute()+rc[2];
+    dT = +rc[2];
     
-    // ゲイン切り替え
-    float vihat_norm = vihat.squaredNorm();
-    if (rc[5] < 0.3f){
-        // Hinf
-        hinf_flag = true;
-        if (vihat_norm < 9.0f || rc[2] < -0.8f){
-            pitchPID.setGain(4.55f, 0.0f, 0.0f);
-            pitchratePID.setGain(0.363f, 0.0f, 0.0f);
-            rollPID.setGain(1.34f, 0.0f, 0.0f);
-            rollratePID.setGain(0.219f, 0.0f,0.0f);
-            pitchPID.resetIntError();
-            rollPID.resetIntError();
-        }else{
-            pitchPID.setGain(4.55f, 9.7f, 0.0f);
-            pitchratePID.setGain(0.363f, 0.0f, 0.0f);
-            rollPID.setGain(1.34f, 0.007f, 0.0f);
-            rollratePID.setGain(0.219f, 0.0f,0.0f);
-        }
-    }else{
-        // 従来
-        hinf_flag = false;
-        if (vihat_norm < 9.0f || rc[2] < -0.8f){
-            pitchPID.setGain(10.0f, 0.0f, 0.0f);
-            pitchratePID.setGain(1.0f, 0.0f, 0.0f);//rad/s
-            rollPID.setGain(5.0f, 0.0f, 0.0f);
-            rollratePID.setGain(0.05f, 0.0f, 0.0f);//rad/s
-            pitchPID.resetIntError();
-            rollPID.resetIntError();
-        }else{
-            pitchPID.setGain(10.0f, 0.0f, 0.0f);
-            pitchratePID.setGain(1.0f, 0.0f, 0.0f);//rad/s
-            rollPID.setGain(5.0f, 0.0f, 0.0f);
-            rollratePID.setGain(0.05f, 0.0f, 0.0f);//rad/s
-        }
+    float pitchobj = rc[1]* 30.0f*M_PI_F/180.0f;
+    float rollobj = rc[0] * 30.0f*M_PI_F/180.0f;
+    yawratePID.setSetPoint(0.0f);
+    rollPID.setSetPoint(rollobj);
+    pitchPID.setSetPoint(pitchobj);
+    
+    float pitchrateobj = pitchPID.compute()* 30.0f*M_PI_F/180.0f;
+    float rollrateobj =  rollPID.compute()* 30.0f*M_PI_F/180.0f;
+    pitchratePID.setSetPoint(pitchrateobj);
+    rollratePID.setSetPoint(rollrateobj);
+    
+    de = pitchratePID.compute();
+    da = rollratePID.compute();
+    dr = -0.1f*rc[3]+yawratePID.compute();
+    
+    scaledMotorOut[0]= dT+de-dr;
+    scaledMotorOut[1]= dT+0.866f*de-0.866f*da+dr;
+    scaledMotorOut[2]= dT-0.866f*de-0.866f*da-dr;
+    scaledMotorOut[3]= dT-de+dr;
+    scaledMotorOut[4]= dT-0.866f*de+0.866f*da-dr;
+    scaledMotorOut[5]= dT+0.866f*de+0.866f*da+dr;
+    
+    float minArmThrottle = 0.25f;
+    for(int i = 0;i<sizeof(scaledMotorOut)/sizeof(scaledMotorOut[0]) ;i++){
+      if(scaledMotorOut[i]<-1.0f+minArmThrottle) {
+          scaledMotorOut[i] = -1.0f+minArmThrottle;
+      }
     }
     
-    if (rc[4]>-0.3f && rc[6] < -0.3f)
-    {
-        //level_flight();
-        //point_guide();
-        //climb();
-        
-        turning();
-        rollPID.setSetPoint(roll_obj);
-        pitchPID.setSetPoint(pitch_obj);
-        dT += dT_obj;
-        de = pitchPID.compute()-(rc[0]-rc[1])/2.0f;
-        da = rollPID.compute() -(rc[0]+rc[1])/2.0f;
-    }else{
-        yawratePID.setSetPoint(0.0f);
-        roll_obj = yawratePID.compute()*30.0f*M_PI/180.0f;
-        rollPID.setSetPoint(roll_obj);
-        climbratePID.setSetPoint(0.0f);
-        pitch_obj = -climbratePID.compute()*10.0f*M_PI/180.0f;
-        pitchPID.setSetPoint(pitch_obj);
-        de = pitchPID.compute() -(rc[0]-rc[1])/2.0f*2.0f;
-        da = rollPID.compute() -(rc[0]+rc[1])/2.0f*3.0f;
-    }
+    //pc.serial.printf("%f %f %f %f\r\n",dT,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI);
     
-    //舵角計算
-    if(rc[4]<-0.3f){
-        de = -(rc[0]-rc[1])/2.0f;
-        da = -(rc[0]+rc[1])/2.0f;
-    }
-    
-    scaledServoOut[0]=-de-da;
-    scaledServoOut[1]=de-da;
-    scaledMotorOut[0]= dT;
-    
-    float LP_servo = 1.0f;
-    float LP_motor = 0.2f;
-    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];
-        if(servoOut[i]<servoPwmMin)
-        {
-            servoOut[i] = servoPwmMin;
-        }
-        if(servoOut[i]>servoPwmMax)
-        {
-            servoOut[i] = servoPwmMax;
-        }
-    }
-    
-    for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;i++){
-        motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))+(1.0-LP_motor)*motorOut[i];
+    float LP_motor = 1.0f;
+    for(int i = 0;i<6 ;i++){
+        motorOut[i] = (mapfloat(scaledMotorOut[i],-1.0f,1.0f,motorPwmMin,motorPwmMax));
         if(motorOut[i]<motorPwmMin) {
             motorOut[i] = motorPwmMin;
         };
@@ -117,11 +81,22 @@
             motorOut[i] = motorPwmMax;
         };
     }
-    servoRight.pulsewidth_us(servoOut[0]);
-    servoLeft.pulsewidth_us(servoOut[1]); 
-    servoThrust.pulsewidth_us(motorOut[0]);
-
-
+    
+    if(rc[7]>0.3f){
+        motor1.pulsewidth_us(motorOut[0]);
+        motor2.pulsewidth_us(motorOut[1]);
+        motor3.pulsewidth_us(motorOut[2]);
+        motor4.pulsewidth_us(motorOut[3]);
+        motor5.pulsewidth_us(motorOut[4]);
+        motor6.pulsewidth_us(motorOut[5]);
+    }else{
+        motor1.pulsewidth_us(motorPwmMin);
+        motor2.pulsewidth_us(motorPwmMin);
+        motor3.pulsewidth_us(motorPwmMin);
+        motor4.pulsewidth_us(motorPwmMin);
+        motor5.pulsewidth_us(motorPwmMin);
+        motor6.pulsewidth_us(motorPwmMin);
+    }
     sendData2PC();
     writeSDcard();
     

--- a/setup.cpp	Fri Jun 24 05:44:34 2022 +0000
+++ b/setup.cpp	Wed Jun 29 07:57:10 2022 +0000
@@ -27,34 +27,60 @@
     float magMax[3] =  {182.602386, 530.811523, 365.834625};
     magCalibrator.setExtremes(magMin,magMax);
     
-    pitchPID.setSetPoint(0.0f);
-    pitchratePID.setSetPoint(0.0f); 
-    rollPID.setSetPoint(0.0f); 
-    rollratePID.setSetPoint(0.0f); 
-    pitchPID.setBias(0.0f);
-    pitchratePID.setBias(0.0f); 
-    rollPID.setBias(0.0f); 
-    rollratePID.setBias(0.0f); 
-    pitchPID.setOutputLimits(-1.0f,1.0f);
-    pitchratePID.setOutputLimits(-1.0f,1.0f);
-    rollPID.setOutputLimits(-1.0f,1.0f); 
-    rollratePID.setOutputLimits(-1.0f,1.0f);
-    pitchPID.setInputLimits(-M_PI_F,M_PI_F);
-    pitchratePID.setInputLimits(-M_PI_F,M_PI_F);
-    rollPID.setInputLimits(-M_PI_F,M_PI_F); 
-    rollratePID.setInputLimits(-M_PI_F,M_PI_F);
+    pitchPID.setSetPoint(0.0);
+    pitchPID.setBias(0.0);
+    pitchPID.setOutputLimits(-1.0,1.0);
+    pitchPID.setInputLimits(-M_PI,M_PI);
+    
+    pitchratePID.setSetPoint(0.0); 
+    pitchratePID.setBias(0.0); 
+    pitchratePID.setOutputLimits(-1.0,1.0);
+    pitchratePID.setInputLimits(-M_PI,M_PI);
+    
+    rollPID.setSetPoint(0.0); 
+    rollPID.setBias(0.0); 
+    rollPID.setOutputLimits(-1.0,1.0);
+    rollPID.setInputLimits(-M_PI,M_PI); 
+     
+    rollratePID.setSetPoint(0.0);
+    rollratePID.setBias(0.0);
+    rollratePID.setOutputLimits(-1.0,1.0);
+    rollratePID.setInputLimits(-M_PI,M_PI);
+    
+    yawratePID.setSetPoint(0.0); 
+    yawratePID.setBias(0.0); 
+    yawratePID.setOutputLimits(-1.0,1.0);
+    yawratePID.setInputLimits(-M_PI,M_PI);
     
-    servoRight.period_us(15000.0f);
-    servoLeft.period_us(15000.0f);
-    servoThrust.period_us(15000.0f);
-    servoRight.pulsewidth_us(1500.0f);
-    servoLeft.pulsewidth_us(1500.0f); 
-    servoThrust.pulsewidth_us(1100.0f);
+    vxPID.setSetPoint(0.0f); 
+    vxPID.setBias(0.0f); 
+    vxPID.setOutputLimits(-1.0f,1.0);
+    vxPID.setInputLimits(-5.0f,5.0f);
+    
+    vyPID.setSetPoint(0.0f); 
+    vyPID.setBias(0.0f); 
+    vyPID.setOutputLimits(-1.0f,1.0);
+    vyPID.setInputLimits(-5.0f,5.0f);
+    
+    vzPID.setSetPoint(0.0f); 
+    vzPID.setBias(0.0f); 
+    vzPID.setOutputLimits(-1.0f,1.0);
+    vzPID.setInputLimits(-1.0f,1.0f);
     
-    autopilot.set_dest(0.0f, 50.0f);
-    autopilot.set_turn(0.0f, 50.0f, 50.0f);
-    autopilot.set_alt(30.0f, 10.0f);
-    autopilot.set_climb(3.0f*M_PI_F/180.0f, 0.0f);
+    
+    motor1.period_us(15000.0);
+    motor2.period_us(15000.0);
+    motor3.period_us(15000.0);
+    motor4.period_us(15000.0);
+    motor5.period_us(15000.0);
+    motor6.period_us(15000.0);
+
+    motor1.pulsewidth_us(motorPwmMin);
+    motor2.pulsewidth_us(motorPwmMin);
+    motor3.pulsewidth_us(motorPwmMin);
+    motor4.pulsewidth_us(motorPwmMin);
+    motor5.pulsewidth_us(motorPwmMin);
+    motor6.pulsewidth_us(motorPwmMin);
     
 }
 

--- a/solaESKF.lib	Fri Jun 24 05:44:34 2022 +0000
+++ b/solaESKF.lib	Wed Jun 29 07:57:10 2022 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/HAPSRG/code/solaESKF_Eigen/#b241c058df83
+https://os.mbed.com/teams/HAPSRG/code/solaESKF_Eigen/#8f6ae61d47ac

--- a/transferData.cpp	Fri Jun 24 05:44:34 2022 +0000
+++ b/transferData.cpp	Wed Jun 29 07:57:10 2022 +0000
@@ -2,9 +2,10 @@
 
 void sendData2PC()
 {
-    sp.da = da;
-    sp.de = de;
-    sp.dT = dT;
+    
+    for(int i = 0;i<sizeof(scaledMotorOut)/sizeof(scaledMotorOut[0]) ;i++){
+        sp.motor[i] = scaledMotorOut[i];
+    }
     sp.euler[0] = euler(0)*180.0f/M_PI_F;
     sp.euler[1] = euler(1)*180.0f/M_PI_F;
     sp.euler[2] = euler(2)*180.0f/M_PI_F;
@@ -62,7 +63,7 @@
         }
         break;
     case 6 :
-        //gyroBias
+        //gravity
         for (int i = 0; i < 3; i++){
             sp.state[i] = state(i+16);
         }
@@ -131,18 +132,10 @@
     tp.wind[0] = 0.0f;
     tp.wind[1] = 0.0f;
     tp.wind[2] = 0.0f;
-    tp.pitch_obj = pitch_obj;
-    tp.roll_obj = roll_obj;
+    tp.pitch_obj = 0.0f;
+    tp.roll_obj = 0.0f;
     tp.u_pitot = 0.0f;
-    
-    if (hinf_flag)
-    {
-        tp.hinf = 1.0f;
-    }
-    else
-    {
-        tp.hinf = 0.0f;
-    }
+    tp.hinf = 0.0f;
 
     wait(0.001f);
 
@@ -189,8 +182,8 @@
     lp.wind[0] = 0.0f;
     lp.wind[1] = 0.0f;
     lp.wind[2] = 0.0f;
-    lp.pitch_obj = pitch_obj;
-    lp.roll_obj = roll_obj;
+    lp.pitch_obj = 0.0f;
+    lp.roll_obj = 0.0f;
     lp.u_pitot = 0.0f;
 
     //sd.printf("%f %f %f %f %f %f\r\n",da,de,dT,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI);