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);
}
Спасибо за попытку помочь... но проверить бы сначала код, прежде чем выкладывать. M_PI - дефайна нет в функции gps_convert_to_rad вызов gps_convert_to_grad, а где функция? GPSDATA - что за структура P.S. так обрадовался, что есть готовый код... но, все равно, спасибо
Дякую за коментар. Але перед тим яки писати коментар подивіться уважно, може Ви не помітили посилання на архів з файлами gps.c, gps.h та іншими файлами з усім кодом...
Вибачаюсь, таки не побачив... буду гризти. Слава Україні!
Бажаю успіхів!
Недавні записи
- Датчики Холла 120/60 градусів
- Модуль драйверів напівмосту IGBT транзисторів
- Драйвер IGBT транзисторів на A316J
- AS5600. Варіант встановлення на BLDC мотор
- DC-DC для IGBT драйверів ізольований 2 W +15 -8 вольт
- U-FOC - Векторне керування безколекторними моторами
- FOC - своя реалізація векторного керування. Підбиваю підсумки 2022 року
- Конструктор регуляторів моторів. Підбиваю підсумки 2022 року.
- Чому трифазні мотори стали такими популярними?
- FOC & Polar coordinates
Tags
bldc atmega uart flash usart smd bkp lcd exti pmsm python timer 3d-printer avr java-script nodemcu bme280 displays hih-4000 stm32 barometer eeprom servo sms piezo options brushless web foc raspberry-pi mongodb mpu-6050 docker programmator ssd1331 solar soldering rtc eb-500 gpio max1674 html flask esp8266 tim ngnix wifi css gps watchdog encoder adc meteo i2c sensors capture battery dht11 mpx4115a motor bmp280 dma books ethernet led examples rs-232 bluetooth st-link remap nvic websocket ssd1306 usb pwm rfid git mpu-9250 dc-dc
Архіви