pprz_geodetic_float.c File Reference

Paparazzi floating point math for geodetic calculations. More...

#include "pprz_geodetic_float.h"
#include "pprz_algebra_float.h"
#include <math.h>
#include "pprz_geodetic_double.h"
#include "math/pprz_geodetic_utm.h"

Include dependency graph for pprz_geodetic_float.c:

Go to the source code of this file.

Data Structures
struct	complex

Macros
#define	CScal(k, z)   { z.re *= k; z.im *= k; }
#define	CAdd(z1, z2)   { z2.re += z1.re; z2.im += z1.im; }
#define	CSub(z1, z2)   { z2.re -= z1.re; z2.im -= z1.im; }
#define	CI(z)   { float tmp = z.re; z.re = - z.im; z.im = tmp; }
#define	CExp(z)   { float e = exp(z.re); z.re = e*cos(z.im); z.im = e*sin(z.im); }
#define	CSin(z)   { CI(z); struct complex _z = {-z.re, -z.im}; float e = exp(z.re); float cos_z_im = cosf(z.im); z.re = e*cos_z_im; float sin_z_im = sinf(z.im); z.im = e*sin_z_im; _z.re = cos_z_im/e; _z.im = -sin_z_im/e; CSub(_z, z); CScal(-0.5, z); CI(z); }

Functions
void	ltp_def_from_ecef_f (struct LtpDef_f *def, struct EcefCoor_f *ecef)
void	ltp_def_from_lla_f (struct LtpDef_f *def, struct LlaCoor_f *lla)
void	enu_of_ecef_point_f (struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void	ned_of_ecef_point_f (struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void	enu_of_ecef_vect_f (struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void	ned_of_ecef_vect_f (struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void	enu_of_lla_point_f (struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla)
void	ned_of_lla_point_f (struct NedCoor_f *ned, struct LtpDef_f *def, struct LlaCoor_f *lla)
void	ecef_of_enu_point_f (struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu)
void	ecef_of_ned_point_f (struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
void	ecef_of_enu_vect_f (struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu)
void	ecef_of_ned_vect_f (struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
void	lla_of_ecef_f (struct LlaCoor_f *out, struct EcefCoor_f *in)
void	ecef_of_lla_f (struct EcefCoor_f *out, struct LlaCoor_f *in)
static float	isometric_latitude_f (float phi, float e)
static float	isometric_latitude_fast_f (float phi)
static float	inverse_isometric_latitude_f (float lat, float e, float epsilon)
void	utm_of_lla_f (struct UtmCoor_f *utm, struct LlaCoor_f *lla)
void	lla_of_utm_f (struct LlaCoor_f *lla, struct UtmCoor_f *utm)

Detailed Description

Paparazzi floating point math for geodetic calculations.

Definition in file pprz_geodetic_float.c.

---

Data Structure Documentation

struct complex

Definition at line 266 of file pprz_geodetic_float.c.

Data Fields
float	im
float	re

Macro Definition Documentation

#define CAdd	(		z1,
			z2
	)		{ z2.re += z1.re; z2.im += z1.im; }

Definition at line 268 of file pprz_geodetic_float.c.

Referenced by utm_of_lla_f().

#define CExp

(

z

)

{ float e = exp(z.re); z.re = e*cos(z.im); z.im = e*sin(z.im); }

Definition at line 271 of file pprz_geodetic_float.c.

#define CI

(

z

)

{ float tmp = z.re; z.re = - z.im; z.im = tmp; }

Definition at line 270 of file pprz_geodetic_float.c.

#define CScal	(		k,
			z
	)		{ z.re *= k; z.im *= k; }

Definition at line 267 of file pprz_geodetic_float.c.

Referenced by lla_of_utm_f(), and utm_of_lla_f().

#define CSin

(

z

)

{ CI(z); struct complex _z = {-z.re, -z.im}; float e = exp(z.re); float cos_z_im = cosf(z.im); z.re = e*cos_z_im; float sin_z_im = sinf(z.im); z.im = e*sin_z_im; _z.re = cos_z_im/e; _z.im = -sin_z_im/e; CSub(_z, z); CScal(-0.5, z); CI(z); }

Definition at line 274 of file pprz_geodetic_float.c.

Referenced by lla_of_utm_f(), and utm_of_lla_f().

#define CSub	(		z1,
			z2
	)		{ z2.re -= z1.re; z2.im -= z1.im; }

Definition at line 269 of file pprz_geodetic_float.c.

Referenced by lla_of_utm_f().

Function Documentation

void ecef_of_enu_point_f	(	struct EcefCoor_f *	ecef,
		struct LtpDef_f *	def,
		struct EnuCoor_f *	enu
	)

Definition at line 129 of file pprz_geodetic_float.c.

References LtpDef_f::ecef, LtpDef_f::ltp_of_ecef, DoubleRMat::m, FloatRMat::m, MAT33_VECT3_TRANSP_MUL, EcefCoor_f::x, EcefCoor_d::x, EnuCoor_f::x, EcefCoor_f::y, EcefCoor_d::y, EnuCoor_f::y, EcefCoor_f::z, EcefCoor_d::z, and EnuCoor_f::z.

Referenced by ecef_of_ned_point_f().

Here is the caller graph for this function:

void ecef_of_enu_vect_f	(	struct EcefCoor_f *	ecef,
		struct LtpDef_f *	def,
		struct EnuCoor_f *	enu
	)

Definition at line 164 of file pprz_geodetic_float.c.

References LtpDef_f::ltp_of_ecef, DoubleRMat::m, FloatRMat::m, MAT33_VECT3_TRANSP_MUL, EcefCoor_f::x, EcefCoor_d::x, EnuCoor_f::x, EcefCoor_f::y, EcefCoor_d::y, EnuCoor_f::y, EcefCoor_f::z, EcefCoor_d::z, and EnuCoor_f::z.

Referenced by ecef_of_ned_vect_f().

Here is the caller graph for this function:

void ecef_of_lla_f	(	struct EcefCoor_f *	out,
		struct LlaCoor_f *	in
	)

Definition at line 241 of file pprz_geodetic_float.c.

References LlaCoor_f::alt, LlaCoor_f::lat, LlaCoor_f::lon, EcefCoor_f::x, EcefCoor_f::y, and EcefCoor_f::z.

Referenced by ecef_of_lla_i(), enu_of_lla_point_f(), ltp_def_from_lla_f(), ned_of_lla_point_f(), nmea_parse_GGA(), stateCalcPositionEcef_f(), stateCalcPositionEcef_i(), stateCalcPositionEnu_i(), and stateCalcPositionNed_i().

Here is the caller graph for this function:

void ecef_of_ned_point_f	(	struct EcefCoor_f *	ecef,
		struct LtpDef_f *	def,
		struct NedCoor_f *	ned
	)

Definition at line 157 of file pprz_geodetic_float.c.

References ecef_of_enu_point_f(), and ENU_OF_TO_NED.

Referenced by stateCalcPositionEcef_f(), stateCalcPositionEcef_i(), and stateCalcPositionLla_f().

Here is the call graph for this function:

Here is the caller graph for this function:

void ecef_of_ned_vect_f	(	struct EcefCoor_f *	ecef,
		struct LtpDef_f *	def,
		struct NedCoor_f *	ned
	)

Definition at line 192 of file pprz_geodetic_float.c.

References ecef_of_enu_vect_f(), and ENU_OF_TO_NED.

Referenced by stateCalcAccelEcef_f(), and stateCalcSpeedEcef_f().

Here is the call graph for this function:

Here is the caller graph for this function:

void enu_of_ecef_point_f	(	struct EnuCoor_f *	enu,
		struct LtpDef_f *	def,
		struct EcefCoor_f *	ecef
	)

Definition at line 85 of file pprz_geodetic_float.c.

References LtpDef_f::ecef, LtpDef_f::ltp_of_ecef, MAT33_VECT3_MUL, and VECT3_DIFF.

Referenced by enu_of_lla_point_f(), ned_of_ecef_point_f(), stateCalcPositionEnu_f(), and stateCalcPositionEnu_i().

Here is the caller graph for this function:

void enu_of_ecef_vect_f	(	struct EnuCoor_f *	enu,
		struct LtpDef_f *	def,
		struct EcefCoor_f *	ecef
	)

Definition at line 100 of file pprz_geodetic_float.c.

References LtpDef_f::ltp_of_ecef, and MAT33_VECT3_MUL.

Referenced by ned_of_ecef_vect_f(), and stateCalcSpeedEnu_f().

Here is the caller graph for this function:

void enu_of_lla_point_f	(	struct EnuCoor_f *	enu,
		struct LtpDef_f *	def,
		struct LlaCoor_f *	lla
	)

Definition at line 112 of file pprz_geodetic_float.c.

References ecef_of_lla_f(), and enu_of_ecef_point_f().

Referenced by acInfoCalcPositionEnu_f(), acInfoCalcPositionEnu_i(), and stateCalcPositionEnu_f().

Here is the call graph for this function:

Here is the caller graph for this function:

static float inverse_isometric_latitude_f ( float  lat, float  e, float  epsilon  )

inlinestatic

Definition at line 287 of file pprz_geodetic_float.c.

Referenced by lla_of_utm_f().

Here is the caller graph for this function:

static float isometric_latitude_f ( float  phi, float  e  )

inlinestatic

Definition at line 277 of file pprz_geodetic_float.c.

Referenced by utm_of_lla_f().

Here is the caller graph for this function:

static float isometric_latitude_fast_f ( float  phi )

inlinestatic

Definition at line 282 of file pprz_geodetic_float.c.

Referenced by lla_of_utm_f(), and utm_of_lla_f().

Here is the caller graph for this function:

void lla_of_ecef_f	(	struct LlaCoor_f *	out,
		struct EcefCoor_f *	in
	)

Definition at line 204 of file pprz_geodetic_float.c.

References LlaCoor_f::alt, LlaCoor_f::lat, LlaCoor_f::lon, P, Q, EcefCoor_f::x, EcefCoor_f::y, and EcefCoor_f::z.

Referenced by lla_of_ecef_i(), ltp_def_from_ecef_f(), and stateCalcPositionLla_f().

Here is the caller graph for this function:

void lla_of_utm_f	(	struct LlaCoor_f *	lla,
		struct UtmCoor_f *	utm
	)

Definition at line 344 of file pprz_geodetic_float.c.

References LlaCoor_f::alt, UtmCoor_f::alt, CScal, CSin, CSub, DELTA_EAST, DELTA_NORTH, E, UtmCoor_f::east, complex::im, inverse_isometric_latitude_f(), isometric_latitude_fast_f(), LambdaOfUtmZone, LlaCoor_f::lat, LlaCoor_f::lon, N, UtmCoor_f::north, complex::re, scale, serie_coeff_proj_mercator, serie_coeff_proj_mercator_inverse, and UtmCoor_f::zone.

Referenced by acInfoCalcPositionLla_f(), acInfoCalcPositionLla_i(), ins_reset_utm_zone(), lla_of_utm_i(), stateCalcPositionLla_f(), stateCalcPositionLla_i(), and vPoint().

Here is the call graph for this function:

Here is the caller graph for this function:

void ltp_def_from_ecef_f	(	struct LtpDef_f *	def,
		struct EcefCoor_f *	ecef
	)

Definition at line 36 of file pprz_geodetic_float.c.

References LtpDef_f::ecef, LlaCoor_f::lat, LtpDef_f::lla, lla_of_ecef_f(), LlaCoor_f::lon, LtpDef_f::ltp_of_ecef, FloatRMat::m, and VECT3_COPY.

Here is the call graph for this function:

void ltp_def_from_lla_f	(	struct LtpDef_f *	def,
		struct LlaCoor_f *	lla
	)

Definition at line 60 of file pprz_geodetic_float.c.

References LtpDef_f::ecef, ecef_of_lla_f(), LlaCoor_f::lat, LtpDef_f::lla, LLA_COPY, LlaCoor_f::lon, LtpDef_f::ltp_of_ecef, and FloatRMat::m.

Here is the call graph for this function:

void ned_of_ecef_point_f	(	struct NedCoor_f *	ned,
		struct LtpDef_f *	def,
		struct EcefCoor_f *	ecef
	)

Definition at line 92 of file pprz_geodetic_float.c.

References enu_of_ecef_point_f(), and ENU_OF_TO_NED.

Referenced by ned_of_lla_point_f(), stateCalcPositionNed_f(), and stateCalcPositionNed_i().

Here is the call graph for this function:

Here is the caller graph for this function:

void ned_of_ecef_vect_f	(	struct NedCoor_f *	ned,
		struct LtpDef_f *	def,
		struct EcefCoor_f *	ecef
	)

Definition at line 105 of file pprz_geodetic_float.c.

References enu_of_ecef_vect_f(), and ENU_OF_TO_NED.

Referenced by ins_float_invariant_update_gps(), stateCalcAccelNed_f(), stateCalcHorizontalSpeedNorm_i(), and stateCalcSpeedNed_f().

Here is the call graph for this function:

Here is the caller graph for this function:

void ned_of_lla_point_f	(	struct NedCoor_f *	ned,
		struct LtpDef_f *	def,
		struct LlaCoor_f *	lla
	)

Definition at line 119 of file pprz_geodetic_float.c.

References ecef_of_lla_f(), and ned_of_ecef_point_f().

Referenced by stateCalcPositionNed_f().

Here is the call graph for this function:

Here is the caller graph for this function:

void utm_of_lla_f	(	struct UtmCoor_f *	utm,
		struct LlaCoor_f *	lla
	)

Definition at line 308 of file pprz_geodetic_float.c.

References LlaCoor_f::alt, UtmCoor_f::alt, CAdd, CScal, CSin, DELTA_EAST, DELTA_NORTH, E, UtmCoor_f::east, complex::im, isometric_latitude_f(), isometric_latitude_fast_f(), LambdaOfUtmZone, LlaCoor_f::lat, LlaCoor_f::lon, N, UtmCoor_f::north, complex::re, serie_coeff_proj_mercator, UtmZoneOfLlaLonRad, and UtmCoor_f::zone.

Referenced by acInfoCalcPositionUtm_f(), acInfoCalcPositionUtm_i(), copilot_parse_move_wp_dl(), firmware_parse_msg(), ins_reset_utm_zone(), mission_point_of_lla(), sim_overwrite_ins(), stateCalcPositionEnu_f(), stateCalcPositionEnu_i(), stateCalcPositionNed_f(), stateCalcPositionNed_i(), stateCalcPositionUtm_f(), and utm_of_lla_i().

Here is the call graph for this function:

Here is the caller graph for this function: