Diff: run.cpp

Revision:

106:36458fb9b5b7

Parent:

105:aaaed895ffaf

Child:

107:78e6f7bee68e

diff -r aaaed895ffaf -r 36458fb9b5b7 run.cpp
--- a/run.cpp	Tue Mar 01 09:19:27 2022 +0000
+++ b/run.cpp	Wed Mar 02 09:23:39 2022 +0000
@@ -69,13 +69,10 @@
                 tgps = _t.read();
             }
             
-            headingUpdateFlag = false;
-            if(tstart-theading>0.05f){
-                Matrix euler = eskf.computeAngles();
-                if(fabsf(euler(2,1))<30.0f*M_PI/180.0f){
-                    headingUpdateFlag = true;
-                    theading = _t.read();
-                }
+            sensorUpdateFlag = false;
+            if(tstart-tsensors>0.05f){
+                sensorUpdateFlag = true;
+                tsensors = _t.read();
             }
             
             if(gpsUpdateFlag==true){
@@ -84,25 +81,39 @@
                 Rgps(1, 1) = (updateValues.gps_acc * 0.849f) * (updateValues.gps_acc * 0.849f);
                 Rgps(2, 2) = (updateValues.gps_acc * 0.849f) * (updateValues.gps_acc * 0.849f);
                 eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
-                pc.printf("gps\r\n");
-            }else if(headingUpdateFlag == true){
-                    float heading = atan2f(-mag.y,mag.x);
-                    eskf.updateHeading(heading,Rheading);
+                //pc.printf("gps\r\n");
+            }else if(sensorUpdateFlag == true){
+                if(sensorUpdateID==1){
+                    Matrix euler = eskf.computeAngles();
+                    if(fabsf(euler(2,1))<30.0f*M_PI/180.0f){
+                        float heading = atan2f(-mag.y,mag.x);
+                        eskf.updateHeading(heading,Rheading);
+                        //pc.printf("heading\r\n");
+                    }
+                    sensorUpdateID += 1;
+                }else{
+                    if(updateValues.dist_ir<1.0f){
+                        Matrix vbOpt(2,1);
+                        vbOpt(1,1) = updateValues.vx_opt;
+                        vbOpt(2,1) = updateValues.vx_opt;
+                        eskf.updateVb(vbOpt,Ropt);
+                        //pc.printf("opt\r\n");
+                    }
+                    sensorUpdateID=1;
+                }
             }else{
                 Matrix dynacc = eskf.calcDynAcc(MatrixMath::Vector2mat(acc));
                 dynaccnorm2 = dynacc(1,1)*dynacc(1,1)+dynacc(2,1)*dynacc(2,1)+dynacc(3,1)*dynacc(3,1);
                 if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){
                     eskf.updateAcc(MatrixMath::Vector2mat(acc),RaccDyn);
+                    //pc.printf("dyn acc\r\n");
                 }else{
                     eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
-                }
+                    //pc.printf("static acc\r\n");
+                    }
+                
             }
-            if(updateValues.dist_ir<1.0f){
-                Matrix vbOpt(2,1);
-                vbOpt(1,1) = opt_vx;
-                vbOpt(2,1) = opt_vy;
-                eskf.updateVb(vbOpt,Ropt);
-            }
+
             
         }