Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[math] geodetic_int: add extra ecef <-> enu/ned position functions

Browse Source 
- ecef_of_enu_point_i has coordinates in cm
- ecef_of_enu_pos_i has ned in meter with INT32_POS_FRAC and ecef with cm
This commit is contained in:
Felix Ruess
2013-09-25 18:27:02 +02:00
parent 4e7975cea9
commit f9ce4aed2c
4 changed files with 215 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/math/pprz_geodetic_int.c
+73 -1
View File
@@ -92,11 +92,40 @@ void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct Ece
void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
struct EnuCoor_i enu;
enu_of_ecef_point_i(&enu, def, ecef);
ENU_OF_TO_NED(*ned, enu);
}
/** Convert a ECEF position to local ENU.
* @param[out] enu ENU position in meter << #INT32_POS_FRAC
* @param[in] def local coordinate system definition
* @param[in] ecef ECEF position in cm
*/
void enu_of_ecef_pos_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
struct EnuCoor_i enu_cm;
enu_of_ecef_point_i(&enu_cm, def, ecef);
/* enu = (enu_cm / 100) << INT32_POS_FRAC
* to loose less range:
* enu_cm = enu << (INT32_POS_FRAC-2) / 25
* which puts max enu output Q23.8 range to 8388km / 25 = 335km
*/
INT32_VECT3_LSHIFT(*enu, enu_cm, INT32_POS_FRAC-2);
VECT3_SDIV(*enu, *enu, 25);
}
/** Convert a ECEF position to local NED.
* @param[out] ned NED position in meter << #INT32_POS_FRAC
* @param[in] def local coordinate system definition
* @param[in] ecef ECEF position in cm
*/
void ned_of_ecef_pos_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
struct EnuCoor_i enu;
enu_of_ecef_pos_i(&enu, def, ecef);
ENU_OF_TO_NED(*ned, enu);
}
void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
@@ -149,11 +178,23 @@ void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct Ne
ecef_of_enu_vect_i(ecef, def, &enu);
}
/** Convert a point in local ENU to ECEF.
* @param[out] ecef ECEF point in cm
* @param[in] def local coordinate system definition
* @param[in] enu ENU point in cm
*/
void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
ecef_of_enu_vect_i(ecef, def, enu);
INT32_VECT3_ADD(*ecef, def->ecef);
}
/** Convert a point in local NED to ECEF.
* @param[out] ecef ECEF point in cm
* @param[in] def local coordinate system definition
* @param[in] ned NED point in cm
*/
void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) {
struct EnuCoor_i enu;
ENU_OF_TO_NED(enu, *ned);
@@ -161,6 +202,37 @@ void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct N
}
/** Convert a local ENU position to ECEF.
* @param[out] ecef ECEF position in cm
* @param[in] def local coordinate system definition
* @param[in] enu ENU position in meter << #INT32_POS_FRAC
*/
void ecef_of_enu_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
/* enu_cm = (enu * 100) >> INT32_POS_FRAC
* to loose less range:
* enu_cm = (enu * 25) >> (INT32_POS_FRAC-2)
* which puts max enu input Q23.8 range to 8388km / 25 = 335km
*/
struct EnuCoor_i enu_cm;
VECT3_SMUL(enu_cm, *enu, 25);
INT32_VECT3_RSHIFT(enu_cm, enu_cm, INT32_POS_FRAC-2);
ecef_of_enu_vect_i(ecef, def, &enu_cm);
INT32_VECT3_ADD(*ecef, def->ecef);
}
/** 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 ecef_of_ned_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) {
struct EnuCoor_i enu;
ENU_OF_TO_NED(enu, *ned);
ecef_of_enu_pos_i(ecef, def, &enu);
}
void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) {
struct EcefCoor_i ecef;
ecef_of_lla_i(&ecef,lla);
sw/airborne/math/pprz_geodetic_int.h
+4
View File
@@ -104,6 +104,8 @@ 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);
extern void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef);
extern void enu_of_ecef_pos_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef);
extern void ned_of_ecef_pos_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef);
extern void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef);
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);
@@ -112,6 +114,8 @@ extern void enu_of_lla_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struc
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_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu);
extern void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned);
extern void ecef_of_enu_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu);
extern void ecef_of_ned_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned);
extern void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu);
extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned);
sw/airborne/test/Makefile
+2 -1
View File
@@ -4,7 +4,8 @@ Q=@
CC = gcc
CFLAGS = -std=c99 -I.. -I../../include -I../booz -I../../booz -Wall
CFLAGS = -std=c99 -I.. -I../../include -Wall
#CFLAGS += -DDEBUG
LDFLAGS = -lm
sw/airborne/test/test_geodetic.c
+91 -14
View File
@@ -20,6 +20,7 @@ static void test_lla_of_utm(void);
static void test_floats(void);
static void test_doubles(void);
static void test_enu_of_ecef_int(void);
static void test_ecef_of_ned_int(void);
static void test_ned_to_ecef_to_ned(void);
static void test_enu_to_ecef_to_enu( void );
@@ -32,12 +33,16 @@ int main(int argc, char** argv) {
//test_floats();
//test_doubles();
test_lla_of_utm();
//test_lla_of_utm();
//test_ned_to_ecef_to_ned();
//test_enu_to_ecef_to_enu();
test_ecef_of_ned_int();
//test_enu_of_ecef_int();
// test_ned_to_ecef_to_ned();
// test_enu_to_ecef_to_enu();
return 0;
}
@@ -78,7 +83,8 @@ static void test_floats(void) {
struct LtpDef_f ltp_def;
ltp_def_from_ecef_f(&ltp_def, &ref_coor);
printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), ltp_def.lla.alt);
printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon),
ltp_def.lla.alt);
struct EcefCoor_f my_ecef_point = ref_coor;
struct EnuCoor_f my_enu_point;
@@ -105,7 +111,8 @@ static void test_doubles(void) {
struct LtpDef_d ltp_def;
ltp_def_from_ecef_d(&ltp_def, &ref_coor);
printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), ltp_def.lla.alt);
printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon),
ltp_def.lla.alt);
struct EcefCoor_d my_ecef_point = ref_coor;
struct EnuCoor_d my_enu_point;
@@ -117,6 +124,78 @@ static void test_doubles(void) {
printf("\n");
}
static void test_ecef_of_ned_int(void) {
printf("\n--- ecef_of_ned int ---\n");
struct EcefCoor_d ref_coor_d = { 4624497.0 , 116475.0, 4376563.0};
struct LtpDef_d ltp_def_d;
ltp_def_from_ecef_d(&ltp_def_d, &ref_coor_d);
struct EcefCoor_i ref_coor_i = { rint(CM_OF_M(ref_coor_d.x)),
rint(CM_OF_M(ref_coor_d.y)),
rint(CM_OF_M(ref_coor_d.z))};
printf("ecef0 : (%d,%d,%d)\n", ref_coor_i.x, ref_coor_i.y, ref_coor_i.z);
struct LtpDef_i ltp_def_i;
ltp_def_from_ecef_i(&ltp_def_i, &ref_coor_i);
#define STEP_NED 100.0
#define RANGE_NED 10000.
double sum_err = 0;
struct FloatVect3 max_err;
FLOAT_VECT3_ZERO(max_err);
struct FloatVect3 offset;
for (offset.x=-RANGE_NED; offset.x<=RANGE_NED; offset.x+=STEP_NED) {
for (offset.y=-RANGE_NED; offset.y<=RANGE_NED; offset.y+=STEP_NED) {
for (offset.z=-RANGE_NED; offset.z<=RANGE_NED; offset.z+=STEP_NED) {
struct NedCoor_d my_ned_point_d;
VECT3_COPY(my_ned_point_d, offset);
struct EcefCoor_d my_ecef_point_d;
ecef_of_ned_point_d(&my_ecef_point_d, &ltp_def_d, &my_ned_point_d);
#if DEBUG
printf("ned to ecef double : (%.02f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n",
my_enu_point_f.x, my_enu_point_f.y, my_enu_point_f.z,
my_ecef_point_f.x, my_ecef_point_f.y, my_ecef_point_f.z);
#endif
struct NedCoor_i my_ned_point_i;
POSITIONS_BFP_OF_REAL(my_ned_point_i, my_ned_point_d);
struct EcefCoor_i my_ecef_point_i;
ecef_of_ned_pos_i(&my_ecef_point_i, &ltp_def_i, &my_ned_point_i);
#if DEBUG
//printf("def->ecef (%d,%d,%d)\n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z);
printf("ned to ecef int : (%.2f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n\n",
POS_BFP_OF_REAL(my_ned_point_i.x),
POS_BFP_OF_REAL(my_ned_point_i.y),
POS_BFP_OF_REAL(my_ned_point_i.z),
M_OF_CM((double)my_ecef_point_i.x),
M_OF_CM((double)my_ecef_point_i.y),
M_OF_CM((double)my_ecef_point_i.z));
#endif
float ex = my_ecef_point_d.x - M_OF_CM((double)my_ecef_point_i.x);
if (fabs(ex) > max_err.x) max_err.x = fabs(ex);
float ey = my_ecef_point_d.y - M_OF_CM((double)my_ecef_point_i.y);
if (fabs(ey) > max_err.y) max_err.y = fabs(ey);
float ez = my_ecef_point_d.z - M_OF_CM((double)my_ecef_point_i.z);
if (fabs(ez) > max_err.z) max_err.z = fabs(ez);
sum_err += ex*ex + ey*ey + ez*ez;
}
}
}
double nb_samples = (2*RANGE_NED / STEP_NED + 1) * (2*RANGE_NED / STEP_NED + 1) *
(2*RANGE_NED / STEP_NED + 1);
printf("ecef_of_ned_pos int/float comparison:\n");
printf("error max (%f,%f,%f) m\n", max_err.x, max_err.y, max_err.z );
printf("error avg (%f ) m \n", sqrt(sum_err) / nb_samples );
printf("\n");
}
static void test_enu_of_ecef_int(void) {
@@ -131,7 +210,8 @@ static void test_enu_of_ecef_int(void) {
printf("ecef0 : (%d,%d,%d)\n", ref_coor_i.x, ref_coor_i.y, ref_coor_i.z);
struct LtpDef_i ltp_def_i;
ltp_def_from_ecef_i(&ltp_def_i, &ref_coor_i);
printf("lla0 : (%f deg, %f deg, %f m) (%f,%f,%f)\n", DegOfRad(ltp_def_f.lla.lat), DegOfRad(ltp_def_f.lla.lon), ltp_def_f.lla.alt,
printf("lla0 : float (%f deg, %f deg, %f m) , int (%f, %f, %f)\n",
DegOfRad(ltp_def_f.lla.lat), DegOfRad(ltp_def_f.lla.lon), ltp_def_f.lla.alt,
DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lat)),
DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lon)),
M_OF_MM((double)ltp_def_i.lla.alt));
@@ -162,7 +242,7 @@ static void test_enu_of_ecef_int(void) {
enu_of_ecef_point_i(&my_enu_point_i, &ltp_def_i, &my_ecef_point_i);
#if DEBUG
// printf("def->ecef (%d,%d,%d)\n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z);
//printf("def->ecef (%d,%d,%d)\n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z);
printf("ecef to enu int : (%.2f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n\n",
M_OF_CM((double)my_ecef_point_i.x),
M_OF_CM((double)my_ecef_point_i.y),
@@ -191,7 +271,6 @@ static void test_enu_of_ecef_int(void) {
printf("error avg (%f ) m \n", sqrt(sum_err) / nb_samples );
printf("\n");
}
@@ -200,7 +279,8 @@ static void test_enu_of_ecef_int(void) {
void test_ned_to_ecef_to_ned( void ) {
#if 0
printf("\n--- NED -> ECEF -> NED in double ---\n");
struct EcefCoor_d ref_coor = { 4624497.0 , 116475.0, 4376563.0};
printf("ecef0 : (%.02f,%.02f,%.02f)\n", ref_coor.x, ref_coor.y, ref_coor.z);
@@ -221,8 +301,6 @@ void test_ned_to_ecef_to_ned( void ) {
ecef_p2.x, ecef_p2.y, ecef_p2.z);
printf("\n");
#endif
}
@@ -233,6 +311,8 @@ void test_ned_to_ecef_to_ned( void ) {
void test_enu_to_ecef_to_enu( void ) {
printf("\n--- ENU -> ECEF -> ENU in float ---\n");
struct EcefCoor_f ref_coor = { 4624497.0 , 116475.0, 4376563.0};
printf("ecef0 : (%.02f,%.02f,%.02f)\n", ref_coor.x, ref_coor.y, ref_coor.z);
@@ -246,7 +326,6 @@ void test_enu_to_ecef_to_enu( void ) {
ecef_p1.x, ecef_p1.y, ecef_p1.z,
enu_p1.x, enu_p1.y, enu_p1.z );
#if 0
struct EcefCoor_f ecef_p2;
ecef_of_enu_point_f(&ecef_p2, &ltp_def, &enu_p1);
printf("enu to ecef : (%f,%f,%f) -> (%f,%f,%f)\n",
@@ -254,8 +333,6 @@ void test_enu_to_ecef_to_enu( void ) {
ecef_p2.x, ecef_p2.y, ecef_p2.z);
printf("\n");
#endif
}