Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-29 19:17:28 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[math] added and updated utm helper funtions

Browse Source
This commit is contained in:
kirkscheper
2016-04-24 13:20:47 +02:00
committed by Felix Ruess
parent f0f1d307c2
commit df91322a19
8 changed files with 267 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/math/pprz_geodetic_double.c
+79 -23
View File
@@ -33,8 +33,7 @@
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef) void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
{ {
/* store the origin of the tangent plane */
/* store the origin of the tangeant plane */
VECT3_COPY(def->ecef, *ecef); VECT3_COPY(def->ecef, *ecef);
/* compute the lla representation of the origin */ /* compute the lla representation of the origin */
lla_of_ecef_d(&def->lla, &def->ecef); lla_of_ecef_d(&def->lla, &def->ecef);
@@ -55,6 +54,31 @@ void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
} }
void ltp_def_from_lla_d(struct LtpDef_d *def, struct LlaCoor_d *lla)
{
/* store the origin of the tangent plane */
LLA_COPY(def->lla, *lla);
/* compute the ecef representation of the origin */
ecef_of_lla_d(&def->ecef, &def->lla);
/* store the rotation matrix */
const double sin_lat = sin(def->lla.lat);
const double cos_lat = cos(def->lla.lat);
const double sin_lon = sin(def->lla.lon);
const double cos_lon = cos(def->lla.lon);
def->ltp_of_ecef.m[0] = -sin_lon;
def->ltp_of_ecef.m[1] = cos_lon;
/* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
def->ltp_of_ecef.m[2] = 0.;
def->ltp_of_ecef.m[3] = -sin_lat * cos_lon;
def->ltp_of_ecef.m[4] = -sin_lat * sin_lon;
def->ltp_of_ecef.m[5] = cos_lat;
def->ltp_of_ecef.m[6] = cos_lat * cos_lon;
def->ltp_of_ecef.m[7] = cos_lat * sin_lon;
def->ltp_of_ecef.m[8] = sin_lat;
}
/* http://en.wikipedia.org/wiki/Geodetic_system */ /* http://en.wikipedia.org/wiki/Geodetic_system */
void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef) void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef)
{ {
@@ -196,7 +220,7 @@ double gc_of_gd_lat_d(double gd_lat, double hmsl)
#include "math/pprz_geodetic_utm.h" #include "math/pprz_geodetic_utm.h"
static inline double UNUSED isometric_latitude_d(double phi, double e) static inline double isometric_latitude_d(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))); return log(tan(M_PI_4 + phi / 2.0)) - e / 2.0 * log((1.0 + e * sin(phi)) / (1.0 - e * sin(phi)));
} }
@@ -224,48 +248,80 @@ static inline double inverse_isometric_latitude_d(double lat, double e, double e
} }
#define CI(v) { \ #define CI(v) { \
double tmp = v.x; \ double tmp = v.x; \
v.x = -v.y; \ v.x = -v.y; \
v.y = tmp; \ v.y = tmp; \
} }
#define CExp(v) { \ #define CExp(v) { \
double e = exp(v.x); \ double e = exp(v.x);\
v.x = e*cosf(v.y); \ v.x = e*cos(v.y); \
v.y = e*sinf(v.y); \ v.y = e*sin(v.y); \
} }
#define CSin(v) { \ #define CSin(v) { \
CI(v); \ CI(v); \
struct DoubleVect2 vstar = {-v.x, -v.y}; \ struct DoubleVect2 vstar = {-v.x, -v.y}; \
CExp(v); \ CExp(v); \
CExp(vstar); \ CExp(vstar); \
VECT2_SUB(v, vstar); \ VECT2_SUB(v, vstar);\
VECT2_SMUL(v, v, -0.5); \ VECT2_SMUL(v, v, -0.5); \
CI(v); \ CI(v); \
} }
void utm_of_lla_d(struct UtmCoor_d *utm, struct LlaCoor_d *lla)
{
// compute zone if not initialised
if (utm->zone == 0) {
utm->zone = UtmZoneOfLlaLonRad(lla->lon);
}
double lambda_c = LambdaOfUtmZone(utm->zone);
double ll = isometric_latitude_d(lla->lat , E);
double dl = lla->lon - lambda_c;
double phi_ = asin(sin(dl) / cosh(ll));
double ll_ = isometric_latitude_fast_d(phi_);
double lambda_ = atan(sinh(ll) / cos(dl));
struct DoubleVect2 z_ = { lambda_, ll_ };
VECT2_SMUL(z_, z_, serie_coeff_proj_mercator[0]);
int8_t k;
for (k = 1; k < 3; k++) {
struct DoubleVect2 z = { lambda_, ll_ };
VECT2_SMUL(z, z, 2.*k);
CSin(z);
VECT2_SMUL(z, z, serie_coeff_proj_mercator[k]);
VECT2_ADD(z_, z);
}
VECT2_SMUL(z_, z_, N);
utm->east = DELTA_EAST + z_.y;
utm->north = DELTA_NORTH + z_.x;
// copy alt above reference ellipsoid
utm->alt = lla->alt;
}
void lla_of_utm_d(struct LlaCoor_d *lla, struct UtmCoor_d *utm) void lla_of_utm_d(struct LlaCoor_d *lla, struct UtmCoor_d *utm)
{ {
struct DoubleVect2 z = {utm->north - DELTA_NORTH, utm->east - DELTA_EAST};
struct DoubleVect2 v = {utm->north - DELTA_NORTH, utm->east - DELTA_EAST};
double scale = 1 / N / serie_coeff_proj_mercator[0]; double scale = 1 / N / serie_coeff_proj_mercator[0];
VECT2_SMUL(v, v, scale); VECT2_SMUL(z, z, scale);
// first order taylor serie of something ? int8_t k;
struct DoubleVect2 v1; for(k = 1; k<2; k++)
VECT2_SMUL(v1, v, 2.); {
CSin(v1); struct DoubleVect2 z_;
VECT2_SMUL(v1, v1, serie_coeff_proj_mercator[1]); VECT2_SMUL(z_, z, 2.*k);
VECT2_SUB(v, v1); CSin(z_);
VECT2_SMUL(z_, z_, serie_coeff_proj_mercator_inverse[k]);
VECT2_SUB(z, z_);
}
double lambda_c = LambdaOfUtmZone(utm->zone); double lambda_c = LambdaOfUtmZone(utm->zone);
lla->lon = lambda_c + atan(sinh(v.y) / cos(v.x)); lla->lon = lambda_c + atan(sinh(z.y) / cos(z.x));
double phi = asin(sin(v.x) / cosh(v.y)); double phi = asin(sin(z.x) / cosh(z.y));
double il = isometric_latitude_fast_d(phi); double il = isometric_latitude_fast_d(phi);
lla->lat = inverse_isometric_latitude_d(il, E, 1e-8); lla->lat = inverse_isometric_latitude_d(il, E, 1e-8);
// copy alt above reference ellipsoid // copy alt above reference ellipsoid
lla->alt = utm->alt; lla->alt = utm->alt;
} }
sw/airborne/math/pprz_geodetic_double.h
+3 -1
View File
@@ -101,8 +101,10 @@ struct LtpDef_d {
double hmsl; ///< height in meters above mean sea level double hmsl; ///< height in meters above mean sea level
}; };
extern void lla_of_utm_d(struct LlaCoor_d *out, struct UtmCoor_d *in); extern void lla_of_utm_d(struct LlaCoor_d *lla, struct UtmCoor_d *utm);
extern void utm_of_lla_d(struct UtmCoor_d *utm, struct LlaCoor_d *lla);
extern void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef); extern void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef);
extern void ltp_def_from_lla_d(struct LtpDef_d *def, struct LlaCoor_d *lla);
extern void lla_of_ecef_d(struct LlaCoor_d *out, struct EcefCoor_d *in); 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); extern void ecef_of_lla_d(struct EcefCoor_d *out, struct LlaCoor_d *in);
sw/airborne/math/pprz_geodetic_float.c
+25 -21
View File
@@ -35,8 +35,7 @@
void ltp_def_from_ecef_f(struct LtpDef_f *def, struct EcefCoor_f *ecef) void ltp_def_from_ecef_f(struct LtpDef_f *def, struct EcefCoor_f *ecef)
{ {
/* store the origin of the tangent plane */
/* store the origin of the tangeant plane */
VECT3_COPY(def->ecef, *ecef); VECT3_COPY(def->ecef, *ecef);
/* compute the lla representation of the origin */ /* compute the lla representation of the origin */
lla_of_ecef_f(&def->lla, &def->ecef); lla_of_ecef_f(&def->lla, &def->ecef);
@@ -60,7 +59,7 @@ void ltp_def_from_ecef_f(struct LtpDef_f *def, struct EcefCoor_f *ecef)
void ltp_def_from_lla_f(struct LtpDef_f *def, struct LlaCoor_f *lla) void ltp_def_from_lla_f(struct LtpDef_f *def, struct LlaCoor_f *lla)
{ {
/* store the origin of the tangeant plane */ /* store the origin of the tangent plane */
LLA_COPY(def->lla, *lla); LLA_COPY(def->lla, *lla);
/* compute the ecef representation of the origin */ /* compute the ecef representation of the origin */
ecef_of_lla_f(&def->ecef, &def->lla); ecef_of_lla_f(&def->ecef, &def->lla);
@@ -125,7 +124,7 @@ void ned_of_lla_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct LlaC
} }
/* /*
* not enought precision with float - use double * not enough precision with float - use double
*/ */
void ecef_of_enu_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu) void ecef_of_enu_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu)
{ {
@@ -272,49 +271,54 @@ struct complex { float re; float im; };
#define CExp(z) { float e = exp(z.re); z.re = e*cos(z.im); z.im = e*sin(z.im); } #define CExp(z) { float e = exp(z.re); z.re = e*cos(z.im); z.im = e*sin(z.im); }
/* Expanded #define CSin(z) { CI(z); struct complex _z = {-z.re, -z.im}; CExp(z); CExp(_z); CSub(_z, z); CScal(-0.5, z); CI(z); } */ /* Expanded #define CSin(z) { CI(z); struct complex _z = {-z.re, -z.im}; CExp(z); CExp(_z); CSub(_z, z); CScal(-0.5, z); CI(z); } */
#define CSin(z) { CI(z); struct complex _z = {-z.re, -z.im}; float e = exp(z.re); float cos_z_im = cos(z.im); z.re = e*cos_z_im; float sin_z_im = sin(z.im); z.im = e*sin_z_im; _z.re = cos_z_im/e; _z.im = -sin_z_im/e; CSub(_z, z); CScal(-0.5, z); CI(z); } #define CSin(z) { CI(z); struct complex _z = {-z.re, -z.im}; float e = exp(z.re); float cos_z_im = cosf(z.im); z.re = e*cos_z_im; float sin_z_im = sinf(z.im); z.im = e*sin_z_im; _z.re = cos_z_im/e; _z.im = -sin_z_im/e; CSub(_z, z); CScal(-0.5, z); CI(z); }
static inline float isometric_latitude_f(float phi, float e) static inline float isometric_latitude_f(float phi, float e)
{ {
return log(tan(M_PI_4 + phi / 2.0)) - e / 2.0 * log((1.0 + e * sin(phi)) / (1.0 - e * sin(phi))); return logf(tanf(M_PI_4 + phi / 2.0)) - e / 2.0 * logf((1.0 + e * sinf(phi)) / (1.0 - e * sinf(phi)));
} }
static inline float isometric_latitude_fast_f(float phi) static inline float isometric_latitude_fast_f(float phi)
{ {
return log(tan(M_PI_4 + phi / 2.0)); return logf(tanf(M_PI_4 + phi / 2.0));
} }
static inline float inverse_isometric_latitude_f(float lat, float e, float epsilon) static inline float inverse_isometric_latitude_f(float lat, float e, float epsilon)
{ {
float exp_l = exp(lat); float exp_l = expf(lat);
float phi0 = 2 * atan(exp_l) - M_PI_2; float phi0 = 2 * atanf(exp_l) - M_PI_2;
float phi_; float phi_;
uint8_t max_iter = 3; /* To be sure to return */ uint8_t max_iter = 3; /* To be sure to return */
do { do {
phi_ = phi0; phi_ = phi0;
float sin_phi = e * sin(phi_); float sin_phi = e * sinf(phi_);
phi0 = 2 * atan(pow((1 + sin_phi) / (1. - sin_phi), e / 2.) * exp_l) - M_PI_2; phi0 = 2 * atanf(powf((1 + sin_phi) / (1. - sin_phi), e / 2.) * exp_l) - M_PI_2;
max_iter--; max_iter--;
} while (max_iter && fabs(phi_ - phi0) > epsilon); } while (max_iter && fabsf(phi_ - phi0) > epsilon);
return phi0; return phi0;
} }
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla) void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
{ {
// compute zone if not initialised
if (utm->zone == 0) {
utm->zone = UtmZoneOfLlaLonRad(lla->lon);
}
float lambda_c = LambdaOfUtmZone(utm->zone); float lambda_c = LambdaOfUtmZone(utm->zone);
float ll = isometric_latitude_f(lla->lat , E); float ll = isometric_latitude_f(lla->lat , E);
float dl = lla->lon - lambda_c; float dl = lla->lon - lambda_c;
float phi_ = asin(sin(dl) / cosh(ll)); float phi_ = asinf(sinf(dl) / coshf(ll));
float ll_ = isometric_latitude_fast_f(phi_); float ll_ = isometric_latitude_fast_f(phi_);
float lambda_ = atan(sinh(ll) / cos(dl)); float lambda_ = atanf(sinhf(ll) / cosf(dl));
struct complex z_ = { lambda_, ll_ }; struct complex z_ = { lambda_, ll_ };
CScal(serie_coeff_proj_mercator[0], z_); CScal(serie_coeff_proj_mercator[0], z_);
uint8_t k; int8_t k;
for (k = 1; k < 3; k++) { for (k = 1; k < 3; k++) {
struct complex z = { lambda_, ll_ }; struct complex z = { lambda_, ll_ };
CScal(2 * k, z); CScal(2.*k, z);
CSin(z); CSin(z);
CScal(serie_coeff_proj_mercator[k], z); CScal(serie_coeff_proj_mercator[k], z);
CAdd(z, z_); CAdd(z, z_);
@@ -334,18 +338,18 @@ void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
float img = (utm->east - DELTA_EAST) * scale; float img = (utm->east - DELTA_EAST) * scale;
struct complex z = { real, img }; struct complex z = { real, img };
uint8_t k; int8_t k;
for (k = 1; k < 2; k++) { for (k = 1; k < 2; k++) {
struct complex z_ = { real, img }; struct complex z_ = { real, img };
CScal(2 * k, z_); CScal(2.*k, z_);
CSin(z_); CSin(z_);
CScal(serie_coeff_proj_mercator_inverse[k], z_); CScal(serie_coeff_proj_mercator_inverse[k], z_);
CSub(z_, z); CSub(z_, z);
} }
float lambda_c = LambdaOfUtmZone(utm->zone); float lambda_c = LambdaOfUtmZone(utm->zone);
lla->lon = lambda_c + atan(sinh(z.im) / cos(z.re)); lla->lon = lambda_c + atanf(sinhf(z.im) / cosf(z.re));
float phi_ = asin(sin(z.re) / cosh(z.im)); float phi_ = asinf(sinf(z.re) / coshf(z.im));
float il = isometric_latitude_fast_f(phi_); float il = isometric_latitude_fast_f(phi_);
lla->lat = inverse_isometric_latitude_f(il, E, 1e-8); lla->lat = inverse_isometric_latitude_f(il, E, 1e-8);
sw/airborne/math/pprz_geodetic_float.h
+3 -3
View File
@@ -54,7 +54,7 @@ struct EcefCoor_f {
struct LlaCoor_f { struct LlaCoor_f {
float lat; ///< in radians float lat; ///< in radians
float lon; ///< in radians float lon; ///< in radians
float alt; ///< in meters above WGS84 reference ellipsoid float alt; ///< in meters (normally above WGS84 reference ellipsoid)
}; };
/** /**
@@ -81,7 +81,7 @@ struct EnuCoor_f {
struct UtmCoor_f { struct UtmCoor_f {
float north; ///< in meters float north; ///< in meters
float east; ///< in meters float east; ///< in meters
float alt; ///< in meters above WGS84 reference ellipsoid float alt; ///< in meters (above WGS84 reference ellipsoid or above MSL)
uint8_t zone; ///< UTM zone number uint8_t zone; ///< UTM zone number
}; };
@@ -110,7 +110,7 @@ extern void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, stru
extern void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla); extern void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla);
extern void ned_of_lla_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct LlaCoor_f *lla); extern void ned_of_lla_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct LlaCoor_f *lla);
/* not enought precision with floats - used the double version */ /* not enough precision with floats - used the double version */
extern void ecef_of_enu_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu); extern void ecef_of_enu_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu);
extern void ecef_of_ned_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned); extern void ecef_of_ned_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned);
extern void ecef_of_enu_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu); extern void ecef_of_enu_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu);
sw/airborne/math/pprz_geodetic_int.c
+84 -32
View File
@@ -34,16 +34,16 @@
void ltp_of_ecef_rmat_from_lla_i(struct Int32RMat *ltp_of_ecef, struct LlaCoor_i *lla) void ltp_of_ecef_rmat_from_lla_i(struct Int32RMat *ltp_of_ecef, struct LlaCoor_i *lla)
{ {
#if USE_DOUBLE_PRECISION_TRIG #if USE_SINGLE_PRECISION_TRIG
int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7DEG((double)lla->lat)), HIGH_RES_TRIG_FRAC));
int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7DEG((double)lla->lat)), HIGH_RES_TRIG_FRAC));
int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7DEG((double)lla->lon)), HIGH_RES_TRIG_FRAC));
int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7DEG((double)lla->lon)), HIGH_RES_TRIG_FRAC));
#else
int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7DEG((float)lla->lat)), HIGH_RES_TRIG_FRAC)); int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7DEG((float)lla->lat)), HIGH_RES_TRIG_FRAC));
int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7DEG((float)lla->lat)), HIGH_RES_TRIG_FRAC)); int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7DEG((float)lla->lat)), HIGH_RES_TRIG_FRAC));
int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7DEG((float)lla->lon)), HIGH_RES_TRIG_FRAC)); int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7DEG((float)lla->lon)), HIGH_RES_TRIG_FRAC));
int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7DEG((float)lla->lon)), HIGH_RES_TRIG_FRAC)); int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7DEG((float)lla->lon)), HIGH_RES_TRIG_FRAC));
#else // use double precision by default
int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7DEG((double)lla->lat)), HIGH_RES_TRIG_FRAC));
int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7DEG((double)lla->lat)), HIGH_RES_TRIG_FRAC));
int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7DEG((double)lla->lon)), HIGH_RES_TRIG_FRAC));
int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7DEG((double)lla->lon)), HIGH_RES_TRIG_FRAC));
#endif #endif
ltp_of_ecef->m[0] = -sin_lon; ltp_of_ecef->m[0] = -sin_lon;
@@ -60,7 +60,7 @@ void ltp_of_ecef_rmat_from_lla_i(struct Int32RMat *ltp_of_ecef, struct LlaCoor_i
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef) void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
{ {
/* store the origin of the tangeant plane */ /* store the origin of the tangent plane */
VECT3_COPY(def->ecef, *ecef); VECT3_COPY(def->ecef, *ecef);
/* compute the lla representation of the origin */ /* compute the lla representation of the origin */
lla_of_ecef_i(&def->lla, &def->ecef); lla_of_ecef_i(&def->lla, &def->ecef);
@@ -72,7 +72,7 @@ void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla) void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
{ {
/* store the origin of the tangeant plane */ /* store the origin of the tangent plane */
LLA_COPY(def->lla, *lla); LLA_COPY(def->lla, *lla);
/* compute the ecef representation of the origin */ /* compute the ecef representation of the origin */
ecef_of_lla_i(&def->ecef, &def->lla); ecef_of_lla_i(&def->ecef, &def->lla);
@@ -332,68 +332,120 @@ void ned_of_lla_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCo
#include "pprz_geodetic_float.h" #include "pprz_geodetic_float.h"
#include "pprz_geodetic_double.h" #include "pprz_geodetic_double.h"
/** Convert a ECEF to LLA.
* @param[out] out LLA in degrees*1e7 and mm above ellipsoid
* @param[in] in ECEF in cm
*/
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in) void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
{ {
#if USE_SINGLE_PRECISION_LLA_ECEF #if USE_SINGLE_PRECISION_LLA_ECEF
/* convert our input to floating point */ /* convert our input to floating point */
struct EcefCoor_f in_f; struct EcefCoor_f in_f;
in_f.x = M_OF_CM((float)in->x); ECEF_FLOAT_OF_BFP(in_f, *in);
in_f.y = M_OF_CM((float)in->y);
in_f.z = M_OF_CM((float)in->z);
/* calls the floating point transformation */ /* calls the floating point transformation */
struct LlaCoor_f out_f; struct LlaCoor_f out_f;
lla_of_ecef_f(&out_f, &in_f); lla_of_ecef_f(&out_f, &in_f);
/* convert the output to fixed point */ /* convert the output to fixed point */
out->lon = (int32_t)rint(EM7DEG_OF_RAD(out_f.lon)); LLA_BFP_OF_REAL(*out, out_f);
out->lat = (int32_t)rint(EM7DEG_OF_RAD(out_f.lat));
out->alt = (int32_t)MM_OF_M(out_f.alt);
#else // use double precision by default #else // use double precision by default
/* convert our input to floating point */ /* convert our input to floating point */
struct EcefCoor_d in_d; struct EcefCoor_d in_d;
in_d.x = M_OF_CM((double)in->x); ECEF_DOUBLE_OF_BFP(in_d, *in);
in_d.y = M_OF_CM((double)in->y);
in_d.z = M_OF_CM((double)in->z);
/* calls the floating point transformation */ /* calls the floating point transformation */
struct LlaCoor_d out_d; struct LlaCoor_d out_d;
lla_of_ecef_d(&out_d, &in_d); lla_of_ecef_d(&out_d, &in_d);
/* convert the output to fixed point */ /* convert the output to fixed point */
out->lon = (int32_t)rint(EM7DEG_OF_RAD(out_d.lon)); LLA_BFP_OF_REAL(*out, out_d);
out->lat = (int32_t)rint(EM7DEG_OF_RAD(out_d.lat));
out->alt = (int32_t)MM_OF_M(out_d.alt);
#endif #endif
} }
/** Convert a LLA to ECEF.
* @param[out] out ECEF in cm
* @param[in] in LLA in degrees*1e7 and mm above ellipsoid
*/
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in) void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
{ {
#if USE_SINGLE_PRECISION_LLA_ECEF #if USE_SINGLE_PRECISION_LLA_ECEF
/* convert our input to floating point */ /* convert our input to floating point */
struct LlaCoor_f in_f; struct LlaCoor_f in_f;
in_f.lon = RAD_OF_EM7DEG((float)in->lon); LLA_FLOAT_OF_BFP(in_f, *in);
in_f.lat = RAD_OF_EM7DEG((float)in->lat);
in_f.alt = M_OF_MM((float)in->alt);
/* calls the floating point transformation */ /* calls the floating point transformation */
struct EcefCoor_f out_f; struct EcefCoor_f out_f;
ecef_of_lla_f(&out_f, &in_f); ecef_of_lla_f(&out_f, &in_f);
/* convert the output to fixed point */ /* convert the output to fixed point */
out->x = (int32_t)CM_OF_M(out_f.x); ECEF_BFP_OF_REAL(*out, out_f);
out->y = (int32_t)CM_OF_M(out_f.y);
out->z = (int32_t)CM_OF_M(out_f.z);
#else // use double precision by default #else // use double precision by default
/* convert our input to floating point */ /* convert our input to floating point */
struct LlaCoor_d in_d; struct LlaCoor_d in_d;
in_d.lon = RAD_OF_EM7DEG((double)in->lon); LLA_DOUBLE_OF_BFP(in_d, *in);
in_d.lat = RAD_OF_EM7DEG((double)in->lat);
in_d.alt = M_OF_MM((double)in->alt);
/* calls the floating point transformation */ /* calls the floating point transformation */
struct EcefCoor_d out_d; struct EcefCoor_d out_d;
ecef_of_lla_d(&out_d, &in_d); ecef_of_lla_d(&out_d, &in_d);
/* convert the output to fixed point */ /* convert the output to fixed point */
out->x = (int32_t)CM_OF_M(out_d.x); ECEF_BFP_OF_REAL(*out, out_d);
out->y = (int32_t)CM_OF_M(out_d.y); #endif
out->z = (int32_t)CM_OF_M(out_d.z);
}
#include "math/pprz_geodetic_utm.h"
/** Convert a LLA to UTM.
* @param[out] out UTM in cm and mm hmsl alt
* @param[in] in LLA in degrees*1e7 and mm above ellipsoid
*/
void utm_of_lla_i(struct UtmCoor_i *utm, struct LlaCoor_i *lla)
{
#if USE_SINGLE_PRECISION_LLA_UTM
/* convert our input to floating point */
struct LlaCoor_f lla_f;
LLA_FLOAT_OF_BFP(lla_f, *lla);
/* calls the floating point transformation */
struct UtmCoor_f utm_f;
utm_f.zone = utm->zone;
utm_of_lla_f(&utm_f, &lla_f);
/* convert the output to fixed point */
UTM_BFP_OF_REAL(*utm, utm_f);
#else // use double precision by default
/* convert our input to floating point */
struct LlaCoor_d lla_d;
LLA_DOUBLE_OF_BFP(lla_d, *lla);
/* calls the floating point transformation */
struct UtmCoor_d utm_d;
utm_d.zone = utm->zone;
utm_of_lla_d(&utm_d, &lla_d);
/* convert the output to fixed point */
UTM_BFP_OF_REAL(*utm, utm_d);
#endif
}
/** Convert a local NED position to ECEF.
* @param[out] ecef ECEF position in cm
* @param[in] def local coordinate system definition
* @param[in] ned NED position in meter << #INT32_POS_FRAC
*/
void lla_of_utm_i(struct LlaCoor_i *lla, struct UtmCoor_i *utm)
{
#if USE_SINGLE_PRECISION_LLA_UTM
/* convert our input to floating point */
struct UtmCoor_f utm_f;
UTM_FLOAT_OF_BFP(utm_f, *utm);
/* calls the floating point transformation */
struct LlaCoor_f lla_f;
lla_of_utm_f(&lla_f, &utm_f);
/* convert the output to fixed point */
LLA_BFP_OF_REAL(*lla, lla_f);
#else // use double precision by default
/* convert our input to floating point */
struct UtmCoor_d utm_d;
UTM_DOUBLE_OF_BFP(utm_d, *utm);
/* calls the floating point transformation */
struct LlaCoor_d lla_d;
lla_of_utm_d(&lla_d, &utm_d);
/* convert the output to fixed point */
LLA_BFP_OF_REAL(*lla, lla_d);
#endif #endif
} }
sw/airborne/math/pprz_geodetic_int.h
+41 -18
View File
@@ -85,9 +85,9 @@ struct EnuCoor_i {
*/ */
struct UtmCoor_i { struct UtmCoor_i {
int32_t north; ///< in centimeters int32_t north; ///< in centimeters
int32_t east; ///< in centimeters int32_t east; ///< in centimeters
int32_t alt; ///< in millimeters above WGS84 reference ellipsoid int32_t alt; ///< in millimeters (above WGS84 reference ellipsoid or above MSL)
uint8_t zone; ///< UTM zone number uint8_t zone; ///< UTM zone number
}; };
/** /**
@@ -102,6 +102,8 @@ struct LtpDef_i {
int32_t hmsl; ///< Height above mean sea level in mm int32_t hmsl; ///< Height above mean sea level in mm
}; };
extern void lla_of_utm_i(struct LlaCoor_i *lla, struct UtmCoor_i *utm);
extern void utm_of_lla_i(struct UtmCoor_i *utm, struct LlaCoor_i *lla);
extern void ltp_of_ecef_rmat_from_lla_i(struct Int32RMat *ltp_of_ecef, struct LlaCoor_i *lla); extern void ltp_of_ecef_rmat_from_lla_i(struct Int32RMat *ltp_of_ecef, struct LlaCoor_i *lla);
extern void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef); extern void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef);
extern void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla); extern void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla);
@@ -154,15 +156,15 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, st
} }
#define ECEF_FLOAT_OF_BFP(_o, _i) { \ #define ECEF_FLOAT_OF_BFP(_o, _i) { \
(_o).x = (float)M_OF_CM((_i).x); \ (_o).x = M_OF_CM((float)(_i).x); \
(_o).y = (float)M_OF_CM((_i).y); \ (_o).y = M_OF_CM((float)(_i).y); \
(_o).z = (float)M_OF_CM((_i).z); \ (_o).z = M_OF_CM((float)(_i).z); \
} }
#define ECEF_DOUBLE_OF_BFP(_o, _i) { \ #define ECEF_DOUBLE_OF_BFP(_o, _i) { \
(_o).x = (double)M_OF_CM((_i).x); \ (_o).x = M_OF_CM((double)(_i).x); \
(_o).y = (double)M_OF_CM((_i).y); \ (_o).y = M_OF_CM((double)(_i).y); \
(_o).z = (double)M_OF_CM((_i).z); \ (_o).z = M_OF_CM((double)(_i).z); \
} }
#define LLA_BFP_OF_REAL(_o, _i) { \ #define LLA_BFP_OF_REAL(_o, _i) { \
(_o).lat = (int32_t)EM7DEG_OF_RAD((_i).lat); \ (_o).lat = (int32_t)EM7DEG_OF_RAD((_i).lat); \
@@ -170,21 +172,21 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, st
(_o).alt = (int32_t)MM_OF_M((_i).alt); \ (_o).alt = (int32_t)MM_OF_M((_i).alt); \
} }
#define LLA_FLOAT_OF_BFP(_o, _i) { \ #define LLA_FLOAT_OF_BFP(_o, _i) { \
(_o).lat = RAD_OF_EM7DEG((float)(_i).lat); \ (_o).lat = RAD_OF_EM7DEG((float)(_i).lat); \
(_o).lon = RAD_OF_EM7DEG((float)(_i).lon); \ (_o).lon = RAD_OF_EM7DEG((float)(_i).lon); \
(_o).alt = M_OF_MM((float)(_i).alt); \ (_o).alt = M_OF_MM((float)(_i).alt); \
} }
#define LLA_DOUBLE_OF_BFP(_o, _i) { \ #define LLA_DOUBLE_OF_BFP(_o, _i) { \
(_o).lat = RAD_OF_EM7DEG((double)(_i).lat); \ (_o).lat = RAD_OF_EM7DEG((double)(_i).lat); \
(_o).lon = RAD_OF_EM7DEG((double)(_i).lon); \ (_o).lon = RAD_OF_EM7DEG((double)(_i).lon); \
(_o).alt = M_OF_MM((double)(_i).alt); \ (_o).alt = M_OF_MM((double)(_i).alt); \
} }
#define NED_BFP_OF_REAL(_o, _i) { \ #define NED_BFP_OF_REAL(_o, _i) { \
(_o).x = POS_BFP_OF_REAL((_i).x); \ (_o).x = (int32_t)POS_BFP_OF_REAL((_i).x); \
(_o).y = POS_BFP_OF_REAL((_i).y); \ (_o).y = (int32_t)POS_BFP_OF_REAL((_i).y); \
(_o).z = POS_BFP_OF_REAL((_i).z); \ (_o).z = (int32_t)POS_BFP_OF_REAL((_i).z); \
} }
#define ENU_BFP_OF_REAL(_o, _i) NED_BFP_OF_REAL(_o, _i) #define ENU_BFP_OF_REAL(_o, _i) NED_BFP_OF_REAL(_o, _i)
@@ -240,6 +242,27 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, st
(_ef).m[8] = DOUBLE_OF_BFP((_ei).m[8], HIGH_RES_TRIG_FRAC); \ (_ef).m[8] = DOUBLE_OF_BFP((_ei).m[8], HIGH_RES_TRIG_FRAC); \
} }
#define UTM_FLOAT_OF_BFP(_o, _i) { \
(_o).east = M_OF_CM((float)(_i).east); \
(_o).north = M_OF_CM((float)(_i).north); \
(_o).alt = M_OF_MM((float)(_i).alt); \
(_o).zone = (_i).zone; \
}
#define UTM_DOUBLE_OF_BFP(_o, _i) { \
(_o).east = M_OF_CM((double)(_i).east); \
(_o).north = M_OF_CM((double)(_i).north); \
(_o).alt = M_OF_MM((double)(_i).alt); \
(_o).zone = (_i).zone; \
}
#define UTM_BFP_OF_REAL(_o, _i) { \
(_o).east = (int32_t)CM_OF_M((_i).east); \
(_o).north = (int32_t)CM_OF_M((_i).north); \
(_o).alt = (int32_t)MM_OF_M((_i).alt); \
(_o).zone = (_i).zone; \
}
#ifdef __cplusplus #ifdef __cplusplus
} /* extern "C" */ } /* extern "C" */
#endif #endif
sw/airborne/math/pprz_geodetic_utm.h
+4
View File
@@ -34,6 +34,8 @@
extern "C" { extern "C" {
#endif #endif
#include "std.h"
/* Computation for the WGS84 geoid only */ /* Computation for the WGS84 geoid only */
#define E 0.08181919106 #define E 0.08181919106
#define K0 0.9996 #define K0 0.9996
@@ -43,6 +45,8 @@ extern "C" {
#define N (K0*A) #define N (K0*A)
#define LambdaOfUtmZone(utm_zone) RadOfDeg((utm_zone-1)*6-180+3) #define LambdaOfUtmZone(utm_zone) RadOfDeg((utm_zone-1)*6-180+3)
#define UtmZoneOfLlaLonRad(lla_lon) (floor(DegOfRad(lla_lon) + 180) / 6) + 1
#define UtmZoneOfLlaLonDeg(lla_lon) (floor((lla_lon) / 1e7 + 180) / 6) + 1
static const float serie_coeff_proj_mercator[5] = { static const float serie_coeff_proj_mercator[5] = {
0.99832429842242842444, 0.99832429842242842444,
tests/math/test_pprz_geodetic.c
+28 -7
View File
@@ -198,7 +198,7 @@ static void test_enu_of_ecef_int(void)
static void test_lla_of_utm(void) static void test_lla_of_utm(void)
{ {
note("--- test lla_of_utm (double and float)"); note("--- test lla_of_utm (double, float and int)");
struct UtmCoor_d utm_d = { .east=348805.71, .north=4759354.89, .zone=31 }; struct UtmCoor_d utm_d = { .east=348805.71, .north=4759354.89, .zone=31 };
note("UTM input: east: %.2f m, north: %.2f m, zone: %d", utm_d.east, utm_d.north, utm_d.zone); note("UTM input: east: %.2f m, north: %.2f m, zone: %d", utm_d.east, utm_d.north, utm_d.zone);
@@ -212,19 +212,20 @@ static void test_lla_of_utm(void)
struct LlaCoor_d lla_d; struct LlaCoor_d lla_d;
lla_of_utm_d(&lla_d, &utm_d); lla_of_utm_d(&lla_d, &utm_d);
const double max_err_rad = RadOfDeg(0.0001); const double max_err_deg = 0.00001;
const double max_err_rad = RadOfDeg(max_err_deg);
double lat_d_err = fabs(l_ref_d.lat - lla_d.lat); double lat_d_err = fabs(l_ref_d.lat - lla_d.lat);
double lon_d_err = fabs(l_ref_d.lon - lla_d.lon); double lon_d_err = fabs(l_ref_d.lon - lla_d.lon);
if (lat_d_err < max_err_rad && lon_d_err < max_err_rad) { if (lat_d_err < max_err_rad && lon_d_err < max_err_rad) {
note(" lla_d.lat=%.12f deg lla_d.lon=%.12f deg", note(" lla_d.lat=%.12f deg lla_d.lon=%.12f deg",
DegOfRad(lla_d.lat), DegOfRad(lla_d.lon)); DegOfRad(lla_d.lat), DegOfRad(lla_d.lon));
pass("lla_of_utm_d error is below 1e-4 deg"); pass("lla_of_utm_d error is below 1e-5 deg");
} }
else { else {
note(" lat=%.16f lon=%.16f\nref_lat=%.16f ref_lon=%.16f\n", note(" lat=%.16f lon=%.16f\nref_lat=%.16f ref_lon=%.16f\n",
lla_d.lat, lla_d.lon, l_ref_d.lat, l_ref_d.lon); lla_d.lat, lla_d.lon, l_ref_d.lat, l_ref_d.lon);
fail("lla_of_utm_d error is NOT below 1e-4 deg"); fail("lla_of_utm_d error is NOT below 1e-5 deg");
} }
struct UtmCoor_f utm_f = { .east=348805.71, .north=4759354.89, .zone=31 }; struct UtmCoor_f utm_f = { .east=348805.71, .north=4759354.89, .zone=31 };
@@ -235,7 +236,8 @@ static void test_lla_of_utm(void)
float lat_f_err = fabs(l_ref_d.lat - lla_f.lat); float lat_f_err = fabs(l_ref_d.lat - lla_f.lat);
float lon_f_err = fabs(l_ref_d.lon - lla_f.lon); float lon_f_err = fabs(l_ref_d.lon - lla_f.lon);
if (lat_f_err < max_err_rad && lon_f_err < max_err_rad) { /* float is not as correct as double and int */
if (lat_f_err < max_err_rad*10 && lon_f_err < max_err_rad*10) {
note(" lla_f.lat=%.12f deg lla_f.lon=%.12f deg", note(" lla_f.lat=%.12f deg lla_f.lon=%.12f deg",
DegOfRad(lla_f.lat), DegOfRad(lla_f.lon)); DegOfRad(lla_f.lat), DegOfRad(lla_f.lon));
pass("lla_of_utm_f error is below 1e-4 deg"); pass("lla_of_utm_f error is below 1e-4 deg");
@@ -245,6 +247,25 @@ static void test_lla_of_utm(void)
lla_f.lat, lla_f.lon, l_ref_f.lat, l_ref_f.lon); lla_f.lat, lla_f.lon, l_ref_f.lat, l_ref_f.lon);
fail("lla_of_utm_f error is NOT below 1e-4 deg"); fail("lla_of_utm_f error is NOT below 1e-4 deg");
} }
struct UtmCoor_i utm_i = { .east=34880571, .north=475935489, .zone=31 };
struct LlaCoor_i lla_i;
struct LlaCoor_i l_ref_i = {.lat=DegOfRad(0.749999999392454875)*1e7,
.lon=DegOfRad(0.019999999054505127)*1e7};
lla_of_utm_i(&lla_i, &utm_i);
float lat_i_err = abs(l_ref_i.lat - lla_i.lat)/1e7;
float lon_i_err = abs(l_ref_i.lon - lla_i.lon)/1e7;
if (lat_i_err < max_err_deg && lon_i_err < max_err_deg) {
note(" lla_i.lat=%.12f deg lla_i.lon=%.12f deg",
lla_i.lat/1e7, lla_i.lon/1e7);
pass("lla_of_utm_i error is below 1e-5 deg");
}
else {
note(" lat=%.16f lon=%.16f\nref_lat=%.16f ref_lon=%.16f\n",
lla_i.lat/1e7, lla_i.lon/1e7, l_ref_i.lat/1e7, l_ref_i.lon/1e7);
fail("lla_of_utm_i error is NOT below 1e-5 deg");
}
} }
static void test_enu_of_ecef_double(void) static void test_enu_of_ecef_double(void)
@@ -349,7 +370,7 @@ static void test_lla_of_ecef(void)
struct LlaCoor_i lla_i; struct LlaCoor_i lla_i;
lla_of_ecef_i(&lla_i, &ecef_ref_i); lla_of_ecef_i(&lla_i, &ecef_ref_i);
/* compare LLA computed in fixed point with reference (convered from double) */ /* compare LLA computed in fixed point with reference (converted from double) */
cmp_ok(lla_i.lat, "==", lla_ref_i.lat, "latitude (int) matches reference"); cmp_ok(lla_i.lat, "==", lla_ref_i.lat, "latitude (int) matches reference");
cmp_ok(lla_i.lon, "==", lla_ref_i.lon, "longitude (int) matches reference"); cmp_ok(lla_i.lon, "==", lla_ref_i.lon, "longitude (int) matches reference");
cmp_ok(lla_i.alt, "==", lla_ref_i.alt, "altitude (int) matches reference"); cmp_ok(lla_i.alt, "==", lla_ref_i.alt, "altitude (int) matches reference");
@@ -358,7 +379,7 @@ static void test_lla_of_ecef(void)
int main() int main()
{ {
note("runing geodetic math tests"); note("runing geodetic math tests");
plan(12); plan(13);
test_ecef_of_ned_int(); test_ecef_of_ned_int();
test_enu_of_ecef_int(); test_enu_of_ecef_int();