fucking git salsa

Merge branch 'master' of github.com:paparazzi/paparazzi

Conflicts:
	sw/airborne/math/pprz_geodetic_double.c

sw/airborne/math/pprz_geodetic_double.c

@@ -3,6 +3,96 @@
 #include <math.h>
 #include "std.h" /* for RadOfDeg */
 
+
+/* 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       */
@@ -135,6 +225,7 @@ 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));
 }
+<<<<<<< HEAD
 
 
 
@@ -224,3 +315,5 @@ void lla_of_utm(struct LlaCoor_d* out, struct UTMCoor_d* in) {
   out->lat = inverse_isometric_latitude(il, E, 1e-8);
 
 }
+=======
+>>>>>>> 62943d6115cdac629878cb488a8592aab4a7ded4