38 #define M_PI 3.141592653589793238462643383280 50 : _callback(callback), _callback_user(callback_user)
69 void GPSHelper::ECEF2lla(
double ecef_x,
double ecef_y,
double ecef_z,
double &latitude,
double &longitude,
73 constexpr
double a = 6378137.;
74 constexpr
double e = 8.1819190842622e-2;
76 constexpr
double asq = a * a;
77 constexpr
double esq = e * e;
83 double b =
sqrt(asq * (1. - esq));
85 double ep =
sqrt((asq - bsq) / bsq);
86 double p =
sqrt(x * x + y * y);
87 double th =
atan2(a * z, b * p);
89 longitude =
atan2(y, x);
90 double sin_th =
sin(th);
91 double cos_th =
cos(th);
92 latitude =
atan2(z + ep * ep * b * sin_th * sin_th * sin_th, p - esq * a * cos_th * cos_th * cos_th);
93 double sin_lat =
sin(latitude);
94 double N = a /
sqrt(1. - esq * sin_lat * sin_lat);
95 altitude = (float)(p /
cos(latitude) - N);
98 longitude *= 180. /
M_PI;
99 latitude *= 180. /
M_PI;
Dual< Scalar, N > cos(const Dual< Scalar, N > &a)
int(* GPSCallbackPtr)(GPSCallbackType type, void *data1, int data2, void *user)
Callback function for platform-specific stuff.
uint64_t _interval_rate_start
uint8_t _rate_count_lat_lon
GPSHelper(GPSCallbackPtr callback, void *callback_user)
#define gps_absolute_time
Get the current time in us.
Dual< Scalar, N > sin(const Dual< Scalar, N > &a)
static void ECEF2lla(double ecef_x, double ecef_y, double ecef_z, double &latitude, double &longitude, float &altitude)
Convert an ECEF (Earth Centered Earth Fixed) coordinate to LLA WGS84 (Lat, Lon, Alt).
Dual< Scalar, N > sqrt(const Dual< Scalar, N > &a)
Dual< Scalar, N > atan2(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)