Ignore:
Timestamp:
Jul 28, 2016, 5:55:31 PM (5 years ago)
Author:
Sanahuja Guillaume
Message:

simu gps

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairSensorActuator/src/geodesie.cpp

    r15 r55  
    322322}
    323323
     324//from https://gist.github.com/klucar/1536194
     325void Geodesie::ECEF_2_Geographique(double x, double y, double z,
     326                                   double &longitude, double &latitude, double &he) {
     327
     328  double asq=pow(GRS_a, 2);
     329  double esq=pow(GRS_e, 2);
     330  double b = sqrt( asq * (1-esq) );
     331  double bsq = pow(b,2);
     332  double ep = sqrt( (asq - bsq)/bsq);
     333  double p = sqrt( pow(x,2) + pow(y,2) );
     334  double th = atan2(GRS_a*z, b*p);
     335
     336  longitude = atan2(y,x);
     337  latitude = atan2( (z + pow(ep,2)*b*pow(sin(th),3) ), (p - esq*GRS_a*pow(cos(th),3)) );
     338  double N = GRS_a/( sqrt(1-esq*pow(sin(latitude),2)) );
     339  double alt = p / cos(latitude) - N;
     340
     341  // mod longitude to 0-2pi
     342  if(longitude<0) longitude +=2*M_PI;
     343
     344  // correction for altitude near poles left out.
     345}
     346
    324347////////////////////////////////////////////////////////////////////////
    325348void Geodesie::ECEF_2_ENU(double x, double y, double z, double &e, double &n,
     
    351374  C.Apply(x, y, z, e, n, u);
    352375}
     376
     377void Geodesie::ENU_2_ECEF(double e, double n,double u,
     378                          double &x, double &y, double &z,
     379                          double lon0, double lat0, double he0) {
     380  double slat = std::sin(lat0);//phi
     381  double clat = std::cos(lat0);
     382  double slon = std::sin(lon0);//lambda
     383  double clon = std::cos(lon0);
     384
     385  Geodesie::Matrice C;
     386  C.c0_l0 = -slon;
     387  C.c1_l0 = -clon*slat;
     388  C.c2_l0 = clon*clat;
     389
     390  C.c0_l1 = clon;
     391  C.c1_l1 = -slon * slat;
     392  C.c2_l1 = slon*clat;
     393
     394  C.c0_l2 = 0;
     395  C.c1_l2 = clat;
     396  C.c2_l2 = slat;
     397
     398  C.Apply(e, n, u,x,y,z);
     399
     400  double x0, y0, z0;
     401  Geographique_2_ECEF(lon0, lat0, he0, x0, y0, z0);
     402
     403  x += x0;
     404  y += y0;
     405  z += z0;
     406}
Note: See TracChangeset for help on using the changeset viewer.