From df91322a19dfcee56118df9e665f733ed0e36000 Mon Sep 17 00:00:00 2001 From: kirkscheper Date: Sun, 24 Apr 2016 13:20:47 +0200 Subject: [PATCH] [math] added and updated utm helper funtions --- sw/airborne/math/pprz_geodetic_double.c | 102 ++++++++++++++++----- sw/airborne/math/pprz_geodetic_double.h | 4 +- sw/airborne/math/pprz_geodetic_float.c | 46 +++++----- sw/airborne/math/pprz_geodetic_float.h | 6 +- sw/airborne/math/pprz_geodetic_int.c | 116 +++++++++++++++++------- sw/airborne/math/pprz_geodetic_int.h | 59 ++++++++---- sw/airborne/math/pprz_geodetic_utm.h | 4 + tests/math/test_pprz_geodetic.c | 35 +++++-- 8 files changed, 267 insertions(+), 105 deletions(-) diff --git a/sw/airborne/math/pprz_geodetic_double.c b/sw/airborne/math/pprz_geodetic_double.c index 69da99dcef..2dc660f15e 100644 --- a/sw/airborne/math/pprz_geodetic_double.c +++ b/sw/airborne/math/pprz_geodetic_double.c @@ -33,8 +33,7 @@ void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef) { - - /* store the origin of the tangeant plane */ + /* store the origin of the tangent plane */ VECT3_COPY(def->ecef, *ecef); /* compute the lla representation of the origin */ 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 */ 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" -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))); } @@ -224,48 +248,80 @@ static inline double inverse_isometric_latitude_d(double lat, double e, double e } #define CI(v) { \ - double tmp = v.x; \ + 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); \ + double e = exp(v.x);\ + v.x = e*cos(v.y); \ + v.y = e*sin(v.y); \ } #define CSin(v) { \ - CI(v); \ + CI(v); \ struct DoubleVect2 vstar = {-v.x, -v.y}; \ - CExp(v); \ + CExp(v); \ CExp(vstar); \ - VECT2_SUB(v, vstar); \ + VECT2_SUB(v, vstar);\ 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) { - - struct DoubleVect2 v = {utm->north - DELTA_NORTH, utm->east - DELTA_EAST}; + struct DoubleVect2 z = {utm->north - DELTA_NORTH, utm->east - DELTA_EAST}; 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 ? - struct DoubleVect2 v1; - VECT2_SMUL(v1, v, 2.); - CSin(v1); - VECT2_SMUL(v1, v1, serie_coeff_proj_mercator[1]); - VECT2_SUB(v, v1); + int8_t k; + for(k = 1; k<2; k++) + { + struct DoubleVect2 z_; + VECT2_SMUL(z_, z, 2.*k); + CSin(z_); + VECT2_SMUL(z_, z_, serie_coeff_proj_mercator_inverse[k]); + VECT2_SUB(z, z_); + } double lambda_c = LambdaOfUtmZone(utm->zone); - lla->lon = lambda_c + atan(sinh(v.y) / cos(v.x)); - double phi = asin(sin(v.x) / cosh(v.y)); + lla->lon = lambda_c + atan(sinh(z.y) / cos(z.x)); + double phi = asin(sin(z.x) / cosh(z.y)); double il = isometric_latitude_fast_d(phi); lla->lat = inverse_isometric_latitude_d(il, E, 1e-8); // copy alt above reference ellipsoid lla->alt = utm->alt; - } diff --git a/sw/airborne/math/pprz_geodetic_double.h b/sw/airborne/math/pprz_geodetic_double.h index 18c851dbad..de776d242f 100644 --- a/sw/airborne/math/pprz_geodetic_double.h +++ b/sw/airborne/math/pprz_geodetic_double.h @@ -101,8 +101,10 @@ struct LtpDef_d { 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_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 ecef_of_lla_d(struct EcefCoor_d *out, struct LlaCoor_d *in); diff --git a/sw/airborne/math/pprz_geodetic_float.c b/sw/airborne/math/pprz_geodetic_float.c index 6c254f82e1..95e1f7db7e 100644 --- a/sw/airborne/math/pprz_geodetic_float.c +++ b/sw/airborne/math/pprz_geodetic_float.c @@ -35,8 +35,7 @@ void ltp_def_from_ecef_f(struct LtpDef_f *def, struct EcefCoor_f *ecef) { - - /* store the origin of the tangeant plane */ + /* store the origin of the tangent plane */ VECT3_COPY(def->ecef, *ecef); /* compute the lla representation of the origin */ 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) { - /* store the origin of the tangeant plane */ + /* store the origin of the tangent plane */ LLA_COPY(def->lla, *lla); /* compute the ecef representation of the origin */ 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) { @@ -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); } /* 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) { - 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) { - 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) { - float exp_l = exp(lat); - float phi0 = 2 * atan(exp_l) - M_PI_2; + float exp_l = expf(lat); + float phi0 = 2 * atanf(exp_l) - M_PI_2; float phi_; uint8_t max_iter = 3; /* To be sure to return */ do { phi_ = phi0; - float sin_phi = e * sin(phi_); - phi0 = 2 * atan(pow((1 + sin_phi) / (1. - sin_phi), e / 2.) * exp_l) - M_PI_2; + float sin_phi = e * sinf(phi_); + phi0 = 2 * atanf(powf((1 + sin_phi) / (1. - sin_phi), e / 2.) * exp_l) - M_PI_2; max_iter--; - } while (max_iter && fabs(phi_ - phi0) > epsilon); + } while (max_iter && fabsf(phi_ - phi0) > epsilon); return phi0; } 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 ll = isometric_latitude_f(lla->lat , E); 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 lambda_ = atan(sinh(ll) / cos(dl)); + float lambda_ = atanf(sinhf(ll) / cosf(dl)); struct complex z_ = { lambda_, ll_ }; CScal(serie_coeff_proj_mercator[0], z_); - uint8_t k; + int8_t k; for (k = 1; k < 3; k++) { - struct complex z = { lambda_, ll_ }; - CScal(2 * k, z); + struct complex z = { lambda_, ll_ }; + CScal(2.*k, z); CSin(z); CScal(serie_coeff_proj_mercator[k], 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; struct complex z = { real, img }; - uint8_t k; + int8_t k; for (k = 1; k < 2; k++) { struct complex z_ = { real, img }; - CScal(2 * k, z_); + CScal(2.*k, z_); CSin(z_); CScal(serie_coeff_proj_mercator_inverse[k], z_); CSub(z_, z); } float lambda_c = LambdaOfUtmZone(utm->zone); - lla->lon = lambda_c + atan(sinh(z.im) / cos(z.re)); - float phi_ = asin(sin(z.re) / cosh(z.im)); + lla->lon = lambda_c + atanf(sinhf(z.im) / cosf(z.re)); + float phi_ = asinf(sinf(z.re) / coshf(z.im)); float il = isometric_latitude_fast_f(phi_); lla->lat = inverse_isometric_latitude_f(il, E, 1e-8); diff --git a/sw/airborne/math/pprz_geodetic_float.h b/sw/airborne/math/pprz_geodetic_float.h index e8315e5dd7..eee55eaed3 100644 --- a/sw/airborne/math/pprz_geodetic_float.h +++ b/sw/airborne/math/pprz_geodetic_float.h @@ -54,7 +54,7 @@ struct EcefCoor_f { struct LlaCoor_f { float lat; ///< 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 { float north; ///< 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 }; @@ -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 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_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); diff --git a/sw/airborne/math/pprz_geodetic_int.c b/sw/airborne/math/pprz_geodetic_int.c index 6a2c70d971..c4b03aec16 100644 --- a/sw/airborne/math/pprz_geodetic_int.c +++ b/sw/airborne/math/pprz_geodetic_int.c @@ -34,16 +34,16 @@ void ltp_of_ecef_rmat_from_lla_i(struct Int32RMat *ltp_of_ecef, struct LlaCoor_i *lla) { -#if USE_DOUBLE_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 +#if USE_SINGLE_PRECISION_TRIG 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 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)); +#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 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) { - /* store the origin of the tangeant plane */ + /* store the origin of the tangent plane */ VECT3_COPY(def->ecef, *ecef); /* compute the lla representation of the origin */ 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) { - /* store the origin of the tangeant plane */ + /* store the origin of the tangent plane */ LLA_COPY(def->lla, *lla); /* compute the ecef representation of the origin */ 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_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) { #if USE_SINGLE_PRECISION_LLA_ECEF /* convert our input to floating point */ struct EcefCoor_f in_f; - in_f.x = M_OF_CM((float)in->x); - in_f.y = M_OF_CM((float)in->y); - in_f.z = M_OF_CM((float)in->z); + ECEF_FLOAT_OF_BFP(in_f, *in); /* calls the floating point transformation */ struct LlaCoor_f out_f; lla_of_ecef_f(&out_f, &in_f); /* convert the output to fixed point */ - out->lon = (int32_t)rint(EM7DEG_OF_RAD(out_f.lon)); - out->lat = (int32_t)rint(EM7DEG_OF_RAD(out_f.lat)); - out->alt = (int32_t)MM_OF_M(out_f.alt); + LLA_BFP_OF_REAL(*out, out_f); #else // use double precision by default /* convert our input to floating point */ struct EcefCoor_d in_d; - in_d.x = M_OF_CM((double)in->x); - in_d.y = M_OF_CM((double)in->y); - in_d.z = M_OF_CM((double)in->z); + ECEF_DOUBLE_OF_BFP(in_d, *in); /* calls the floating point transformation */ struct LlaCoor_d out_d; lla_of_ecef_d(&out_d, &in_d); /* convert the output to fixed point */ - out->lon = (int32_t)rint(EM7DEG_OF_RAD(out_d.lon)); - out->lat = (int32_t)rint(EM7DEG_OF_RAD(out_d.lat)); - out->alt = (int32_t)MM_OF_M(out_d.alt); + LLA_BFP_OF_REAL(*out, out_d); #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) { #if USE_SINGLE_PRECISION_LLA_ECEF /* convert our input to floating point */ struct LlaCoor_f in_f; - in_f.lon = RAD_OF_EM7DEG((float)in->lon); - in_f.lat = RAD_OF_EM7DEG((float)in->lat); - in_f.alt = M_OF_MM((float)in->alt); + LLA_FLOAT_OF_BFP(in_f, *in); /* calls the floating point transformation */ struct EcefCoor_f out_f; ecef_of_lla_f(&out_f, &in_f); /* convert the output to fixed point */ - out->x = (int32_t)CM_OF_M(out_f.x); - out->y = (int32_t)CM_OF_M(out_f.y); - out->z = (int32_t)CM_OF_M(out_f.z); + ECEF_BFP_OF_REAL(*out, out_f); #else // use double precision by default /* convert our input to floating point */ struct LlaCoor_d in_d; - in_d.lon = RAD_OF_EM7DEG((double)in->lon); - in_d.lat = RAD_OF_EM7DEG((double)in->lat); - in_d.alt = M_OF_MM((double)in->alt); + LLA_DOUBLE_OF_BFP(in_d, *in); /* calls the floating point transformation */ struct EcefCoor_d out_d; ecef_of_lla_d(&out_d, &in_d); /* convert the output to fixed point */ - out->x = (int32_t)CM_OF_M(out_d.x); - out->y = (int32_t)CM_OF_M(out_d.y); - out->z = (int32_t)CM_OF_M(out_d.z); + ECEF_BFP_OF_REAL(*out, out_d); +#endif + +} + +#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 } diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h index daa59920d0..64d4641f09 100644 --- a/sw/airborne/math/pprz_geodetic_int.h +++ b/sw/airborne/math/pprz_geodetic_int.h @@ -85,9 +85,9 @@ struct EnuCoor_i { */ struct UtmCoor_i { int32_t north; ///< in centimeters - int32_t east; ///< in centimeters - int32_t alt; ///< in millimeters above WGS84 reference ellipsoid - uint8_t zone; ///< UTM zone number + int32_t east; ///< in centimeters + int32_t alt; ///< in millimeters (above WGS84 reference ellipsoid or above MSL) + uint8_t zone; ///< UTM zone number }; /** @@ -102,6 +102,8 @@ struct LtpDef_i { 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_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); @@ -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) { \ - (_o).x = (float)M_OF_CM((_i).x); \ - (_o).y = (float)M_OF_CM((_i).y); \ - (_o).z = (float)M_OF_CM((_i).z); \ + (_o).x = M_OF_CM((float)(_i).x); \ + (_o).y = M_OF_CM((float)(_i).y); \ + (_o).z = M_OF_CM((float)(_i).z); \ } -#define ECEF_DOUBLE_OF_BFP(_o, _i) { \ - (_o).x = (double)M_OF_CM((_i).x); \ - (_o).y = (double)M_OF_CM((_i).y); \ - (_o).z = (double)M_OF_CM((_i).z); \ +#define ECEF_DOUBLE_OF_BFP(_o, _i) { \ + (_o).x = M_OF_CM((double)(_i).x); \ + (_o).y = M_OF_CM((double)(_i).y); \ + (_o).z = M_OF_CM((double)(_i).z); \ } #define LLA_BFP_OF_REAL(_o, _i) { \ (_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); \ } -#define LLA_FLOAT_OF_BFP(_o, _i) { \ - (_o).lat = RAD_OF_EM7DEG((float)(_i).lat); \ - (_o).lon = RAD_OF_EM7DEG((float)(_i).lon); \ - (_o).alt = M_OF_MM((float)(_i).alt); \ +#define LLA_FLOAT_OF_BFP(_o, _i) { \ + (_o).lat = RAD_OF_EM7DEG((float)(_i).lat); \ + (_o).lon = RAD_OF_EM7DEG((float)(_i).lon); \ + (_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).lon = RAD_OF_EM7DEG((double)(_i).lon); \ (_o).alt = M_OF_MM((double)(_i).alt); \ } #define NED_BFP_OF_REAL(_o, _i) { \ - (_o).x = POS_BFP_OF_REAL((_i).x); \ - (_o).y = POS_BFP_OF_REAL((_i).y); \ - (_o).z = POS_BFP_OF_REAL((_i).z); \ + (_o).x = (int32_t)POS_BFP_OF_REAL((_i).x); \ + (_o).y = (int32_t)POS_BFP_OF_REAL((_i).y); \ + (_o).z = (int32_t)POS_BFP_OF_REAL((_i).z); \ } #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); \ } +#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 } /* extern "C" */ #endif diff --git a/sw/airborne/math/pprz_geodetic_utm.h b/sw/airborne/math/pprz_geodetic_utm.h index 18fd8a27ed..1e4926a0e6 100644 --- a/sw/airborne/math/pprz_geodetic_utm.h +++ b/sw/airborne/math/pprz_geodetic_utm.h @@ -34,6 +34,8 @@ extern "C" { #endif +#include "std.h" + /* Computation for the WGS84 geoid only */ #define E 0.08181919106 #define K0 0.9996 @@ -43,6 +45,8 @@ extern "C" { #define N (K0*A) #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] = { 0.99832429842242842444, diff --git a/tests/math/test_pprz_geodetic.c b/tests/math/test_pprz_geodetic.c index 9b4414545d..47ae50f6c1 100644 --- a/tests/math/test_pprz_geodetic.c +++ b/tests/math/test_pprz_geodetic.c @@ -198,7 +198,7 @@ static void test_enu_of_ecef_int(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 }; 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; 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 lon_d_err = fabs(l_ref_d.lon - lla_d.lon); if (lat_d_err < max_err_rad && lon_d_err < max_err_rad) { note(" lla_d.lat=%.12f deg lla_d.lon=%.12f deg", 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 { 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); - 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 }; @@ -235,7 +236,8 @@ static void test_lla_of_utm(void) float lat_f_err = fabs(l_ref_d.lat - lla_f.lat); 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", DegOfRad(lla_f.lat), DegOfRad(lla_f.lon)); 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); 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) @@ -349,7 +370,7 @@ static void test_lla_of_ecef(void) struct LlaCoor_i lla_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.lon, "==", lla_ref_i.lon, "longitude (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() { note("runing geodetic math tests"); - plan(12); + plan(13); test_ecef_of_ned_int(); test_enu_of_ecef_int();