LeonardoMbos - A porting of a GPS decoding and presenting progra…

Users » guiott » Code » LeonardoMbos

Guido Ottaviani / Mbed 2 deprecated LeonardoMbos

A porting of a GPS decoding and presenting program within the mbos RTOS. It is not a definitive application but a study program to test NMEA full decoding library and a first approach to an RTOS. Many thanks to Andrew Levido for his support and his patience on teaching me the RTOS principles from the other side of the Earth. It uses NMEA library by Tim (xtimor@gmail.com) ported by Ken Todotani (http://mbed.org/users/todotani/) on public mbed library (http://mbed.org/users/todotani/programs/GPS_nmeaLib/5yo4h) also available, as original universal C library, on http://nmea.sourceforge.net

Dependencies: mbos Watchdog TextLCD mbed ConfigFile

nmea/gmath.c@3:a2f9eb3b8a16, 2012-02-03 (annotated)

Committer:: guiott
Date:: Fri Feb 03 16:29:52 2012 +0000
Revision:: 3:a2f9eb3b8a16
Parent:: 0:d177c0087d1f

Who changed what in which revision?

User	Revision	Line number	New contents of line
guiott	0:d177c0087d1f	1	/*
guiott	0:d177c0087d1f	2	*
guiott	0:d177c0087d1f	3	* NMEA library
guiott	0:d177c0087d1f	4	* URL: http://nmea.sourceforge.net
guiott	0:d177c0087d1f	5	* Author: Tim (xtimor@gmail.com)
guiott	0:d177c0087d1f	6	* Licence: http://www.gnu.org/licenses/lgpl.html
guiott	0:d177c0087d1f	7	* $Id: gmath.c 17 2008-03-11 11:56:11Z xtimor $
guiott	0:d177c0087d1f	8	*
guiott	0:d177c0087d1f	9	The original gmath.c has been modified to fix a bug in:
guiott	0:d177c0087d1f	10	nmea_distance_ellipsoid() function
guiott	0:d177c0087d1f	11	according to bug report ID: 2945855
guiott	0:d177c0087d1f	12
guiott	0:d177c0087d1f	13	http://sourceforge.net/tracker/?func=detail&aid=2945855&group_id=192054&atid=939854
guiott	0:d177c0087d1f	14
guiott	0:d177c0087d1f	15	// while ((delta_lambda > 1e-12) && (remaining_steps > 0)) original by xtimor
guiott	0:d177c0087d1f	16	while (( remaining_steps == 20 ) \|\| ((fabs(delta_lambda) > 1e-12) && (remaining_steps > 0)))
guiott	0:d177c0087d1f	17
guiott	0:d177c0087d1f	18	the original code always returns a zero distance if the arrival point longitude
guiott	0:d177c0087d1f	19	is equal or smaller than the starting point one.
guiott	0:d177c0087d1f	20
guiott	0:d177c0087d1f	21	*/
guiott	0:d177c0087d1f	22
guiott	0:d177c0087d1f	23	/! \file gmath.h /
guiott	0:d177c0087d1f	24
guiott	0:d177c0087d1f	25	#include "nmea/gmath.h"
guiott	0:d177c0087d1f	26
guiott	0:d177c0087d1f	27	#include <math.h>
guiott	0:d177c0087d1f	28	#include <float.h>
guiott	0:d177c0087d1f	29
guiott	0:d177c0087d1f	30	/**
guiott	0:d177c0087d1f	31	* \fn nmea_degree2radian
guiott	0:d177c0087d1f	32	* \brief Convert degree to radian
guiott	0:d177c0087d1f	33	*/
guiott	0:d177c0087d1f	34	double nmea_degree2radian(double val)
guiott	0:d177c0087d1f	35	{ return (val * NMEA_PI180); }
guiott	0:d177c0087d1f	36
guiott	0:d177c0087d1f	37	/**
guiott	0:d177c0087d1f	38	* \fn nmea_radian2degree
guiott	0:d177c0087d1f	39	* \brief Convert radian to degree
guiott	0:d177c0087d1f	40	*/
guiott	0:d177c0087d1f	41	double nmea_radian2degree(double val)
guiott	0:d177c0087d1f	42	{ return (val / NMEA_PI180); }
guiott	0:d177c0087d1f	43
guiott	0:d177c0087d1f	44	/**
guiott	0:d177c0087d1f	45	* \brief Convert NDEG (NMEA degree) to fractional degree
guiott	0:d177c0087d1f	46	*/
guiott	0:d177c0087d1f	47	double nmea_ndeg2degree(double val)
guiott	0:d177c0087d1f	48	{
guiott	0:d177c0087d1f	49	double deg = ((int)(val / 100));
guiott	0:d177c0087d1f	50	val = deg + (val - deg * 100) / 60;
guiott	0:d177c0087d1f	51	return val;
guiott	0:d177c0087d1f	52	}
guiott	0:d177c0087d1f	53
guiott	0:d177c0087d1f	54	/**
guiott	0:d177c0087d1f	55	* \brief Convert fractional degree to NDEG (NMEA degree)
guiott	0:d177c0087d1f	56	*/
guiott	0:d177c0087d1f	57	double nmea_degree2ndeg(double val)
guiott	0:d177c0087d1f	58	{
guiott	0:d177c0087d1f	59	double int_part;
guiott	0:d177c0087d1f	60	double fra_part;
guiott	0:d177c0087d1f	61	fra_part = modf(val, &int_part);
guiott	0:d177c0087d1f	62	val = int_part * 100 + fra_part * 60;
guiott	0:d177c0087d1f	63	return val;
guiott	0:d177c0087d1f	64	}
guiott	0:d177c0087d1f	65
guiott	0:d177c0087d1f	66	/**
guiott	0:d177c0087d1f	67	* \fn nmea_ndeg2radian
guiott	0:d177c0087d1f	68	* \brief Convert NDEG (NMEA degree) to radian
guiott	0:d177c0087d1f	69	*/
guiott	0:d177c0087d1f	70	double nmea_ndeg2radian(double val)
guiott	0:d177c0087d1f	71	{ return nmea_degree2radian(nmea_ndeg2degree(val)); }
guiott	0:d177c0087d1f	72
guiott	0:d177c0087d1f	73	/**
guiott	0:d177c0087d1f	74	* \fn nmea_radian2ndeg
guiott	0:d177c0087d1f	75	* \brief Convert radian to NDEG (NMEA degree)
guiott	0:d177c0087d1f	76	*/
guiott	0:d177c0087d1f	77	double nmea_radian2ndeg(double val)
guiott	0:d177c0087d1f	78	{ return nmea_degree2ndeg(nmea_radian2degree(val)); }
guiott	0:d177c0087d1f	79
guiott	0:d177c0087d1f	80	/**
guiott	0:d177c0087d1f	81	* \brief Calculate PDOP (Position Dilution Of Precision) factor
guiott	0:d177c0087d1f	82	*/
guiott	0:d177c0087d1f	83	double nmea_calc_pdop(double hdop, double vdop)
guiott	0:d177c0087d1f	84	{
guiott	0:d177c0087d1f	85	return sqrt(pow(hdop, 2) + pow(vdop, 2));
guiott	0:d177c0087d1f	86	}
guiott	0:d177c0087d1f	87
guiott	0:d177c0087d1f	88	double nmea_dop2meters(double dop)
guiott	0:d177c0087d1f	89	{ return (dop * NMEA_DOP_FACTOR); }
guiott	0:d177c0087d1f	90
guiott	0:d177c0087d1f	91	double nmea_meters2dop(double meters)
guiott	0:d177c0087d1f	92	{ return (meters / NMEA_DOP_FACTOR); }
guiott	0:d177c0087d1f	93
guiott	0:d177c0087d1f	94	/**
guiott	0:d177c0087d1f	95	* \brief Calculate distance between two points
guiott	0:d177c0087d1f	96	* \return Distance in meters
guiott	0:d177c0087d1f	97	*/
guiott	0:d177c0087d1f	98	double nmea_distance(
guiott	0:d177c0087d1f	99	const nmeaPOS from_pos, /< From position in radians /
guiott	0:d177c0087d1f	100	const nmeaPOS to_pos /< To position in radians /
guiott	0:d177c0087d1f	101	)
guiott	0:d177c0087d1f	102	{
guiott	0:d177c0087d1f	103	double dist = ((double)NMEA_EARTHRADIUS_M) * acos(
guiott	0:d177c0087d1f	104	sin(to_pos->lat) * sin(from_pos->lat) +
guiott	0:d177c0087d1f	105	cos(to_pos->lat) * cos(from_pos->lat) * cos(to_pos->lon - from_pos->lon)
guiott	0:d177c0087d1f	106	);
guiott	0:d177c0087d1f	107	return dist;
guiott	0:d177c0087d1f	108	}
guiott	0:d177c0087d1f	109
guiott	0:d177c0087d1f	110	/**
guiott	0:d177c0087d1f	111	* \brief Calculate distance between two points
guiott	0:d177c0087d1f	112	* This function uses an algorithm for an oblate spheroid earth model.
guiott	0:d177c0087d1f	113	* The algorithm is described here:
guiott	0:d177c0087d1f	114	* http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
guiott	0:d177c0087d1f	115	* \return Distance in meters
guiott	0:d177c0087d1f	116	*/
guiott	0:d177c0087d1f	117	double nmea_distance_ellipsoid(
guiott	0:d177c0087d1f	118	const nmeaPOS from_pos, /< From position in radians /
guiott	0:d177c0087d1f	119	const nmeaPOS to_pos, /< To position in radians /
guiott	0:d177c0087d1f	120	double from_azimuth, /< (O) azimuth at "from" position in radians /
guiott	0:d177c0087d1f	121	double to_azimuth /< (O) azimuth at "to" position in radians /
guiott	0:d177c0087d1f	122	)
guiott	0:d177c0087d1f	123	{
guiott	0:d177c0087d1f	124	/* All variables */
guiott	0:d177c0087d1f	125	double f, a, b, sqr_a, sqr_b;
guiott	0:d177c0087d1f	126	double L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2;
guiott	0:d177c0087d1f	127	double sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda;
guiott	0:d177c0087d1f	128	int remaining_steps;
guiott	0:d177c0087d1f	129	double sqr_u, A, B, delta_sigma;
guiott	0:d177c0087d1f	130
guiott	0:d177c0087d1f	131	/* Check input */
guiott	0:d177c0087d1f	132	NMEA_ASSERT(from_pos != 0);
guiott	0:d177c0087d1f	133	NMEA_ASSERT(to_pos != 0);
guiott	0:d177c0087d1f	134
guiott	0:d177c0087d1f	135	if ((from_pos->lat == to_pos->lat) && (from_pos->lon == to_pos->lon))
guiott	0:d177c0087d1f	136	{ /* Identical points */
guiott	0:d177c0087d1f	137	if ( from_azimuth != 0 )
guiott	0:d177c0087d1f	138	*from_azimuth = 0;
guiott	0:d177c0087d1f	139	if ( to_azimuth != 0 )
guiott	0:d177c0087d1f	140	*to_azimuth = 0;
guiott	0:d177c0087d1f	141	return 0;
guiott	0:d177c0087d1f	142	} /* Identical points */
guiott	0:d177c0087d1f	143
guiott	0:d177c0087d1f	144	/* Earth geometry */
guiott	0:d177c0087d1f	145	f = NMEA_EARTH_FLATTENING;
guiott	0:d177c0087d1f	146	a = NMEA_EARTH_SEMIMAJORAXIS_M;
guiott	0:d177c0087d1f	147	b = (1 - f) * a;
guiott	0:d177c0087d1f	148	sqr_a = a * a;
guiott	0:d177c0087d1f	149	sqr_b = b * b;
guiott	0:d177c0087d1f	150
guiott	0:d177c0087d1f	151	/* Calculation */
guiott	0:d177c0087d1f	152	L = to_pos->lon - from_pos->lon;
guiott	0:d177c0087d1f	153	phi1 = from_pos->lat;
guiott	0:d177c0087d1f	154	phi2 = to_pos->lat;
guiott	0:d177c0087d1f	155	U1 = atan((1 - f) * tan(phi1));
guiott	0:d177c0087d1f	156	U2 = atan((1 - f) * tan(phi2));
guiott	0:d177c0087d1f	157	sin_U1 = sin(U1);
guiott	0:d177c0087d1f	158	sin_U2 = sin(U2);
guiott	0:d177c0087d1f	159	cos_U1 = cos(U1);
guiott	0:d177c0087d1f	160	cos_U2 = cos(U2);
guiott	0:d177c0087d1f	161
guiott	0:d177c0087d1f	162	/* Initialize iteration */
guiott	0:d177c0087d1f	163	sigma = 0;
guiott	0:d177c0087d1f	164	sin_sigma = sin(sigma);
guiott	0:d177c0087d1f	165	cos_sigma = cos(sigma);
guiott	0:d177c0087d1f	166	cos_2_sigmam = 0;
guiott	0:d177c0087d1f	167	sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
guiott	0:d177c0087d1f	168	sqr_cos_alpha = 0;
guiott	0:d177c0087d1f	169	lambda = L;
guiott	0:d177c0087d1f	170	sin_lambda = sin(lambda);
guiott	0:d177c0087d1f	171	cos_lambda = cos(lambda);
guiott	0:d177c0087d1f	172	delta_lambda = lambda;
guiott	0:d177c0087d1f	173	remaining_steps = 20;
guiott	0:d177c0087d1f	174
guiott	0:d177c0087d1f	175	// while ((delta_lambda > 1e-12) && (remaining_steps > 0)) original by xtimor
guiott	0:d177c0087d1f	176	while (( remaining_steps == 20 ) \|\| ((fabs(delta_lambda) > 1e-12) && (remaining_steps > 0)))
guiott	0:d177c0087d1f	177	{ /* Iterate */
guiott	0:d177c0087d1f	178	/* Variables */
guiott	0:d177c0087d1f	179	double tmp1, tmp2, tan_sigma, sin_alpha, cos_alpha, C, lambda_prev;
guiott	0:d177c0087d1f	180
guiott	0:d177c0087d1f	181	/* Calculation */
guiott	0:d177c0087d1f	182	tmp1 = cos_U2 * sin_lambda;
guiott	0:d177c0087d1f	183	tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda;
guiott	0:d177c0087d1f	184	sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2);
guiott	0:d177c0087d1f	185	cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda;
guiott	0:d177c0087d1f	186	tan_sigma = sin_sigma / cos_sigma;
guiott	0:d177c0087d1f	187	sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma;
guiott	0:d177c0087d1f	188	cos_alpha = cos(asin(sin_alpha));
guiott	0:d177c0087d1f	189	sqr_cos_alpha = cos_alpha * cos_alpha;
guiott	0:d177c0087d1f	190	cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha;
guiott	0:d177c0087d1f	191	sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
guiott	0:d177c0087d1f	192	C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
guiott	0:d177c0087d1f	193	lambda_prev = lambda;
guiott	0:d177c0087d1f	194	sigma = asin(sin_sigma);
guiott	0:d177c0087d1f	195	lambda = L +
guiott	0:d177c0087d1f	196	(1 - C) * f * sin_alpha
guiott	0:d177c0087d1f	197	* (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam)));
guiott	0:d177c0087d1f	198	delta_lambda = lambda_prev - lambda;
guiott	0:d177c0087d1f	199	if ( delta_lambda < 0 ) delta_lambda = -delta_lambda;
guiott	0:d177c0087d1f	200	sin_lambda = sin(lambda);
guiott	0:d177c0087d1f	201	cos_lambda = cos(lambda);
guiott	0:d177c0087d1f	202	remaining_steps--;
guiott	0:d177c0087d1f	203	} /* Iterate */
guiott	0:d177c0087d1f	204
guiott	0:d177c0087d1f	205	/* More calculation */
guiott	0:d177c0087d1f	206	sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b;
guiott	0:d177c0087d1f	207	A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
guiott	0:d177c0087d1f	208	B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
guiott	0:d177c0087d1f	209	delta_sigma = B * sin_sigma * (
guiott	0:d177c0087d1f	210	cos_2_sigmam + B / 4 * (
guiott	0:d177c0087d1f	211	cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) -
guiott	0:d177c0087d1f	212	B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam)
guiott	0:d177c0087d1f	213	));
guiott	0:d177c0087d1f	214
guiott	0:d177c0087d1f	215	/* Calculate result */
guiott	0:d177c0087d1f	216	if ( from_azimuth != 0 )
guiott	0:d177c0087d1f	217	{
guiott	0:d177c0087d1f	218	double tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda);
guiott	0:d177c0087d1f	219	*from_azimuth = atan(tan_alpha_1);
guiott	0:d177c0087d1f	220	}
guiott	0:d177c0087d1f	221	if ( to_azimuth != 0 )
guiott	0:d177c0087d1f	222	{
guiott	0:d177c0087d1f	223	double tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda);
guiott	0:d177c0087d1f	224	*to_azimuth = atan(tan_alpha_2);
guiott	0:d177c0087d1f	225	}
guiott	0:d177c0087d1f	226
guiott	0:d177c0087d1f	227	return b * A * (sigma - delta_sigma);
guiott	0:d177c0087d1f	228	}
guiott	0:d177c0087d1f	229
guiott	0:d177c0087d1f	230	/**
guiott	0:d177c0087d1f	231	* \brief Horizontal move of point position
guiott	0:d177c0087d1f	232	*/
guiott	0:d177c0087d1f	233	int nmea_move_horz(
guiott	0:d177c0087d1f	234	const nmeaPOS start_pos, /< Start position in radians /
guiott	0:d177c0087d1f	235	nmeaPOS end_pos, /< Result position in radians /
guiott	0:d177c0087d1f	236	double azimuth, /*< Azimuth (degree) [0, 359] /
guiott	0:d177c0087d1f	237	double distance /*< Distance (km) /
guiott	0:d177c0087d1f	238	)
guiott	0:d177c0087d1f	239	{
guiott	0:d177c0087d1f	240	nmeaPOS p1 = *start_pos;
guiott	0:d177c0087d1f	241	int RetVal = 1;
guiott	0:d177c0087d1f	242
guiott	0:d177c0087d1f	243	distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */
guiott	0:d177c0087d1f	244	azimuth = nmea_degree2radian(azimuth);
guiott	0:d177c0087d1f	245
guiott	0:d177c0087d1f	246	end_pos->lat = asin(
guiott	0:d177c0087d1f	247	sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth));
guiott	0:d177c0087d1f	248	end_pos->lon = p1.lon + atan2(
guiott	0:d177c0087d1f	249	sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat));
guiott	0:d177c0087d1f	250
guiott	0:d177c0087d1f	251	if(NMEA_POSIX(isnan)(end_pos->lat) \|\| NMEA_POSIX(isnan)(end_pos->lon))
guiott	0:d177c0087d1f	252	{
guiott	0:d177c0087d1f	253	end_pos->lat = 0; end_pos->lon = 0;
guiott	0:d177c0087d1f	254	RetVal = 0;
guiott	0:d177c0087d1f	255	}
guiott	0:d177c0087d1f	256
guiott	0:d177c0087d1f	257	return RetVal;
guiott	0:d177c0087d1f	258	}
guiott	0:d177c0087d1f	259
guiott	0:d177c0087d1f	260	/**
guiott	0:d177c0087d1f	261	* \brief Horizontal move of point position
guiott	0:d177c0087d1f	262	* This function uses an algorithm for an oblate spheroid earth model.
guiott	0:d177c0087d1f	263	* The algorithm is described here:
guiott	0:d177c0087d1f	264	* http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
guiott	0:d177c0087d1f	265	*/
guiott	0:d177c0087d1f	266	int nmea_move_horz_ellipsoid(
guiott	0:d177c0087d1f	267	const nmeaPOS start_pos, /< Start position in radians /
guiott	0:d177c0087d1f	268	nmeaPOS end_pos, /< (O) Result position in radians /
guiott	0:d177c0087d1f	269	double azimuth, /*< Azimuth in radians /
guiott	0:d177c0087d1f	270	double distance, /*< Distance (km) /
guiott	0:d177c0087d1f	271	double end_azimuth /< (O) Azimuth at end position in radians /
guiott	0:d177c0087d1f	272	)
guiott	0:d177c0087d1f	273	{
guiott	0:d177c0087d1f	274	/* Variables */
guiott	0:d177c0087d1f	275	double f, a, b, sqr_a, sqr_b;
guiott	0:d177c0087d1f	276	double phi1, tan_U1, sin_U1, cos_U1, s, alpha1, sin_alpha1, cos_alpha1;
guiott	0:d177c0087d1f	277	double tan_sigma1, sigma1, sin_alpha, cos_alpha, sqr_cos_alpha, sqr_u, A, B;
guiott	0:d177c0087d1f	278	double sigma_initial, sigma, sigma_prev, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, delta_sigma;
guiott	0:d177c0087d1f	279	int remaining_steps;
guiott	0:d177c0087d1f	280	double tmp1, phi2, lambda, C, L;
guiott	0:d177c0087d1f	281
guiott	0:d177c0087d1f	282	/* Check input */
guiott	0:d177c0087d1f	283	NMEA_ASSERT(start_pos != 0);
guiott	0:d177c0087d1f	284	NMEA_ASSERT(end_pos != 0);
guiott	0:d177c0087d1f	285
guiott	0:d177c0087d1f	286	if (fabs(distance) < 1e-12)
guiott	0:d177c0087d1f	287	{ /* No move */
guiott	0:d177c0087d1f	288	end_pos = start_pos;
guiott	0:d177c0087d1f	289	if ( end_azimuth != 0 ) *end_azimuth = azimuth;
guiott	0:d177c0087d1f	290	return ! (NMEA_POSIX(isnan)(end_pos->lat) \|\| NMEA_POSIX(isnan)(end_pos->lon));
guiott	0:d177c0087d1f	291	} /* No move */
guiott	0:d177c0087d1f	292
guiott	0:d177c0087d1f	293	/* Earth geometry */
guiott	0:d177c0087d1f	294	f = NMEA_EARTH_FLATTENING;
guiott	0:d177c0087d1f	295	a = NMEA_EARTH_SEMIMAJORAXIS_M;
guiott	0:d177c0087d1f	296	b = (1 - f) * a;
guiott	0:d177c0087d1f	297	sqr_a = a * a;
guiott	0:d177c0087d1f	298	sqr_b = b * b;
guiott	0:d177c0087d1f	299
guiott	0:d177c0087d1f	300	/* Calculation */
guiott	0:d177c0087d1f	301	phi1 = start_pos->lat;
guiott	0:d177c0087d1f	302	tan_U1 = (1 - f) * tan(phi1);
guiott	0:d177c0087d1f	303	cos_U1 = 1 / sqrt(1 + tan_U1 * tan_U1);
guiott	0:d177c0087d1f	304	sin_U1 = tan_U1 * cos_U1;
guiott	0:d177c0087d1f	305	s = distance;
guiott	0:d177c0087d1f	306	alpha1 = azimuth;
guiott	0:d177c0087d1f	307	sin_alpha1 = sin(alpha1);
guiott	0:d177c0087d1f	308	cos_alpha1 = cos(alpha1);
guiott	0:d177c0087d1f	309	tan_sigma1 = tan_U1 / cos_alpha1;
guiott	0:d177c0087d1f	310	sigma1 = atan2(tan_U1, cos_alpha1);
guiott	0:d177c0087d1f	311	sin_alpha = cos_U1 * sin_alpha1;
guiott	0:d177c0087d1f	312	sqr_cos_alpha = 1 - sin_alpha * sin_alpha;
guiott	0:d177c0087d1f	313	cos_alpha = sqrt(sqr_cos_alpha);
guiott	0:d177c0087d1f	314	sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b;
guiott	0:d177c0087d1f	315	A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
guiott	0:d177c0087d1f	316	B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
guiott	0:d177c0087d1f	317
guiott	0:d177c0087d1f	318	/* Initialize iteration */
guiott	0:d177c0087d1f	319	sigma_initial = s / (b * A);
guiott	0:d177c0087d1f	320	sigma = sigma_initial;
guiott	0:d177c0087d1f	321	sin_sigma = sin(sigma);
guiott	0:d177c0087d1f	322	cos_sigma = cos(sigma);
guiott	0:d177c0087d1f	323	cos_2_sigmam = cos(2 * sigma1 + sigma);
guiott	0:d177c0087d1f	324	sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
guiott	0:d177c0087d1f	325	delta_sigma = 0;
guiott	0:d177c0087d1f	326	sigma_prev = 2 * NMEA_PI;
guiott	0:d177c0087d1f	327	remaining_steps = 20;
guiott	0:d177c0087d1f	328
guiott	0:d177c0087d1f	329	while ((fabs(sigma - sigma_prev) > 1e-12) && (remaining_steps > 0))
guiott	0:d177c0087d1f	330	{ /* Iterate */
guiott	0:d177c0087d1f	331	cos_2_sigmam = cos(2 * sigma1 + sigma);
guiott	0:d177c0087d1f	332	sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
guiott	0:d177c0087d1f	333	sin_sigma = sin(sigma);
guiott	0:d177c0087d1f	334	cos_sigma = cos(sigma);
guiott	0:d177c0087d1f	335	delta_sigma = B * sin_sigma * (
guiott	0:d177c0087d1f	336	cos_2_sigmam + B / 4 * (
guiott	0:d177c0087d1f	337	cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) -
guiott	0:d177c0087d1f	338	B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam)
guiott	0:d177c0087d1f	339	));
guiott	0:d177c0087d1f	340	sigma_prev = sigma;
guiott	0:d177c0087d1f	341	sigma = sigma_initial + delta_sigma;
guiott	0:d177c0087d1f	342	remaining_steps --;
guiott	0:d177c0087d1f	343	} /* Iterate */
guiott	0:d177c0087d1f	344
guiott	0:d177c0087d1f	345	/* Calculate result */
guiott	0:d177c0087d1f	346	tmp1 = (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1);
guiott	0:d177c0087d1f	347	phi2 = atan2(
guiott	0:d177c0087d1f	348	sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1,
guiott	0:d177c0087d1f	349	(1 - f) * sqrt(sin_alpha * sin_alpha + tmp1 * tmp1)
guiott	0:d177c0087d1f	350	);
guiott	0:d177c0087d1f	351	lambda = atan2(
guiott	0:d177c0087d1f	352	sin_sigma * sin_alpha1,
guiott	0:d177c0087d1f	353	cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1
guiott	0:d177c0087d1f	354	);
guiott	0:d177c0087d1f	355	C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
guiott	0:d177c0087d1f	356	L = lambda -
guiott	0:d177c0087d1f	357	(1 - C) * f * sin_alpha * (
guiott	0:d177c0087d1f	358	sigma + C * sin_sigma *
guiott	0:d177c0087d1f	359	(cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))
guiott	0:d177c0087d1f	360	);
guiott	0:d177c0087d1f	361
guiott	0:d177c0087d1f	362	/* Result */
guiott	0:d177c0087d1f	363	end_pos->lon = start_pos->lon + L;
guiott	0:d177c0087d1f	364	end_pos->lat = phi2;
guiott	0:d177c0087d1f	365	if ( end_azimuth != 0 )
guiott	0:d177c0087d1f	366	{
guiott	0:d177c0087d1f	367	*end_azimuth = atan2(
guiott	0:d177c0087d1f	368	sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1
guiott	0:d177c0087d1f	369	);
guiott	0:d177c0087d1f	370	}
guiott	0:d177c0087d1f	371	return ! (NMEA_POSIX(isnan)(end_pos->lat) \|\| NMEA_POSIX(isnan)(end_pos->lon));
guiott	0:d177c0087d1f	372	}
guiott	0:d177c0087d1f	373
guiott	0:d177c0087d1f	374	/**
guiott	0:d177c0087d1f	375	* \brief Convert position from INFO to radians position
guiott	0:d177c0087d1f	376	*/
guiott	0:d177c0087d1f	377	void nmea_info2pos(const nmeaINFO info, nmeaPOS pos)
guiott	0:d177c0087d1f	378	{
guiott	0:d177c0087d1f	379	pos->lat = nmea_ndeg2radian(info->lat);
guiott	0:d177c0087d1f	380	pos->lon = nmea_ndeg2radian(info->lon);
guiott	0:d177c0087d1f	381	}
guiott	0:d177c0087d1f	382
guiott	0:d177c0087d1f	383	/**
guiott	0:d177c0087d1f	384	* \brief Convert radians position to INFOs position
guiott	0:d177c0087d1f	385	*/
guiott	0:d177c0087d1f	386	void nmea_pos2info(const nmeaPOS pos, nmeaINFO info)
guiott	0:d177c0087d1f	387	{
guiott	0:d177c0087d1f	388	info->lat = nmea_radian2ndeg(pos->lat);
guiott	0:d177c0087d1f	389	info->lon = nmea_radian2ndeg(pos->lon);
guiott	0:d177c0087d1f	390	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	29 Jan 2012
Imports:	51
Forks:	0
Commits:	4
Dependents:	0
Dependencies:	5
Followers:	4

nmea/gmath.c@3:a2f9eb3b8a16, 2012-02-03 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning