Eigen Revision

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

Files at this revision

API Documentation at this revision

Comitter:
NaotoMorita
Date:
Fri Jun 24 05:44:34 2022 +0000
Parent:
142:e1b8357d1ea4
Commit message:
complete

Changed in this revision

GPSUBX_UART.lib Show annotated file Show diff for this revision Revisions of this file
PIDcontroller.lib Show annotated file Show diff for this revision Revisions of this file
autopilot.cpp Show annotated file Show diff for this revision Revisions of this file
global.cpp Show annotated file Show diff for this revision Revisions of this file
global.hpp Show annotated file Show diff for this revision Revisions of this file
gps.cpp Show annotated file Show diff for this revision Revisions of this file
hil.cpp Show annotated file Show diff for this revision Revisions of this file
preflight.cpp Show annotated file Show diff for this revision Revisions of this file
run.cpp Show annotated file Show diff for this revision Revisions of this file
servo.cpp Show annotated file Show diff for this revision Revisions of this file
setup.cpp Show annotated file Show diff for this revision Revisions of this file
solaESKF.lib Show annotated file Show diff for this revision Revisions of this file
transferData.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r e1b8357d1ea4 -r 53808e4e684c GPSUBX_UART.lib
--- 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
diff -r e1b8357d1ea4 -r 53808e4e684c PIDcontroller.lib
--- 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
diff -r e1b8357d1ea4 -r 53808e4e684c autopilot.cpp
--- 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);
diff -r e1b8357d1ea4 -r 53808e4e684c global.cpp
--- 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;
diff -r e1b8357d1ea4 -r 53808e4e684c global.hpp
--- 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();
 
diff -r e1b8357d1ea4 -r 53808e4e684c gps.cpp
--- 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;
         }
     }
 }
diff -r e1b8357d1ea4 -r 53808e4e684c hil.cpp
--- 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);
diff -r e1b8357d1ea4 -r 53808e4e684c preflight.cpp
--- /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
diff -r e1b8357d1ea4 -r 53808e4e684c run.cpp
--- 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(); 
         
         //制御時間を計測
diff -r e1b8357d1ea4 -r 53808e4e684c servo.cpp
--- 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();
     
diff -r e1b8357d1ea4 -r 53808e4e684c setup.cpp
--- 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()
diff -r e1b8357d1ea4 -r 53808e4e684c solaESKF.lib
--- 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
diff -r e1b8357d1ea4 -r 53808e4e684c transferData.cpp
--- 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));