Читайте початок в статті 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); }