EKF: Sync to upstream repo

3 files changed, 32 insertions, 4 deletions

diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
index 817590bab..967402fff 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
@@ -41,13 +41,17 @@
 #include <string.h>
 #include <stdio.h>
 #include <stdarg.h>
-#include <math.h>
+#include <cmath>
 #include <algorithm>
 
 #ifndef M_PI_F
 #define M_PI_F static_cast<float>(M_PI)
 #endif
 
+#ifndef isfinite
+#define isfinite(__x) std::isfinite(__x)
+#endif
+
 constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f;
 
 AttPosEKF::AttPosEKF() :
@@ -2626,11 +2630,9 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val)
         ret = val;
     }
 
-#if 0
     if (!isfinite(val)) {
         ekf_debug("constrain: non-finite!");
     }
-#endif
 
     return ret;
 }
diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
index 83a738463..284a09902 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
@@ -114,6 +114,27 @@ Mat3f Mat3f::transpose() const
     return ret;
 }
 
+void calcvelNED(float (&velNEDr)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
+{
+    velNEDr[0] = gpsGndSpd*cosf(gpsCourse);
+    velNEDr[1] = gpsGndSpd*sinf(gpsCourse);
+    velNEDr[2] = gpsVelD;
+}
+
+void calcposNED(float (&posNEDr)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference)
+{
+    posNEDr[0] = earthRadius * (lat - latReference);
+    posNEDr[1] = earthRadius * cos(latReference) * (lon - lonReference);
+    posNEDr[2] = -(hgt - hgtReference);
+}
+
+void calcLLH(float posNEDi[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef)
+{
+    lat = latRef + (double)posNEDi[0] * earthRadiusInv;
+    lon = lonRef + (double)posNEDi[1] * earthRadiusInv / cos(latRef);
+    hgt = hgtRef - posNEDi[2];
+}
+
 // overload + operator to provide a vector addition
 Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2)
 {
diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h
index c137209ff..788fb85ec 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h
@@ -45,7 +45,6 @@
 #define GRAVITY_MSS 9.80665f
 #define deg2rad 0.017453292f
 #define rad2deg 57.295780f
-#define pi 3.141592657f
 #define earthRate 0.000072921f
 #define earthRadius 6378145.0
 #define earthRadiusInv  1.5678540e-7
@@ -128,3 +127,9 @@ struct ekf_status_report {
 };
 
 void ekf_debug(const char *fmt, ...);
+
+void calcvelNED(float (&velNEDr)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
+
+void calcposNED(float (&posNEDr)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference);
+
+void calcLLH(float posNEDi[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);