Diff: gps.cpp

Revision:

106:2d854e92cebb

Parent:

100:7589b663d462

Child:

139:b378528c05f2

--- a/gps.cpp	Wed Nov 10 06:35:10 2021 +0000
+++ b/gps.cpp	Fri Nov 12 09:03:41 2021 +0000
@@ -4,41 +4,23 @@
 {
     gpsUpdateFlag = false;
     gps.Update();
-    if (gps.iTOW_VELNED != itow_velned)
-    {
-        itow_velned = gps.iTOW_VELNED;
-        vi.x = gps.VelocityNED.x;
-        vi.y = gps.VelocityNED.y;
-        vi.z = gps.VelocityNED.z;
-    }
-    if (gps.iTOW_POSLLH != itow_posllh)
-    {
-        itow_posllh = gps.iTOW_POSLLH;
-        gps.Calculate();
-        change_refpos();
-        pi.x = gps.PositionNED.x;
-        pi.y = gps.PositionNED.y;
-        pi.z = gps.PositionNED.z;
-        gpsUpdateFlag = true;
+    if (gps.iTOW_STATUS != itow_status){
+        itow_status = gps.iTOW_STATUS;
+        if(gps.gpsFix == 0x02 || gps.gpsFix == 0x03){
+            vi.x = gps.VelocityNED.x;
+            vi.y = gps.VelocityNED.y;
+            vi.z = gps.VelocityNED.z;
+            if(gpsLlh0Fixed == false){
+                gps.CalculateUnit();
+                gpsLlh0Fixed = true;
+            }else{
+                gps.Calculate();
+            }
+            pi.x = gps.PositionNED.x;
+            pi.y = gps.PositionNED.y;
+            pi.z = gps.PositionNED.z;
+            gpsUpdateFlag = true;
+        }
     }
 }
 
-//20m以上の変位で基準位置更新
-void change_refpos()
-{
-    float x_dist = gps.PositionNED.x - pi.x;
-    float y_dist = gps.PositionNED.y - pi.y;
-    float z_dist = gps.PositionNED.z - pi.z;
-    float dist2 = x_dist*x_dist + y_dist*y_dist + z_dist*z_dist;
-    if (dist2 >= 400.0f)
-    {
-        gps.CalculateUnit();
-        eskf.setPihat(0.0f, 0.0f, 0.0f);
-        //palt0 = palt;
-        
-    }
-    else
-    {
-        //何もしない
-    }
-}
\ No newline at end of file