Changeset 55 in flair-src for trunk/lib/FlairSensorActuator/src/geodesie.cpp

Timestamp:

07/28/16 17:55:31 (8 years ago)

Author:

Sanahuja Guillaume

Message:

simu gps

File:

• trunk/lib/FlairSensorActuator/src/geodesie.cpp (modified) (2 diffs)

trunk/lib/FlairSensorActuator/src/geodesie.cpp

@@ -322 +322 @@
 }
 
+//from https://gist.github.com/klucar/1536194
+void Geodesie::ECEF_2_Geographique(double x, double y, double z,
+                                   double &longitude, double &latitude, double &he) {
+
+  double asq=pow(GRS_a, 2);
+  double esq=pow(GRS_e, 2);
+  double b = sqrt( asq * (1-esq) );
+  double bsq = pow(b,2);
+  double ep = sqrt( (asq - bsq)/bsq);
+  double p = sqrt( pow(x,2) + pow(y,2) );
+  double th = atan2(GRS_a*z, b*p);
+
+  longitude = atan2(y,x);
+  latitude = atan2( (z + pow(ep,2)*b*pow(sin(th),3) ), (p - esq*GRS_a*pow(cos(th),3)) );
+  double N = GRS_a/( sqrt(1-esq*pow(sin(latitude),2)) );
+  double alt = p / cos(latitude) - N;
+
+  // mod longitude to 0-2pi
+  if(longitude<0) longitude +=2*M_PI;
+
+  // correction for altitude near poles left out.
+}
+
 ////////////////////////////////////////////////////////////////////////
 void Geodesie::ECEF_2_ENU(double x, double y, double z, double &e, double &n,
@@ -351 +374 @@
   C.Apply(x, y, z, e, n, u);
 }
+
+void Geodesie::ENU_2_ECEF(double e, double n,double u,
+                          double &x, double &y, double &z,
+                          double lon0, double lat0, double he0) {
+  double slat = std::sin(lat0);//phi
+  double clat = std::cos(lat0);
+  double slon = std::sin(lon0);//lambda
+  double clon = std::cos(lon0);
+
+  Geodesie::Matrice C;
+  C.c0_l0 = -slon;
+  C.c1_l0 = -clon*slat;
+  C.c2_l0 = clon*clat;
+
+  C.c0_l1 = clon;
+  C.c1_l1 = -slon * slat;
+  C.c2_l1 = slon*clat;
+
+  C.c0_l2 = 0;
+  C.c1_l2 = clat;
+  C.c2_l2 = slat;
+
+  C.Apply(e, n, u,x,y,z);
+
+  double x0, y0, z0;
+  Geographique_2_ECEF(lon0, lat0, he0, x0, y0, z0);
+
+  x += x0;
+  y += y0;
+  z += z0;
+}