Revision 143:53808e4e684c, committed 2022-06-24

Comitter:

NaotoMorita

Date:

Fri Jun 24 05:44:34 2022 +0000

Parent:

142:e1b8357d1ea4

Commit message:

complete

Changed in this revision

--- a/GPSUBX_UART.lib	Fri Dec 10 11:20:13 2021 +0000
+++ b/GPSUBX_UART.lib	Fri Jun 24 05:44:34 2022 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/HAPSRG/code/GPSUBX_UART_Eigen/#2e6147aeba54
+https://os.mbed.com/teams/HAPSRG/code/GPSUBX_UART_Eigen/#712076ece407

--- a/PIDcontroller.lib	Fri Dec 10 11:20:13 2021 +0000
+++ b/PIDcontroller.lib	Fri Jun 24 05:44:34 2022 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/HAPSRG/code/PIDcontroller/#4afcf53be684
+https://os.mbed.com/teams/HAPSRG/code/PIDcontroller/#dfd3e6512bd5

--- a/autopilot.cpp	Fri Dec 10 11:20:13 2021 +0000
+++ b/autopilot.cpp	Fri Jun 24 05:44:34 2022 +0000
@@ -5,7 +5,7 @@
     Vector3f vdot = calc_vdot();
     Vector3f pihat = eskf.getPihat();
     Vector3f vihat = eskf.getVihat();
-    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
+    autopilot.update_val(euler, -palt, pihat, vihat, vdot);
     autopilot.level();
     autopilot.keep_alt();
     autopilot.return_val(roll_obj, pitch_obj, dT_obj);
@@ -16,7 +16,7 @@
     Vector3f vdot = calc_vdot();
     Vector3f pihat = eskf.getPihat();
     Vector3f vihat = eskf.getVihat();
-    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
+    autopilot.update_val(euler, -palt, pihat, vihat, vdot);
     autopilot.guide();
     autopilot.keep_alt();
     autopilot.return_val(roll_obj, pitch_obj, dT_obj);  
@@ -27,7 +27,7 @@
     Vector3f vdot = calc_vdot();
     Vector3f pihat = eskf.getPihat();
     Vector3f vihat = eskf.getVihat();
-    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
+    autopilot.update_val(euler, -palt, pihat, vihat, vdot);
     autopilot.turn();
     autopilot.keep_alt();
     autopilot.return_val(roll_obj, pitch_obj, dT_obj);
@@ -38,7 +38,7 @@
     Vector3f vdot = calc_vdot();
     Vector3f pihat = eskf.getPihat();
     Vector3f vihat = eskf.getVihat();
-    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
+    autopilot.update_val(euler, -palt, pihat, vihat, vdot);
     autopilot.level();
     autopilot.climb();
     autopilot.return_val(roll_obj, pitch_obj, dT_obj);

--- a/global.cpp	Fri Dec 10 11:20:13 2021 +0000
+++ b/global.cpp	Fri Jun 24 05:44:34 2022 +0000
@@ -25,8 +25,9 @@
 PID pitchratePID(1.0f, 0.0f, 0.0f, PID_dt);//rad/s
 PID rollPID(5.0f,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
 solaESKF eskf; // ESKF class
-int obsCount = 0;
 Autopilot autopilot;
 float roll_obj;
 float pitch_obj;
@@ -39,9 +40,7 @@
 Matrix3f SensorAlignmentAG;
 Matrix3f SensorAlignmentMAG;
 Vector3f euler;
-Vector3f rpy(0.0f, 0.0f, 0.0f); // x:roll  y:pitch  z:yaw
 Vector3f acc;
-Vector3f accref(0.0f, 0.0f, 9.8f);
 Vector3f mag;
 Vector3f magref(0.0f, 0.0f, 0.0f);
 Vector3f gyro;
@@ -54,13 +53,11 @@
 bool gpsUpdateFlag = false;
 bool gpsLlh0Fixed = false;
 bool headingUpdateFlag = false;
+bool hinf_flag = false;
 
 float de = 0.0f;
 float da = 0.0f;
 float dT = 0.0f;
-MedianFilter accMedian(3);
-MedianFilter gyroMedian(3);
-MedianFilter magMedian(3);
 
 float scaledServoOut[2]= {0.0f,0.0f};
 float scaledMotorOut[1]= {-1.0f};
@@ -70,14 +67,6 @@
 int calibrationFlag = 0;
 float agoffset[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
 
-float magbias[4] = {-134.817047, 127.833481, -152.631836, 44.508556};
-float magbiasMin[3] = {-174.831711, 93.465691, -188.617172};
-float magbiasMax[3] = {-110.413963, 162.321548, -122.566071};
-float accMin[3] = {-0.963692, -0.974141, -1.012899};
-float accMax[3] = { 1.025954, 0.974748, 0.987155};
-
-Vector3f rpy_align( 0.0f*M_PI_F/180.0f, 5.0f*M_PI_F/180.0f, 0.0f*M_PI_F/180.0f);
-
 
 // UsaPack
 valuePack vp;
@@ -87,9 +76,7 @@
 
 // HIL
 bool hilFlag = true;
-bool serialControlSource = false;
-bool serialParamSource = false;
-int checkParamSerial[5] = {0,0,0,0,0};
+int16_t hilDataOut = 0;
 
 float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
 {
@@ -98,11 +85,21 @@
 
 void setDiag(Matrix3f& mat, float val){
     for (int i = 0; i < mat.cols(); i++){
+            for (int j = 0; j < mat.rows(); j++){
+                mat(i,j) = 0.0f;
+            }
+    }
+    for (int i = 0; i < mat.cols(); i++){
             mat(i,i) = val;
     }
 }
 
 void setDiag(MatrixXf& mat, float val){
+    for (int i = 0; i < mat.cols(); i++){
+        for (int j = 0; j < mat.rows(); j++){
+            mat(i,j) = 0.0f;
+        }
+    }
     for (int i = 0; i < mat.cols(); i++)
     {
         mat(i, i) = val;

--- a/global.hpp	Fri Dec 10 11:20:13 2021 +0000
+++ b/global.hpp	Fri Jun 24 05:44:34 2022 +0000
@@ -8,12 +8,10 @@
 #include "FastPWM.h"
 #include <cmath>
 #include "UsaPack.hpp"
-#include "Vector3.hpp"
 #include "LSM9DS1.h"
 #include "LPS.h"
 #include "CalibrateMagneto.h"
 #include "solaESKF.hpp"
-#include "MedianFilter.hpp"
 #include "GPSUBX_UART.hpp"
 #include "Autopilot.hpp"
 
@@ -38,6 +36,7 @@
 #define M_PI_F 3.141592f
 #endif
 
+
 struct valuePack
 {
     int16_t accData[3];
@@ -45,7 +44,8 @@
     int16_t magData[3];
     int16_t viData[3];
     int16_t piData[3];
-    int16_t actData[4];
+    //int16_t airspeed;
+    //int16_t actData[4];
     int16_t commandIndex;
     int16_t commandVal;
 };
@@ -55,38 +55,57 @@
     float da;
     float de;
     float dT;
-    float rpy[3];
-    float vihat[3];
+    float euler[3];
+    float state[4];
+    float variance[3];
+    
 };
 struct logPack
 {
-    float time;
-    float hertz;
-    float gpsFix;
-    float da;
-    float de;
-    float dT;
-    float rpy[3];
-    float pihat[3];
-    float vihat[3];
-    float pi[3];
-    float vi[3];
-    float palt;
-    float acc[3];
-    float gyro[3];
-    float mag[3];
-    float rc[16];
+    float time; // 1
+    float hertz; // 2
+    float gpsFix; // 3
+    float da; // 4
+    float de; // 5
+    float dT; // 6
+    float rpy[3]; // 7 8 9
+    float pihat[3]; // 10 11 12
+    float vihat[3]; // 13 14 15
+    float pi[3]; // 16 17 18
+    float vi[3]; // 19 20 21
+    float palt; // 22
+    float acc[3]; // 23 24 25
+    float gyro[3]; // 26 27 28
+    float mag[3]; // 29 30 31
+    float rc[16]; // 32 ... 47
+    float wind[3]; // 48 49 50
+    float pitch_obj; // 51
+    float roll_obj; // 52
+    float u_pitot; // 53
 };
 
 struct telemetryPack
 {
+    float pi[3];
+    float rpy_l[3];
+    float rpy_c[3];
+    float rpy_r[3];
+    float vi[3];
+    float palt;
+    float gps_fix;
+    float mode; //preflight:0, flight:1
     float time;
-    float hertz;
-    float gpsFix;
-    float rpy[3];
-    float pihat[3];
-    float vihat[3];
-    float dynaccNorm;
+    float gps_acc;
+    float vx_opt;
+    float vy_opt;
+    float dist_ir;
+    float voltage[2];
+    float current[2];
+    float wind[3];
+    float pitch_obj;
+    float roll_obj;
+    float hinf;
+    float u_pitot;
 };
 
 // var
@@ -105,7 +124,7 @@
 extern LPS lps;
 extern GPSUBX_UART gps;
 extern CalibrateMagneto magCalibrator;
-extern float magres;
+
 // control
 extern Timer _t;
 extern FastPWM servoRight;
@@ -115,8 +134,9 @@
 extern PID pitchratePID;//rad/s
 extern PID rollPID;
 extern PID rollratePID;//rad/s
+extern PID yawratePID;//rad/s
+extern PID climbratePID;//rad/s
 extern solaESKF eskf; // EKF class
-extern int obsCount;
 extern Autopilot autopilot;
 extern float roll_obj;
 extern float pitch_obj;
@@ -125,16 +145,13 @@
 extern float rc[16];
 extern int loop_count;
 extern float att_dt;
-extern bool accmagSwitch;
 
 
 // position
 extern Matrix3f SensorAlignmentAG;
 extern Matrix3f SensorAlignmentMAG; 
 extern Vector3f euler;
-extern Vector3f rpy; // x:roll  y:pitch  z:yaw
 extern Vector3f acc;
-extern Vector3f accref;
 extern Vector3f mag;
 extern Vector3f magref;
 extern Vector3f gyro;
@@ -147,13 +164,11 @@
 extern bool gpsUpdateFlag;
 extern bool gpsLlh0Fixed;
 extern bool headingUpdateFlag;
+extern bool hinf_flag;
 
 extern float de;
 extern float da;
 extern float dT;
-extern MedianFilter accMedian;
-extern MedianFilter gyroMedian;
-extern MedianFilter magMedian;
 
 extern float scaledServoOut[2];
 extern float scaledMotorOut[1];
@@ -163,13 +178,6 @@
 
 extern int calibrationFlag;
 extern float agoffset[6];
-extern float magbiasMin[3];
-extern float magbiasMax[3];
-extern float magbias[4];
-extern float accMin[3];
-extern float accMax[3];
-
-extern Vector3f rpy_align;
 
 
 //// UsaPack
@@ -180,9 +188,8 @@
 
 // HIL
 extern bool hilFlag;
-extern bool serialControlSource;
-extern bool serialParamSource;
-extern int checkParamSerial[5];
+extern int16_t hilDataOut;
+
 // function
 
 // main.cpp
@@ -190,6 +197,12 @@
 // setup.cpp
 extern void setup();
 extern void calibrate();
+
+// preflight.cpp
+extern void preflightCalibration();
+extern void preflightCheck();
+extern void setEskfCov();
+
 // run.cpp
 extern void run();
 

--- a/gps.cpp	Fri Dec 10 11:20:13 2021 +0000
+++ b/gps.cpp	Fri Jun 24 05:44:34 2022 +0000
@@ -7,15 +7,15 @@
     if (gps.iTOW_STATUS != itow_status){
         itow_status = gps.iTOW_STATUS;
         if(gps.gpsFix == 0x02 || gps.gpsFix == 0x03){
-            vi = gps.VelocityNED;
             if(gpsLlh0Fixed == false){
                 gps.CalculateUnit();
+                gps.Calculate(pi(0),pi(1),pi(2),vi(0),vi(1),vi(2));
+                gpsUpdateFlag = true;
                 gpsLlh0Fixed = true;
             }else{
-                gps.Calculate();
+                gps.Calculate(pi(0),pi(1),pi(2),vi(0),vi(1),vi(2));
+                gpsUpdateFlag = true;
             }
-            pi = gps.PositionNED;
-            gpsUpdateFlag = true;
         }
     }
 }

--- a/hil.cpp	Fri Dec 10 11:20:13 2021 +0000
+++ b/hil.cpp	Fri Jun 24 05:44:34 2022 +0000
@@ -6,12 +6,12 @@
         case 1:
             NVIC_SystemReset();
             break;
+        case 2:
+            hilDataOut = vp.commandVal;
+            break;
         default : 
             break;
         }
-    rpy_align(0) = 0.0f;
-    rpy_align(1) = 0.0f;
-    accref(2) =  1.0f;
     
     
     lsm.readAccel();
@@ -41,10 +41,6 @@
     magraw(1) = float(vp.magData[1])/1000.0f + (lsm.my-magref(1));
     magraw(2) = float(vp.magData[2])/1000.0f + (lsm.mz-magref(2));
 
-    //magres = magCalibrator.calcResidial(magraw);
-    //if(magres > magresThreshold){
-    //    magCalibrator.updateParams(magraw,0.001f);
-    //};
     Vector3f magmod = magraw;
     mag(0) = magmod(0);
     mag(1) = magmod(1);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/preflight.cpp	Fri Jun 24 05:44:34 2022 +0000
@@ -0,0 +1,79 @@
+#include "global.hpp"
+
+void preflightCalibration()
+{
+    wait(0.5f);
+    //センサの初期化・ジャイロバイアス・加速度スケールの取得
+    int n_init = 1000;
+    for(int i = 0;i<n_init;i++){
+        lsm.readAccel();
+        lsm.readMag();
+        lsm.readGyro();
+        //agoffset[0] += lsm.ax * 9.8f;
+        //agoffset[1] += lsm.ay * 9.8f;
+        //agoffset[2] += lsm.az * 9.8f-9.8f;
+        agoffset[3] +=(lsm.gx * M_PI_F / 180.0f);
+        agoffset[4] +=(lsm.gy * M_PI_F / 180.0f); 
+        agoffset[5] +=(lsm.gz * M_PI_F / 180.0f); 
+        palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars());
+        magref(0) += lsm.mx;
+        magref(1) += lsm.my;
+        magref(2) += lsm.mz;
+    }
+    for(int i = 0;i<6;i++){
+        agoffset[i] /= float(n_init);
+    }
+    magref /= float(n_init);
+    palt0 /= float(n_init);
+    twelite.serial.printf("Sensor offset : %f %f %f %f %f %f\r\n",agoffset[0],agoffset[1],agoffset[2],agoffset[3],agoffset[4],agoffset[5]);
+    wait(1.0f);
+}
+
+
+void preflightCheck()
+{
+    //センサ正常性チェック
+    if(hilFlag == false){
+        while(1){
+            bool preflightCheck = true;
+            if(sbus.failSafe){
+                preflightCheck = false;
+                twelite.serial.printf("PreFlight Check : no RC\r\n");
+            }
+            // 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");
+            }
+            if(!(gps.gpsFix == 0x02 || gps.gpsFix == 0x03)){
+                preflightCheck = false;
+                twelite.serial.printf("PreFlight Check : no gps lock\r\n");
+            }
+            preflightCheck = true;
+            if(preflightCheck == true){
+                break;
+            }
+        }
+    }
+    twelite.serial.printf("PreFlight Check Completed\r\n");
+}
+
+void setEskfCov(){
+    
+    //ESKFの共分散設定
+    eskf.setGravity(0.0f,0.0f,9.8f);
+    eskf.setPhatPosition(2.0f,2.0f);
+    eskf.setPhatVelocity(1.01f,1.01f);
+    eskf.setPhatAngleError(0.1f);
+    eskf.setPhatAccBias(0.5f);
+    eskf.setPhatGyroBias(0.0001f);
+    eskf.setPhatGravity(0.0000001f);
+    
+    eskf.setQVelocity(0.1f,0.2f);
+    eskf.setQAngleError(0.0000025f);
+    eskf.setQAccBias(0.000001f);
+    eskf.setQGyroBias(0.0000001f);
+    }
\ No newline at end of file

--- a/run.cpp	Fri Dec 10 11:20:13 2021 +0000
+++ b/run.cpp	Fri Jun 24 05:44:34 2022 +0000
@@ -2,137 +2,40 @@
 
 void run()
 {
-    wait(0.5);
-
-    //センサの初期化・ジャイロバイアス・加速度スケールの取得
-    int n_init = 1000;
-    for(int i = 0;i<n_init;i++){
-        lsm.readAccel();
-        lsm.readMag();
-        lsm.readGyro();
-        agoffset[0] += lsm.ax * 9.8f;
-        agoffset[1] += lsm.ay * 9.8f;
-        agoffset[2] += lsm.az * 9.8f-9.8f;
-        agoffset[3] +=(lsm.gx * M_PI_F / 180.0f);
-        agoffset[4] +=(lsm.gy * M_PI_F / 180.0f); 
-        agoffset[5] +=(lsm.gz * M_PI_F / 180.0f); 
-        palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars());
-        magref(0) += lsm.mx;
-        magref(1) += lsm.my;
-        magref(2) += lsm.mz;
-    }
-    for(int i = 0;i<6;i++){
-        agoffset[i] /= float(n_init);
-    }
-    magref /= float(n_init);
-    palt0 /= float(n_init);
-    twelite.serial.printf("Sensor offset : %f %f %f %f %f %f\r\n",agoffset[0],agoffset[1],agoffset[2],agoffset[3],agoffset[4],agoffset[5]);
+    preflightCalibration();
     
-    //ESKFの共分散設定
-    eskf.setGravity(0.0f,0.0f,9.8f);
-    eskf.setPhatPosition(0.1f);
-    eskf.setPhatVelocity(0.1f);
-    eskf.setPhatAngleError(0.5f);
-    eskf.setPhatAccBias(0.001f);
-    eskf.setPhatGyroBias(0.001f);
-    eskf.setPhatGravity(0.0000001f);
-    
-    eskf.setQVelocity(0.001f);
-    eskf.setQAngleError(0.0000025f);
-    eskf.setQAccBias(0.000001f);
-    eskf.setQGyroBias(0.000001f);
-    
-    Matrix3f Rgpspos;
-    setDiag(Rgpspos,1.0f);
-    
-    Matrix3f Rgpsvel;
-    Rgpsvel(0,0) = 0.01f;
-    Rgpsvel(1,1) = 0.01f;
-    Rgpsvel(2,2) = 0.1f;
+    setEskfCov();
     
     MatrixXf Rgps(5,5);
-    setDiag(Rgps,0.05f);
+    setDiag(Rgps,1.5f);
+    Rgps(2,2) = 0.1f;
     Rgps(3,3) = 0.1f;
     Rgps(4,4) = 0.1f;
     
-    float dynAccCriteria = 0.4f;
-    Matrix3f Racc;
-    setDiag(Racc,100.0f);
-    Matrix3f RaccDyn;
-    setDiag(RaccDyn,5000.0f);
-
-    Matrix<float, 1, 1> Rheading;
-    Rheading(0, 0) = 0.01f;
+    MatrixXf Rwhole = MatrixXf::Zero(9,9);
+    Rwhole(0,0) = 1.5f;
+    Rwhole(1,1) = 1.5f;
+    Rwhole(2,2) = 0.1f;
+    Rwhole(3,3) = 0.1f;
+    Rwhole(4,4) = 0.1f;
+    Rwhole(5,5) = 5000.0f;
+    Rwhole(6,6) = 5000.0f;
+    Rwhole(7,7) = 5000.0f;
+    Rwhole(8,8) = 0.007f;
+    
     
     _t.start();
-    //センサ正常性チェック
-    if(hilFlag == false){
-        while(1){
-            float tstart = _t.read();
-            getIMUval();
-            getGPSval();
-            eskf.updateNominal(acc, gyro, att_dt);
-            eskf.updateErrState(acc, gyro, att_dt);
-            eskf.updateGPS(pi, palt, vi, Rgps);
-            float heading = std::atan2(-mag(2), mag(0));
-            eskf.updateHeading(heading, Rheading);
-            Matrix3f Raccpreflight;
-            setDiag(Raccpreflight, 5.0f);
-            eskf.updateAcc(acc, Raccpreflight);
-            
-            bool preflightCheck = true;
-            Vector3f gyroBias = eskf.getGyroBias();
-            if(fabsf(gyro(0)-gyroBias(0))>2.0f || fabsf(gyro(1)-gyroBias(1))>2.0f || fabsf(gyro(2)-gyroBias(2))>2.0f){
-                preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : high gyro value\r\n");
-            }
-            Vector3f accBias = eskf.getAccBias();
-            if(fabsf(acc(0)-accBias(0))>2.0f || fabsf(acc(1)-gyroBias(1))>2.0f){
-                preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : high acc value\r\n");
-            }
-            Vector3f vihat = eskf.getVihat();
-            if(fabsf(vihat(0))>2.0f || fabsf(vihat(1))>2.0f||fabsf(vihat(2))>2.0f){
-                preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : high velocity value\r\n");
-            }
-            Vector3f pihat = eskf.getPihat();
-            if(fabsf(pihat(0))>2.0f || fabsf(pihat(1))>2.0f||fabsf(pihat(2))>2.0f){
-                preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : not home position\r\n");
-            }
-            if(sbus.failSafe){
-                preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : no RC\r\n");
-            }
-            // 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");
-            }
-            if(!(gps.gpsFix == 0x02 || gps.gpsFix == 0x03)){
-                preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : no gps lock\r\n");
-            }
-            
-            if(preflightCheck == true){
-                break;
-            }
-        }
-    }
-    twelite.serial.printf("PreFlight Check Completed\r\n");
-    //usaPack通信開始
+
+    preflightCheck();
+    wait(1.0f);
+    //usaPack通信開始 制御ループのアタッチ
     pc.Subscribe(0000, &(vp));
-
-    //制御ループのアタッチ
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
     
     float tgps = _t.read();
     float theading = _t.read();
+    float tstart = _t.read();
     while(1)
     {
         float tstart = _t.read();
@@ -162,32 +65,22 @@
             getGPSval();
         }
         
-        headingUpdateFlag = false;
-        if(tstart-theading>0.05f){
-            Vector3f euler = eskf.computeAngles();
-            if(fabsf(euler(1))<30.0f*M_PI_F/180.0f){
-                headingUpdateFlag = true;
-                theading = _t.read();
+        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); 
         }
         
-        if(gpsUpdateFlag == true){
-            eskf.updateGPS(pi, palt, vi, Rgps);
-            //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos);
-            //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
-        }else if(headingUpdateFlag == true){
-                float heading = std::atan2(-mag(1),mag(0));
-                eskf.updateHeading(heading,Rheading);
-        }else{
-            Vector3f dynacc = eskf.calcDynAcc(acc);
-            dynaccnorm2 = dynacc.squaredNorm();
-            //twelite.serial.printf("%f\r\n",sqrt(dynaccnorm2));
-            if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){
-                eskf.updateAcc(acc, RaccDyn);
-            }else{
-                eskf.updateAcc(acc, Racc);
-            }
-        }
         PIDtick.loop(); 
         
         //制御時間を計測

--- a/servo.cpp	Fri Dec 10 11:20:13 2021 +0000
+++ b/servo.cpp	Fri Jun 24 05:44:34 2022 +0000
@@ -11,47 +11,93 @@
     
     //姿勢角の所得
     euler = eskf.computeAngles();
-    rpy = euler;
-
+    
+    Vector3f vihat = eskf.getVihat();
+    Vector3f gyroBias = eskf.getGyroBias();
     //PIDへの状態量のセット
-    pitchPID.setProcessValue(rpy(1));
-    pitchratePID.setProcessValue(gyro(1));
-    rollPID.setProcessValue(rpy(0));
-    rollratePID.setProcessValue(gyro(0));
+    pitchPID.setProcessValue(euler(1));
+    pitchratePID.setProcessValue(gyro(1)-gyroBias(1));
+    rollPID.setProcessValue(euler(0));
+    rollratePID.setProcessValue(gyro(0)-gyroBias(0));
+    yawratePID.setProcessValue(gyro(2)-gyroBias(2));
+    climbratePID.setProcessValue(vihat(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
+        }
+    }
+    
     if (rc[4]>-0.3f && rc[6] < -0.3f)
     {
         //level_flight();
         //point_guide();
-        climb();
+        //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{
-        rollPID.setSetPoint(0.0f);
-        pitchPID.setSetPoint(0.0f);
+        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;
     }
     
     //舵角計算
     if(rc[4]<-0.3f){
-        de = (rc[0]-rc[1])/2.0f;
-        da = (rc[0]+rc[1])/2.0f;
-    }else{
-        de = (pitchPID.compute()+pitchratePID.compute())+(rc[0]-rc[1])/2.0f;
-        da = (rollPID.compute()+rollratePID.compute())+(rc[0]+rc[1])/2.0f;
+        de = -(rc[0]-rc[1])/2.0f;
+        da = -(rc[0]+rc[1])/2.0f;
     }
     
-    scaledServoOut[0]=de+da;
-    scaledServoOut[1]=-de+da;
+    scaledServoOut[0]=-de-da;
+    scaledServoOut[1]=de-da;
     scaledMotorOut[0]= dT;
     
-    float LP_servo = 0.2;
-    float LP_motor = 0.2;
+    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.0f,1.0f,servoPwmMin,servoPwmMax))+(1.0f-LP_servo)*servoOut[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;
@@ -63,7 +109,7 @@
     }
     
     for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;i++){
-        motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1.0f,1.0f,motorPwmMin,motorPwmMax))+(1.0f-LP_motor)*motorOut[i];
+        motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))+(1.0-LP_motor)*motorOut[i];
         if(motorOut[i]<motorPwmMin) {
             motorOut[i] = motorPwmMin;
         };
@@ -75,6 +121,7 @@
     servoLeft.pulsewidth_us(servoOut[1]); 
     servoThrust.pulsewidth_us(motorOut[0]);
 
+
     sendData2PC();
     writeSDcard();
     

--- a/setup.cpp	Fri Dec 10 11:20:13 2021 +0000
+++ b/setup.cpp	Fri Jun 24 05:44:34 2022 +0000
@@ -55,6 +55,7 @@
     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);
+    
 }
 
 void calibrate()

--- a/solaESKF.lib	Fri Dec 10 11:20:13 2021 +0000
+++ b/solaESKF.lib	Fri Jun 24 05:44:34 2022 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/HAPSRG/code/solaESKF_Eigen/#365ea9277167
+https://os.mbed.com/teams/HAPSRG/code/solaESKF_Eigen/#b241c058df83

--- a/transferData.cpp	Fri Dec 10 11:20:13 2021 +0000
+++ b/transferData.cpp	Fri Jun 24 05:44:34 2022 +0000
@@ -5,42 +5,160 @@
     sp.da = da;
     sp.de = de;
     sp.dT = dT;
-    sp.rpy[0] = rpy(0)*180.0f/M_PI_F;
-    sp.rpy[1] = rpy(1)*180.0f/M_PI_F;
-    sp.rpy[2] = rpy(2)*180.0f/M_PI_F;
-    Vector3f vihat = eskf.getVihat();
-    sp.vihat[0] = vihat(0);
-    sp.vihat[1] = vihat(1);
-    sp.vihat[2] = vihat(2);
-    pc.Send(0000, &(sp));
+    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;
+    VectorXf state = eskf.getState();
+    VectorXf variance = eskf.getVariance();
+    
+    switch(hilDataOut){
+    case 1 :
+        //posNED
+        for (int i = 0; i < 3; i++){
+            sp.state[i] = state(i);
+        }
+        sp.state[3] = 1.0f/att_dt;
+        for (int i = 0; i < 3; i++){
+            sp.variance[i] = variance(i);
+        }
+        break;
+    case 2 :
+        //velNED
+        for (int i = 0; i < 3; i++){
+            sp.state[i] = state(i+3);
+        }
+        sp.state[3] = 1.0f/att_dt;
+        for (int i = 0; i < 3; i++){
+            sp.variance[i] = variance(i+3);
+        }
+        break;
+    case 3 :
+        //quaternion
+        for (int i = 0; i < 4; i++){
+            sp.state[i] = state(i+6);
+        }
+        for (int i = 0; i < 3; i++){
+            sp.variance[i] = variance(i+6);
+        }
+        break;
+    case 4 :
+        //accBias
+        for (int i = 0; i < 3; i++){
+            sp.state[i] = state(i+10);
+        }
+        sp.state[3] = 1.0f/att_dt;
+        for (int i = 0; i < 3; i++){
+            sp.variance[i] = variance(i+9);
+        }
+        break;
+    case 5 :
+        //gyroBias
+        for (int i = 0; i < 3; i++){
+            sp.state[i] = state(i+13);
+        }
+        sp.state[3] = 1.0f/att_dt;
+        for (int i = 0; i < 3; i++){
+            sp.variance[i] = variance(i+12);
+        }
+        break;
+    case 6 :
+        //gyroBias
+        for (int i = 0; i < 3; i++){
+            sp.state[i] = state(i+16);
+        }
+        sp.state[3] = 1.0f/att_dt;
+        for (int i = 0; i < 3; i++){
+            sp.variance[i] = variance(i+15);
+        }
+        break;
+    }
+
+    if(hilFlag == true){
+        pc.Send(0000, &(sp));
+    }else{
+        pc.serial.printf("%f %f %f %f %f\r\n",sp.euler[0],sp.euler[1],sp.euler[2],1.0f/att_dt,std::atan2(-mag(1),mag(0))*180.0f/M_PI);
+    }
+    
 }
 
 void sendTelemetry()
 {
-//    Vector3f pihat = eskf.getPihat();
-//    Vector3f vihat = eskf.getVihat();
-//    tp.time=_t.read();
-//    tp.hertz = 1.0f/att_dt;
-//    tp.gpsFix = float(gps.gpsFix);
-//    for(int i = 0;i<3;i++){
-//        tp.rpy[i] = euler(i*180.0f/M_PI_F);
-//        tp.pihat[i] = pihat(i);
-//        tp.vihat[i] = vihat(i);
-//    }
-//    tp.dynaccNorm = std::sqrt(dynaccnorm2);
-//    
-//    twelite.Send(0000, &(tp));
-        
+
+    Vector3f pihat = eskf.getPihat();
+    Vector3f vihat = eskf.getVihat();
+    //Matrix wind = eskf.getWind();
+    
+    tp.pi[0] = (float)pihat(0);
+    tp.pi[1] = (float)pihat(1);
+    tp.pi[2] = (float)pihat(2);
+    tp.rpy_l[0] = euler(0);
+    tp.rpy_l[1] = euler(1);
+    tp.rpy_l[2] = euler(2);
+    tp.rpy_c[0] = euler(0);
+    tp.rpy_c[1] = euler(1);
+    tp.rpy_c[2] = euler(2);
+    tp.rpy_r[0] = euler(0);
+    tp.rpy_r[1] = euler(1);
+    tp.rpy_r[2] = euler(2);
+    tp.vi[0] = (float)vihat(0);
+    tp.vi[1] = (float)vihat(1);
+    tp.vi[2] = (float)vihat(2);
+    tp.palt = -palt;
+    tp.gps_fix = (float)gps.gpsFix;
+    /*
+    if (preflightFlag)
+    {
+        tp.mode = (float)0.0f;
+    }
+    else if (posValues[0].ap_flag == 0 || posValues[1].ap_flag == 0 || posValues[2].ap_flag == 0 )
+    {
+        tp.mode = (float)1.0f;
+    }
+    else {
+        tp.mode = (float)2.0f;
+    }
+    */
+    tp.mode = (float)1.0f;
+    tp.time = _t.read();
+    tp.gps_acc = gps.Hacc;
+    tp.vx_opt = 0.0f;
+    tp.vy_opt = 0.0f;
+    tp.dist_ir = 0.0f;
+    tp.voltage[0] = 0.0f;
+    tp.voltage[1] = 0.0f;
+    tp.current[0] = 0.0f;
+    tp.current[1] = 0.0f;
+    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.u_pitot = 0.0f;
+    
+    if (hinf_flag)
+    {
+        tp.hinf = 1.0f;
+    }
+    else
+    {
+        tp.hinf = 0.0f;
+    }
+
+    wait(0.001f);
+
+    twelite.Send(0000, &(tp));
+    //pc.printf("r: %f %f %f p: %f %f %f y: %f %f %f de: %f %f %f\r\n",posValues[0].rpy[0]*180.0f/M_PI,posValues[1].rpy[0]*180.0f/M_PI,posValues[2].rpy[0]*180.0f/M_PI,posValues[0].rpy[1]*180.0f/M_PI,posValues[1].rpy[1]*180.0f/M_PI,posValues[2].rpy[1]*180.0f/M_PI,posValues[0].rpy[2]*180.0f/M_PI,posValues[1].rpy[2]*180.0f/M_PI,posValues[2].rpy[2]*180.0f/M_PI,posValues[0].de,posValues[1].de,posValues[2].de);
 }
 
 void writeSDcard()
 {
     Vector3f pihat = eskf.getPihat();
     Vector3f vihat = eskf.getVihat();
+    //Matrix wind = eskf.getWind();
 
     lp.time = _t.read();
     lp.hertz = 1.0f/att_dt;
-//    lp.gpsFix = float(gps.gpsFix);
+    lp.gpsFix = float(gps.gpsFix);
     lp.da = da;
     lp.de = de;
     lp.dT = dT;
@@ -68,6 +186,12 @@
     lp.mag[1] = mag(1);
     lp.mag[2] = mag(2);
     lp.palt = palt;
+    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.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);
     //sd.printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",_t.read(),da,de,dT,rc[0],rc[1],rc[2],rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI, pihat(1,1),pihat(2,1),pihat(3,1),vihat(1,1),vihat(2,1),vihat(3,1));