[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4266] initialization of booz using geo_double funct
From: |
Gautier Hattenberger |
Subject: |
[paparazzi-commits] [4266] initialization of booz using geo_double functions for better precision |
Date: |
Mon, 19 Oct 2009 09:24:36 +0000 |
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);
}
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4266] initialization of booz using geo_double functions for better precision,
Gautier Hattenberger <=