Diff: gps.cpp

Revision:

97:2b3242c2dd85

Parent:

96:3645f8f9bdd3

Child:

98:bdaa6bbfb1d9

--- a/gps.cpp	Tue Nov 02 14:29:21 2021 +0000
+++ b/gps.cpp	Wed Nov 03 09:28:38 2021 +0000
@@ -3,11 +3,38 @@
 void getGPSval()
 {
     gps.Update();
-    gps.Calculate();
-    vi.x = gps.VelocityNED.x;
-    vi.y = gps.VelocityNED.y;
-    vi.z = gps.VelocityNED.z;
-    pi.x = gps.PositionNED.x;
-    pi.y = gps.PositionNED.y;
-    pi.z = gps.PositionNED.z;
+    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;
+        change_refpos();
+        gps.Calculate();
+        pi.x = gps.PositionNED.x;
+        pi.y = gps.PositionNED.y;
+        pi.z = gps.PositionNED.z;
+    }
+}
+
+//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 dist = sqrt(x_dist*x_dist + y_dist*y_dist + z_dist*z_dist);
+    if (dist >= 20.0f)
+    {
+        gps.CalculateUnit();
+        eskf.setPihat(0.0f, 0.0f, 0.0f);
+    }
+    else
+    {
+        //何もしない
+    }
 }
\ No newline at end of file