From 4be39137cf2609eab16b209badffd0106a8b81b6 Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Wed, 23 Feb 2011 12:49:17 +0100
Subject: [PATCH] added lla_of_utm function from Antoine

---
 sw/airborne/math/pprz_geodetic_double.c | 93 ++++++++++++++++++++++++-
 sw/airborne/math/pprz_geodetic_double.h | 11 +++
 2 files changed, 101 insertions(+), 3 deletions(-)

diff --git a/sw/airborne/math/pprz_geodetic_double.c b/sw/airborne/math/pprz_geodetic_double.c
index d8570396b0a..e2c42348eaf 100644
--- a/sw/airborne/math/pprz_geodetic_double.c
+++ b/sw/airborne/math/pprz_geodetic_double.c
@@ -2,6 +2,96 @@
 
 #include <math.h>
 
+
+/* Computation for the WGS84 geoid only */
+#define E 0.08181919106
+#define K0 0.9996
+#define DELTA_EAST  500000.
+#define DELTA_NORTH 0.
+#define A 6378137.0
+#define N (K0*A)
+
+#define LambdaOfUtmZone(utm_zone) RadOfDeg((utm_zone-1)*6-180+3)
+
+static const float serie_coeff_proj_mercator[5] = {
+  0.99832429842242842444,
+  0.00083632803657738403,
+  0.00000075957783563707,
+  0.00000000119563131778,
+  0.00000000000241079916
+};
+
+static inline double isometric_latitude(double phi, double e) {
+  return log (tan (M_PI_4 + phi / 2.0)) - e / 2.0 * log((1.0 + e * sin(phi)) / (1.0 - e * sin(phi)));
+}
+
+static inline double isometric_latitude_fast(double phi) {
+  return log (tan (M_PI_4 + phi / 2.0));
+}
+
+static inline double inverse_isometric_latitude(double lat, double e, double epsilon) {
+  double exp_l = exp(lat);
+  double phi0 = 2 * atan(exp_l) - M_PI_2;
+  double phi_;
+  uint8_t max_iter = 3; /* To be sure to return */
+
+  do {
+    phi_ = phi0;
+    double sin_phi = e * sin(phi_);
+    phi0 = 2 * atan (pow((1 + sin_phi) / (1. - sin_phi), e/2.) * exp_l) - M_PI_2;
+    max_iter--;
+  }
+  while (max_iter && fabs(phi_ - phi0) > epsilon);
+
+  return phi0;
+}
+
+#define CI(v) {                                 \
+    double tmp = v.x;                           \
+    v.x = -v.y;                                 \
+    v.y = tmp;                                  \
+  }
+
+#define CExp(v) {                               \
+    double e = exp(v.x);                        \
+    v.x = e*cosf(v.y);                          \
+    v.y = e*sinf(v.y);                          \
+  }
+
+#define CSin(v) {                               \
+    CI(v);                                      \
+    struct DoubleVect2 vstar = {-v.x, -v.y};    \
+    CExp(v);                                    \
+    CExp(vstar);                                \
+    VECT2_SUB(v, vstar);                        \
+    VECT2_SMUL(v, v, -0.5);                     \
+    CI(v);                                      \
+  }
+
+
+
+void lla_of_utm(struct LlaCoor_d* out, struct UTMCoor_d* in) {
+
+  //  struct DoubleVect2 v = {in->east - YS, in->north - XS};
+  struct DoubleVect2 v = {in->north - DELTA_NORTH, in->east - DELTA_EAST};
+  double scale = 1 / N / serie_coeff_proj_mercator[0];
+  VECT2_SMUL(v, v, scale);
+
+  // first order taylor serie of something ?
+  struct DoubleVect2 v1;
+  VECT2_SMUL(v1, v, 2.);
+  CSin(v1);
+  VECT2_SMUL(v1, v1, serie_coeff_proj_mercator[1]);
+  VECT2_SUB(v, v1);
+
+  double lambda_c = LambdaOfUtmZone(in->zone);
+  out->lon = lambda_c + atan(sinh(v.y) / cos(v.x));
+  double phi = asin (sin(v.x) / cosh(v.y));
+  double il = isometric_latitude_fast(phi);
+  out->lat = inverse_isometric_latitude(il, E, 1e-8);
+
+}
+
 void ltp_def_from_ecef_d(struct LtpDef_d* def, struct EcefCoor_d* ecef) {
 
   /* store the origin of the tangeant plane       */
@@ -134,6 +224,3 @@ double gc_of_gd_lat_d(double gd_lat, double hmsl) {
   double ls = atan(c2*tan(gd_lat));
   return atan2(hmsl*sin(gd_lat) + a*sin(ls), hmsl*cos(gd_lat) + a*cos(ls));
 }
-
-
-
diff --git a/sw/airborne/math/pprz_geodetic_double.h b/sw/airborne/math/pprz_geodetic_double.h
index 2bed3451c21..4f2b8eba176 100644
--- a/sw/airborne/math/pprz_geodetic_double.h
+++ b/sw/airborne/math/pprz_geodetic_double.h
@@ -32,6 +32,7 @@
 
 #include "pprz_geodetic.h"
 #include "pprz_algebra_double.h"
+#include "std.h"
 
 /**
  * @brief vector in EarthCenteredEarthFixed coordinates
@@ -74,6 +75,15 @@ struct EnuCoor_d {
   double z; ///< in meters
 };
 
+/**
+ * @brief position in UTM coordinates
+ * Units: meters */
+struct UTMCoor_d {
+  double north; ///< in meters
+  double east; ///< in meters
+  uint8_t zone; ///< UTM zone number
+};
+
 /**
  * @brief definition of the local (flat earth) coordinate system
  * @details Defines the origin of the local coordinate system
@@ -85,6 +95,7 @@ struct LtpDef_d {
   struct DoubleMat33 ltp_of_ecef; ///< rotation from ECEF to local frame
 };
 
+extern void lla_of_utm(struct LlaCoor_d* out, struct UTMCoor_d* in);
 extern void ltp_def_from_ecef_d(struct LtpDef_d* def, struct EcefCoor_d* ecef);
 extern void lla_of_ecef_d(struct LlaCoor_d* out, struct EcefCoor_d* in);
 extern void ecef_of_lla_d(struct EcefCoor_d* out, struct LlaCoor_d* in);