Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2009-01-30 20:46:24 +00:00
parent 770a99808d
commit de3406ea98
5 changed files with 108 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/pprz_algebra_float.h
+7
View File
@@ -3,6 +3,13 @@
#define PPRZ_FLOAT_VECT_COPY(_o, _i) { \
_o.x = _i.x; \
_o.y = _i.y; \
_o.z = _i.z; \
}
#endif /* PPRZ_ALGEBRA_FLOAT_H */
sw/airborne/pprz_geodetic_float.c
+2 -3
View File
@@ -1,11 +1,10 @@
#include "pprz_geodetic_float.h"
#include "pprz_algebra_float.h"
#include <math.h>
void init_ltp_from_ecef_f(struct LtpRef_f* ref_param, struct EcefCoor_f* ref_pos) {
ref_param->ecef.x = ref_pos->x;
ref_param->ecef.y = ref_pos->y;
ref_param->ecef.z = ref_pos->z;
PPRZ_FLOAT_VECT_COPY(ref_param->ecef, (*ref_pos));
/* compute lon and lat */
}
sw/airborne/pprz_geodetic_float.h
+43
View File
@@ -0,0 +1,43 @@
#ifndef PPRZ_GEODETIC_FLOAT_H
#define PPRZ_GEODETIC_FLOAT_H
/* Earth Centered Earth Fixed in meters */
struct EcefCoor_f {
FLOAT_T x;
FLOAT_T y;
FLOAT_T z;
};
/* lon, lat in radians */
/* alt in meters */
struct LlaCoor_f {
FLOAT_T lon;
FLOAT_T lat;
FLOAT_T alt;
};
/* North East Down local tangeant plane */
struct NedCoor_f {
FLOAT_T x;
FLOAT_T y;
FLOAT_T z;
};
/* East North Down local tangeant plane */
struct EnuCoor_f {
FLOAT_T x;
FLOAT_T y;
FLOAT_T z;
};
/* Local tangeant plane reference */
struct LtpRef_f {
struct EcefCoor_f ecef;
struct LlaCoor_f lla;
};
extern void init_ltp_from_ecef_f(struct LtpRef_f* ref_param, struct EcefCoor_f* ref_pos);
extern void init_ltp_from_lla_f(struct LtpRef_f* ref_param, struct LlaCoor_f* ref_pos);
extern void enu_of_ecef_f(struct LtpRef_f* ref_param, struct EnuCoor_f* out, struct EcefCoor_f* in);
#endif /* PPRZ_GEODETIC_FLOAT_H */
sw/airborne/test/Makefile
+13
View File
@@ -0,0 +1,13 @@
CC = gcc
CFLAGS = -I.. -DFLOAT_T=double -Wall
LDFLAGS = -lm
test_geodetic: test_geodetic.c ../pprz_geodetic_float.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
clean:
rm -f *~ test_geodetic
sw/airborne/test/test_geodetic.c
+43
View File
@@ -0,0 +1,43 @@
/*
http://en.wikipedia.org/wiki/Geodetic_system
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#define DEG_OF_RAD(_r) (_r * 180. / M_PI)
#define RAD_OF_DEG(_d) (_d / 180. * M_PI)
#include "pprz_geodetic_float.h"
/*
* toulouse 43.605278,1.442778,180.0 -> 4624497.0 116475.0 4376563.0
*/
int main(int argc, char** argv) {
struct LlaCoor_f ref_coor;
ref_coor.lat = RAD_OF_DEG(43.605278);
ref_coor.lon = RAD_OF_DEG(1.442778);
ref_coor.alt = 180.0;
struct LtpRef_f lpt_param;
init_ltp_from_lla_f(&lpt_param, &ref_coor);
/* FIXME: init_ltp_from_lla_f not implemented */
lpt_param.ecef.x = 4624497.0;
lpt_param.ecef.y = 116475.0;
lpt_param.ecef.z = 4376563.0;
struct EcefCoor_f my_ecef_point = { 4624491.0, 116475.0, 4376563.0};
struct EnuCoor_f my_enu_point;
enu_of_ecef_f(&lpt_param, &my_enu_point, &my_ecef_point);
printf("ecef to enu : (%f,%f,%f) -> (%f,%f,%f)\n",
my_ecef_point.x, my_ecef_point.y, my_ecef_point.z,
my_enu_point.x, my_enu_point.y, my_enu_point.z );
return 0;
}