[paparazzi-commits] [4266] initialization of booz using geo_double functions for better precision

[Gautier Hattenberger]

Revision: 4266
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4266
Author:   gautier
Date:     2009-10-19 09:24:35 +0000 (Mon, 19 Oct 2009)
Log Message:
-----------
initialization of booz using geo_double functions for better precision
normalize course for int32

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/math/pprz_algebra_int.h
    paparazzi3/trunk/sw/airborne/math/pprz_geodetic_double.c
    paparazzi3/trunk/sw/airborne/math/pprz_geodetic_double.h
    paparazzi3/trunk/sw/airborne/math/pprz_geodetic_int.c

Modified: paparazzi3/trunk/sw/airborne/math/pprz_algebra_int.h
===================================================================
--- paparazzi3/trunk/sw/airborne/math/pprz_algebra_int.h        2009-10-18 
06:24:58 UTC (rev 4265)
+++ paparazzi3/trunk/sw/airborne/math/pprz_algebra_int.h        2009-10-19 
09:24:35 UTC (rev 4266)
@@ -92,7 +92,12 @@
     while (_a < -INT32_ANGLE_PI) _a += INT32_ANGLE_2_PI;       \
   }
 
+#define INT32_COURSE_NORMALIZE(_a) {                           \
+    while (_a < 0) _a += INT32_ANGLE_2_PI;     \
+    while (_a >= INT32_ANGLE_2_PI)  _a -= INT32_ANGLE_2_PI;    \
+  }
 
+
 struct Int16Eulers {
   int16_t phi;
   int16_t theta;

Modified: paparazzi3/trunk/sw/airborne/math/pprz_geodetic_double.c
===================================================================
--- paparazzi3/trunk/sw/airborne/math/pprz_geodetic_double.c    2009-10-18 
06:24:58 UTC (rev 4265)
+++ paparazzi3/trunk/sw/airborne/math/pprz_geodetic_double.c    2009-10-19 
09:24:35 UTC (rev 4266)
@@ -61,6 +61,25 @@
 
 }
 
+void ecef_of_lla_d(struct EcefCoor_d* ecef, struct LlaCoor_d* lla) {
+
+  // FIXME : make an ellipsoid struct
+  static const double a = 6378137.0;           /* earth semimajor axis in 
meters */
+  static const double f = 1./298.257223563;    /* reciprocal flattening        
  */
+  const double e2 = 2.*f-(f*f);                /* first eccentricity squared   
  */
+
+  const double sin_lat = sinf(lla->lat);
+  const double cos_lat = cosf(lla->lat);
+  const double sin_lon = sinf(lla->lon);
+  const double cos_lon = cosf(lla->lon);
+  const double chi = sqrtf(1. - e2*sin_lat*sin_lat);
+  const double a_chi = a / chi;
+
+  ecef->x = (a_chi + lla->alt) * cos_lat * cos_lon;
+  ecef->y = (a_chi + lla->alt) * cos_lat * sin_lon;
+  ecef->z = (a_chi*(1. - e2) + lla->alt) * sin_lat;
+}
+
 void enu_of_ecef_point_d(struct EnuCoor_d* enu, struct LtpDef_d* def, struct 
EcefCoor_d* ecef) {
   struct EcefCoor_d delta;
   VECT3_DIFF(delta, *ecef, def->ecef);

Modified: paparazzi3/trunk/sw/airborne/math/pprz_geodetic_double.h
===================================================================
--- paparazzi3/trunk/sw/airborne/math/pprz_geodetic_double.h    2009-10-18 
06:24:58 UTC (rev 4265)
+++ paparazzi3/trunk/sw/airborne/math/pprz_geodetic_double.h    2009-10-19 
09:24:35 UTC (rev 4266)
@@ -42,6 +42,7 @@
 
 extern void ltp_def_from_ecef_d(struct LtpDef_d* def, struct EcefCoor_d* ecef);
 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 enu_of_ecef_point_d(struct EnuCoor_d* ned, struct LtpDef_d* def, 
struct EcefCoor_d* ecef);
 extern void ned_of_ecef_point_d(struct NedCoor_d* ned, struct LtpDef_d* def, 
struct EcefCoor_d* ecef);

Modified: paparazzi3/trunk/sw/airborne/math/pprz_geodetic_int.c
===================================================================
--- paparazzi3/trunk/sw/airborne/math/pprz_geodetic_int.c       2009-10-18 
06:24:58 UTC (rev 4265)
+++ paparazzi3/trunk/sw/airborne/math/pprz_geodetic_int.c       2009-10-19 
09:24:35 UTC (rev 4266)
@@ -129,38 +129,39 @@
    Anyone up for writing it in fixed point ? 
 */
 #include "pprz_geodetic_float.h"
+#include "pprz_geodetic_double.h"
 
 void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in) {
 
   /* 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);
+  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);
   /* calls the floating point transformation */
-  struct LlaCoor_f out_f;
-  lla_of_ecef_f(&out_f, &in_f);
+  struct LlaCoor_d out_d;
+  lla_of_ecef_d(&out_d, &in_d);
   /* convert the output to fixed point       */
-  out->lon = (int32_t)rint(EM7RAD_OF_RAD(out_f.lon));
-  out->lat = (int32_t)rint(EM7RAD_OF_RAD(out_f.lat));
-  out->alt = (int32_t)CM_OF_M(out_f.alt);
+  out->lon = (int32_t)rint(EM7RAD_OF_RAD(out_d.lon));
+  out->lat = (int32_t)rint(EM7RAD_OF_RAD(out_d.lat));
+  out->alt = (int32_t)CM_OF_M(out_d.alt);
 
 }
 
 void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in) {
 
   /* convert our input to floating point */
-  struct LlaCoor_f in_f;
-  in_f.lon = RAD_OF_EM7RAD((float)in->lon);
-  in_f.lat = RAD_OF_EM7RAD((float)in->lat);
-  in_f.alt = M_OF_CM((float)in->alt);
+  struct LlaCoor_d in_d;
+  in_d.lon = RAD_OF_EM7RAD((double)in->lon);
+  in_d.lat = RAD_OF_EM7RAD((double)in->lat);
+  in_d.alt = M_OF_CM((double)in->alt);
   /* calls the floating point transformation */
-  struct EcefCoor_f out_f;
-  ecef_of_lla_f(&out_f, &in_f);
+  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_f.x);
-  out->y = (int32_t)CM_OF_M(out_f.y);
-  out->z = (int32_t)CM_OF_M(out_f.z);
+  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);
 
 }