GPS. Расчет дистанции между двумя точками по GPS координатам. Расчет курса на точку.

Читайте начало в статье GPS модуль EB-500 и ATMega

При использовании GPS модуля появилась необходимость вычислить расстояние от текущего положения до заданной точки. Фактически это вычисление расстояния по двум GPS координатам. Поскольку, в этом вопросе у меня было недостаточно знаний, пришлось немного почитать. Рекомендую для прочтения эти статьи: Системы геодезических координат или "Что такое датум?"

Вычисление постоянного азимута и длины линии румба между двумя точками для геодезических координат Сравнение расчетов длин и азимутов для разных способов вычисления

Даже если Вы не будете глубоко вникать в суть этих статей, это поможет Вам осознать суть некоторых проблем и получить ответы на большинство вопросов, связанных с точностью вычислений. В одной из этих статей приводится алгоритм, который и был адаптирован для библиотеки gps.c.

При вычислении применяются упрощения. Предполагается, что точки находятся на сфере с радиусом 6372795 метров. Следует понимать, что если точки находятся на разных высотах, то вычисленное расстояние будет отличаться от реального, поскольку разница высот не учитывается.

Кроме того, форма нашей планеты далека от формы идеальной сферы, а мы допускаем, что точки лежат именно на сфере, а не на эллипсоиде или геоиде, что ближе к истине. Но для моей задачи такие упрощения и связанные с ними погрешности вполне допустимы. Если Вам требуется вычислять расстояние с высокой точностью, то такой метод может Вам не подойти.

Рассмотрим функции расчета дистанции и курса. В первую очередь обращаем внимание на строку:

#define EATH_RADIUS 6372795

Здесь указывается радиус Земли в метрах. Структура GPS_Point должна содержать GPS координаты точки. Текущие координаты берутся из структуры GPS (см. gps.c, gps.h).

Функция gps_convert_to_rad используется для пересчета координат в радианы.

Функция gps_distance вычисляет дистанцию между GPS и GPS_Point в километрах. Если надо в метрах замените

return (atan2(y,x) * EATH_RADIUS)/1000;

на

return (atan2(y,x) * EATH_RADIUS);

Функция gps_angle вычисляет курс на точку GPS_Point с текущей точки GPS в градусах.

#define EATH_RADIUS 6372795

// GPS Poit
typedef struct {
	unsigned long	latitude;		// Долгота
	char			latitude_c;		// Долгота литера
	unsigned long	longitude; 		// Широта
	char			longitude_c; 	// Широта литера
} tpGPG_Point;

tpGPG_Point GPS_Point;

//-----------------------------------------
double gps_convert_to_rad (unsigned long int GPS_DATA, char c) {
	double rad;

	rad = (double)gps_convert_to_grad(GPS_DATA)* M_PI/1000000/180;
	if ((c==`S`) | (c==`W`))
		rad = -1*rad;
	return rad;
}
//-----------------------------------------

double gps_distance() {
	double lat1_cos, lat2_cos, lat1_sin, lat2_sin;
	double lat1, long1, lat2, long2;
	double sin_delta_long, cos_delta_long;
	double y, x;

	lat1 = gps_convert_to_rad(GPS.latitude, GPS.latitude_c);
	long1 = gps_convert_to_rad(GPS.longitude, GPS.longitude_c);

	lat2 = gps_convert_to_rad(GPS_Point.latitude, GPS_Point.latitude_c); // Координаты точки
	long2 = gps_convert_to_rad(GPS_Point.longitude, GPS_Point.longitude_c);

	lat1_cos = cos(lat1);
	lat2_cos = cos(lat2);
	lat1_sin = sin(lat1);
	lat2_sin = sin(lat2);

	sin_delta_long = sin(long2-long1);
	cos_delta_long = cos(long2-long1);

	y=sqrt(pow(lat2_cos*sin_delta_long, 2)+pow(lat1_cos*lat2_sin-lat1_sin*lat2_cos*cos_delta_long,2));
	x=lat1_sin*lat2_sin+lat1_cos*lat2_cos*cos_delta_long;

	return (atan2(y,x) * EATH_RADIUS)/1000;
}

double gps_angle() {
	double lat1, long1, lat2, long2;
	double dlon_W, dlon_E, dphi, atn2, dlon, tc;
	int sign;

	lat1 = gps_convert_to_rad(GPS.latitude, GPS.latitude_c);
	long1 = gps_convert_to_rad(GPS.longitude, GPS.longitude_c);

	lat2 = gps_convert_to_rad(GPS_Point.latitude, GPS_Point.latitude_c); // Координаты точки
	long2 = gps_convert_to_rad(GPS_Point.longitude, GPS_Point.longitude_c);

	dlon_W = (long2 - long1) - (2*M_PI*(floor((long2 - long1)/(2*M_PI))));
	dlon_E = (long1 - long2) - (2*M_PI*(floor((long1 - long2)/(2*M_PI))));
	dphi = log((tan((lat2/2) + (M_PI/4)))/(tan((lat1/2) + (M_PI/4))));

	if (dlon_W < dlon_E) {
 		dlon_W = -1*dlon_W;

 		//get sign
 		if (dlon_W >= 0)
			sign = 1;
		else
			sign = -1;

		if (abs(dlon_W) >= abs(dphi)) {
			atn2 = (sign * M_PI/2) - atan(dphi / dlon_W);
		}
		else {
			if (dphi > 0) {
				atn2 = atan(dlon_W / dphi);
			}
			else {
				if ((-1*dlon_W) >= 0) {
					atn2 = M_PI + atan(dlon_W / dphi);
				}
				else {
					atn2 = (-1*M_PI) + atan(dlon_W / dphi);
				}
			}
		}
	}
	else {
		//get sign
		if (dlon_W >= 0)
			sign = 1;
		else
			sign = -1;

		if (abs(dlon_E) >= abs(dphi)) {
			if (dlon_E > 0)
				atn2 = sign * M_PI/2 - atan(dphi / (dlon_E));
			else
				atn2 = 0;
		}
		else {
			if (dphi > 0) {
				atn2 = atan((dlon_E) / dphi);
			}
			else {
				if ((dlon_E) >= 0) {
					atn2 = M_PI + atan((dlon_E) / dphi);
				}
				else {
					atn2 = (-1*M_PI) + atan((dlon_E) / dphi);
				}
			}
		}
		dlon = dlon_E;
	}

	tc = atn2 - (2*M_PI*(floor((atn2)/(2*M_PI))));
	return 360-((tc*180)/M_PI);
}