Jump to content

    
Sign in to follow this  
Newegor

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

Recommended Posts

Есть задача определения пробега авто по координатам получаемым с 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

Join the conversation

You can post now and register later. If you have an account, sign in now to post with your account.

Guest
Reply to this topic...

×   Pasted as rich text.   Paste as plain text instead

  Only 75 emoji are allowed.

×   Your link has been automatically embedded.   Display as a link instead

×   Your previous content has been restored.   Clear editor

×   You cannot paste images directly. Upload or insert images from URL.

Sign in to follow this