HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

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

Diff: gps.cpp

Revision:
98:bdaa6bbfb1d9
Parent:
97:2b3242c2dd85
Child:
99:98b892ca70ec
--- a/gps.cpp	Wed Nov 03 09:28:38 2021 +0000
+++ b/gps.cpp	Wed Nov 03 14:59:26 2021 +0000
@@ -27,11 +27,12 @@
     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)
+    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
     {