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);
}
Додати коментар
Недавні записи
- 🇺🇦 FOC Board STM32F103RB 🧩
- STM32 Motor control SDK - керування оборотами мотора за допомогою потенціометра 📑
- Flask✙Gunicorn✙Nginx➭😎
- STM32 Motor control SDK - програмне керування обертам мотора
- STM32 Motor control SDK - як створити перший проект
- Vue SVG. Приклад побудови живого параметричного креслення
- Вимірювання моменту мотора
- Vue SVG - компонент. Приклад 📑
- Flask + Vue 🏁 Финальный пример 🏁
- Flask, CORS, JSON-файл. Пример#6
Tags
bldc brushless stm32 motor web html css flask atmega foc git java-script pmsm raspberry-pi python websocket mongodb esp8266 nodemcu st-link tim timer docker ngnix programmator ssd1331 ssd1306 wifi uart meteo bme280 bmp280 i2c gps mpu-6050 mpu-9250 sensors 3d-printer options usb barometer remap watchdog flash eeprom rtc bkp encoder pwm servo capture examples dma adc nvic usart gpio books battery dc-dc sms max1674 avr lcd dht11 piezo rs-232 rfid solar exti bluetooth eb-500 displays ethernet led smd soldering mpx4115a hih-4000
Архіви