Skip to content

Commit

Permalink
math: added ltp_def_from_lla and ecef_of_XX_f by converting to double…
Browse files Browse the repository at this point in the history 
… first
  • Loading branch information
@flixr
flixr committed Feb 19, 2011
1 parent c54e783 commit be57123
Show file tree
Hide file tree
Showing 4 changed files with 120 additions and 18 deletions.
91 changes: 77 additions & 14 deletions sw/airborne/math/pprz_geodetic_float.c
View file
Expand Up @@ -3,6 +3,9 @@
#include "pprz_algebra_float.h"
#include <math.h>

/* for ecef_of_XX functions the double versions are needed */
#include "pprz_geodetic_double.h"

void ltp_def_from_ecef_f(struct LtpDef_f* def, struct EcefCoor_f* ecef) {

/* store the origin of the tangeant plane */
Expand All @@ -16,7 +19,7 @@ void ltp_def_from_ecef_f(struct LtpDef_f* def, struct EcefCoor_f* ecef) {
const float cos_lon = cosf(def->lla.lon);
def->ltp_of_ecef.m[0] = -sin_lon;
def->ltp_of_ecef.m[1] = cos_lon;
def->ltp_of_ecef.m[2] = 0.;
def->ltp_of_ecef.m[2] = 0.; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
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;
Expand All @@ -26,14 +29,28 @@ void ltp_def_from_ecef_f(struct LtpDef_f* def, struct EcefCoor_f* ecef) {

}

#if 0
void init_ltp_ref_from_lla_f(struct LtpRef_f* def, struct LlaCoor_f* ref_pos) {
def->lla.lon = ref_pos->lon;
def->lla.lat = ref_pos->lat;
/* compute ecef */
void ltp_def_from_lla_f(struct LtpDef_f* def, struct LlaCoor_f* lla) {
/* store the origin of the tangeant plane */
LLA_COPY(def->lla, *lla);
/* compute the ecef representation of the origin */
ecef_of_lla_f(&def->ecef, &def->lla);

/* store the rotation matrix */
const float sin_lat = sinf(def->lla.lat);
const float cos_lat = cosf(def->lla.lat);
const float sin_lon = sinf(def->lla.lon);
const float cos_lon = cosf(def->lla.lon);

def->ltp_of_ecef.m[0] = -sin_lon;
def->ltp_of_ecef.m[1] = cos_lon;
def->ltp_of_ecef.m[2] = 0.; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
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;
}
#endif

void enu_of_ecef_point_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct EcefCoor_f* ecef) {
struct EcefCoor_f delta;
Expand All @@ -58,29 +75,75 @@ void ned_of_ecef_vect_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct Ecef
ENU_OF_TO_NED(*ned, enu);
}

/* not enought precision with float - use double */
# if 0
/*
* not enought precision with float - use double
*/
void ecef_of_enu_point_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct EnuCoor_f* enu) {
MAT33_VECT3_TRANSP_MUL(*ecef, def->ltp_of_ecef.m, *enu);
VECT3_ADD(*ecef, def->ecef);
/* convert used floats to double */
struct DoubleMat33 ltp_of_ecef_d;
ltp_of_ecef_d.m[0] = (double) def->ltp_of_ecef.m[0];
ltp_of_ecef_d.m[1] = (double) def->ltp_of_ecef.m[1];
ltp_of_ecef_d.m[2] = (double) def->ltp_of_ecef.m[2];
ltp_of_ecef_d.m[3] = (double) def->ltp_of_ecef.m[3];
ltp_of_ecef_d.m[4] = (double) def->ltp_of_ecef.m[4];
ltp_of_ecef_d.m[5] = (double) def->ltp_of_ecef.m[5];
ltp_of_ecef_d.m[6] = (double) def->ltp_of_ecef.m[6];
ltp_of_ecef_d.m[7] = (double) def->ltp_of_ecef.m[7];
ltp_of_ecef_d.m[8] = (double) def->ltp_of_ecef.m[8];
struct EnuCoor_f enu_d;
enu_d.x = (double) enu->x;
enu_d.y = (double) enu->y;
enu_d.z = (double) enu->z;

/* compute in double */
struct EcefCoor_d ecef_d;
MAT33_VECT3_TRANSP_MUL(ecef_d, ltp_of_ecef_d, enu_d);

/* convert result back to float and add it*/
ecef->x = (float) ecef_d.x + def->ecef.x;
ecef->y = (float) ecef_d.y + def->ecef.y;
ecef->z = (float) ecef_d.z + def->ecef.z;
}

void ecef_of_ned_point_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct NedCoor_f* ned) {
struct EnuCoor_f enu;
ENU_OF_TO_NED(enu, *ned);
ecef_of_enu_pos_f(ecef, def, &enu);
ecef_of_enu_point_f(ecef, def, &enu);
}

void ecef_of_enu_vect_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct EnuCoor_f* enu) {
MAT33_VECT3_TRANSP_MUL(*ecef, def->ltp_of_ecef.m, *enu);
/* convert used floats to double */
struct DoubleMat33 ltp_of_ecef_d;
ltp_of_ecef_d.m[0] = (double) def->ltp_of_ecef.m[0];
ltp_of_ecef_d.m[1] = (double) def->ltp_of_ecef.m[1];
ltp_of_ecef_d.m[2] = (double) def->ltp_of_ecef.m[2];
ltp_of_ecef_d.m[3] = (double) def->ltp_of_ecef.m[3];
ltp_of_ecef_d.m[4] = (double) def->ltp_of_ecef.m[4];
ltp_of_ecef_d.m[5] = (double) def->ltp_of_ecef.m[5];
ltp_of_ecef_d.m[6] = (double) def->ltp_of_ecef.m[6];
ltp_of_ecef_d.m[7] = (double) def->ltp_of_ecef.m[7];
ltp_of_ecef_d.m[8] = (double) def->ltp_of_ecef.m[8];
struct EnuCoor_f enu_d;
enu_d.x = (double) enu->x;
enu_d.y = (double) enu->y;
enu_d.z = (double) enu->z;

/* compute in double */
struct EcefCoor_d ecef_d;
MAT33_VECT3_TRANSP_MUL(ecef_d, ltp_of_ecef_d, enu_d);

/* convert result back to float*/
ecef->x = (float) ecef_d.x;
ecef->y = (float) ecef_d.y;
ecef->z = (float) ecef_d.z;
}

void ecef_of_ned_vect_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct NedCoor_f* ned) {
struct EnuCoor_f enu;
ENU_OF_TO_NED(enu, *ned);
ecef_of_enu_vect_f(ecef, def, &enu);
}
#endif
/* end use double versions */



Expand Down
5 changes: 2 additions & 3 deletions sw/airborne/math/pprz_geodetic_float.h
View file
Expand Up @@ -41,7 +41,7 @@ struct LtpDef_f {
};

extern void ltp_def_from_ecef_f(struct LtpDef_f* def, struct EcefCoor_f* ecef);
//extern void ltp_def_from_lla_f(struct LtpDef_f* def, struct LlaCoor_f* lla);
extern void ltp_def_from_lla_f(struct LtpDef_f* def, struct LlaCoor_f* lla);
extern void lla_of_ecef_f(struct LlaCoor_f* out, struct EcefCoor_f* in);
extern void ecef_of_lla_f(struct EcefCoor_f* out, struct LlaCoor_f* in);
extern void enu_of_ecef_point_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct EcefCoor_f* ecef);
Expand All @@ -50,12 +50,11 @@ extern void enu_of_ecef_vect_f(struct EnuCoor_f* enu, struct LtpDef_f* def, stru
extern void ned_of_ecef_vect_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct EcefCoor_f* ecef);

/* not enought precision with floats - used the double version */
#if 0
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);
extern void ecef_of_ned_vect_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct NedCoor_f* ned);
#endif
/* end use double versions */


#endif /* PPRZ_GEODETIC_FLOAT_H */
32 changes: 32 additions & 0 deletions sw/airborne/math/pprz_geodetic_int.c
View file
Expand Up @@ -63,6 +63,38 @@ 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 */
LLA_COPY(def->lla, *lla);
/* compute the ecef representation of the origin */
ecef_of_lla_i(&def->ecef, &def->lla);
/* store the rotation matrix */

#if 1
int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
#else
int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
#endif


def->ltp_of_ecef.m[0] = -sin_lon;
def->ltp_of_ecef.m[1] = cos_lon;
def->ltp_of_ecef.m[2] = 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
def->ltp_of_ecef.m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[5] = cos_lat;
def->ltp_of_ecef.m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[8] = sin_lat;

}

void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) {

Expand Down
10 changes: 9 additions & 1 deletion sw/airborne/math/pprz_geodetic_int.h
View file
Expand Up @@ -46,7 +46,7 @@ struct LtpDef_i {
};

extern void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef);
//extern void ltp_def_from_lla_i(struct LtpRef_i* def, struct LlaCoor_i* lla);
extern void ltp_def_from_lla_i(struct LtpDef_i* def, struct LlaCoor_i* lla);
extern void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in);
extern void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in);
extern void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef);
Expand All @@ -55,6 +55,14 @@ extern void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, stru
extern void ned_of_ecef_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef);
extern void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla);
extern void ned_of_lla_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla);
#if 0
extern void enu_of_lla_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla);
extern void ned_of_lla_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla);
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);
extern void ecef_of_ned_vect_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct NedCoor_f* ned);
#endif

#define INT32_VECT3_ENU_OF_NED(_o, _i) { \
(_o).x = (_i).y; \
Expand Down

0 comments on commit be57123

Please sign in to comment.