Jump to content

    

Определение расстояние по координатам GPS

Есть задача определения пробега авто по координатам получаемым с GPS модуля по протоколу NMEA. Все работает, вот только осталось сделать подсчет расстояния. Какие есть алгоритмы. Сейчас считаю очень просто по теореме Пифагора, но это для маленьких расстояний подходит, плюс еще не определился как исключить погрешность определения координат, когда авто стоит. Они прыгают иногда и на десятки метров, а авто в это время стоит. В общем поиском по инету ничего путного не нашел (может и плохо искал). Если есть какие-нибудь стандартные методики, то буду благодарен за них.

Share this post


Link to post
Share on other sites

да ... 3й класс.. вторая четверть ...

 

 

/********************************************************************************

***

* Рассчитываем дистанцию (между точками 1 и 2) и отклонение *

*(линия 1-2 относительно севера). navcalc() *

********************************************************************************

****/

#define RAD_CONVERT 57.300000000

#define KMETERS_CONVERT 3437.7387000*0.62500000*4.96728972 //коэффициент преобразования в километры

void navcalc(void)

{

long double t1,t2,t3,t4,t5;

long double Lat1,Lon1,Lat2,Lon2;

long double rad_dist,dist, rad_bearing,bearing;

/*****************************

* Начальные переменные в ноль*

******************************/

t1 = t2 = t3 = t4 = t5 = dist = bearing = 0.0000000;

/************************************************************

* Задаем коориданаты двух точек (тест)

*************************************************************/

//1063 meters 30,5град истинный угол

Lat2=59.42811; //Эрмитаж

Lon2=30.24195;

Lat1=59.42318; // собор

Lon1=30.23625;

/**********************************

* Преобразуем координаты в радианы*

***********************************/

Lat1 = Lat1 / RAD_CONVERT;

Lon1 = Lon1 / RAD_CONVERT;

Lat2 = Lat2 / RAD_CONVERT;

Lon2 = Lon2 / RAD_CONVERT;

t1 = sin(Lat1) * sin(Lat2);

t2 = cos(Lat1) * cos(Lat2);

t3 = cos(Lon1 - Lon2);

t4 = t2 * t3;

t5 = t1 + t4;

rad_dist = atan(-t5 / sqrt(-t5 * t5 + 1.0000))+2.0000* atan(1.0000);

/*****************************************

* Преобразуем в километры (метры) *

******************************************/

dist = rad_dist * KMETERS_CONVERT;

// send_decimal_uart(dist*1000); putchar(' '); //метры

send_decimal_uart(dist); putchar(' '); //километры

/**************************************************************************

* Рассчитываем отклонение

* b = acos(sin(Lat2) - sin(Lat1)* cos(rad_dist)/sin(rad_dist) * cos(lat1));

**************************************************************************/

if(sin(Lon2 - Lon1) < 0.0000)

{

t1 = sin(Lat2) - sin(Lat1) * cos(rad_dist);

t2 = cos(Lat1) * sin(rad_dist);

t3 = t1 / t2;

t4 = atan(-t3 / sqrt(-t3 * t3 + 1.0000)) + 2.0000 * atan(1.000);

rad_bearing = t4;

}

else

{

t1 = sin(Lat2) - sin(Lat1) * cos(rad_dist);

t2 = cos(Lat1) * sin(rad_dist);

t3 = t1 / t2;

t4 = -t3 * t3 + 1.0000;

t5 = 2.0000 * 3.1415926535 - (atan(-t3 / sqrt(-t3 * t3 + 1.0000)) + 2.0000 * atan(1.0000));

rad_bearing = t5;

}

/***********************************

* Преобразуем радианы в градусы *

************************************/

bearing = rad_bearing * RAD_CONVERT;

// Отклонение в градусах

send_decimal_uart(bearing);putchar(' ');

return;

}

Share this post


Link to post
Share on other sites

//v - скорость из VTG, узел = 1852 м/час

S += v*dt

if(v<V0) v = 0; // V0 поставьте, скажем 0.2 м/с

Share this post


Link to post
Share on other sites

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now
Sign in to follow this