Diff: run.cpp

Revision:

94:a90b4b920fa9

Parent:

93:b0f7a1e91476

Child:

95:98dbbcc6b39d

diff -r b0f7a1e91476 -r a90b4b920fa9 run.cpp
--- a/run.cpp	Tue Feb 01 07:48:56 2022 +0000
+++ b/run.cpp	Tue Feb 01 15:15:46 2022 +0000
@@ -110,11 +110,11 @@
         getIMUval();
         eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
         eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
-        //Vector3 pi(0.0f, 0.0f, 0.0f);
-        //Vector3 vi(0.0f, 0.0f, 0.0f);
-        //eskf.updateGPS(MatrixMath::Vector2mat(pi),0.0f,MatrixMath::Vector2mat(vi),Rgps);
-        //float heading = atan2f(-mag.y,mag.x);
-        //eskf.updateHeading(heading,Rheading);
+        Vector3 pi(0.0f, 0.0f, 0.0f);
+        Vector3 vi(0.0f, 0.0f, 0.0f);
+        eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
+        float heading = atan2f(-mag.y,mag.x);
+        eskf.updateHeading(heading,Rheading);
         Matrix Raccpreflight(3,3);
         setDiag(Raccpreflight,5.0f);
         eskf.updateAcc(MatrixMath::Vector2mat(acc),Raccpreflight);
@@ -144,15 +144,6 @@
             pc.printf("sbus\r\n");
             preflightCheck = false;
         }
-        // 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;
-        }
-        */
         if(!(updateValues.gps_fix == 0x03)){
             pc.printf("gps\r\n");
             preflightCheck = false;