From cc11e8a0985f1a9790510fb3f19447a6c6946ba1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 3 Apr 2015 17:01:32 +0200
Subject: EKF: Sync to upstream repo

---
 .../ekf_att_pos_estimator/estimator_22states.cpp    |  8 +++++---
 .../ekf_att_pos_estimator/estimator_utilities.cpp   | 21 +++++++++++++++++++++
 .../ekf_att_pos_estimator/estimator_utilities.h     |  7 ++++++-
 3 files changed, 32 insertions(+), 4 deletions(-)

(limited to 'src')

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);
-- 
cgit v1.2.3