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

- 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

sw/airborne/math/pprz_geodetic_int.c

@@ -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

@@ -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

@@ -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

@@ -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
-
 
 }