|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <math.h>
|
|
|
|
struct position {
|
|
int latitude;
|
|
int longitude;
|
|
};
|
|
|
|
void decode_trame(char trame[1000], int* latitude, int* longitude);
|
|
|
|
float calcule_distance(struct position p_1,struct position p_2);
|
|
|
|
float calcule_vitesse(struct position p_1,struct position p_2);
|
|
|
|
void decode_trame(char trame[1000], int* latitude, int* longitude){
|
|
int i;
|
|
|
|
for(i = 17; i < 21;i++){
|
|
*latitude = *latitude*10 + (trame[i]-48);
|
|
}
|
|
|
|
for(i = 22; i < 26;i++){
|
|
*latitude = *latitude*10 + (trame[i]-48);
|
|
|
|
}
|
|
for(i = 29; i < 34;i++){
|
|
*longitude = *longitude*10 + (trame[i]-48);
|
|
|
|
}
|
|
for(i = 35; i < 39;i++){
|
|
*longitude = *longitude*10 + (trame[i]-48);
|
|
}
|
|
*latitude = *latitude - *latitude%1000000 + *latitude%1000000 * 5/3;
|
|
*longitude = *longitude - *longitude%1000000 + *longitude%1000000 * 5/3;
|
|
}
|
|
|
|
float calcule_distance(struct position p_1, struct position p_2){
|
|
float distance;
|
|
float lat1 = p_1.latitude;
|
|
float lat2 = p_2.latitude;
|
|
|
|
float lon1 = p_1.longitude;
|
|
float lon2 = p_2.longitude;
|
|
|
|
|
|
lat1 = lat1/1000000;
|
|
lat2 = lat2/1000000;
|
|
lon1 = lon1/1000000;
|
|
lon2 = lon2/1000000;
|
|
|
|
printf("LAT1 = %f\n", lat1);
|
|
printf("LON1 = %f\n", lon1);
|
|
printf("LAT2 = %f\n", lat2);
|
|
printf("LON2 = %f\n", lon2);
|
|
|
|
|
|
distance = acos(sin(lat1*M_PI/180)*sin(lat2*M_PI/180)+cos(lat1*M_PI/180)*cos(lat2*M_PI/180)*cos(lon1*M_PI/180-lon2*M_PI/180))*6371;
|
|
printf("D = %f\n", distance);
|
|
return distance;
|
|
|
|
}
|
|
|
|
float calcule_vitesse(struct position p_1, struct position p_2){
|
|
float distance;
|
|
float vitesse;
|
|
float lat1 = p_1.latitude;
|
|
float lat2 = p_2.latitude;
|
|
|
|
float lon1 = p_1.longitude;
|
|
float lon2 = p_2.longitude;
|
|
|
|
|
|
lat1 = lat1/1000000;
|
|
lat2 = lat2/1000000;
|
|
lon1 = lon1/1000000;
|
|
lon2 = lon2/1000000;
|
|
|
|
|
|
distance = acos(sin(lat1*M_PI/180)*sin(lat2*M_PI/180)+cos(lat1*M_PI/180)*cos(lat2*M_PI/180)*cos(lon1*M_PI/180-lon2*M_PI/180))*6371;
|
|
vitesse = distance * 3600;
|
|
printf("V = %f\n", vitesse);
|
|
return vitesse;
|
|
|
|
}
|
|
|
|
int main()
|
|
{
|
|
char trame1[] = {"$GPGGA,141914.00,4545.0000,N,00306.6036,E,1,05,3.4,499.3,M,,M,,*7D"};
|
|
char trame2[] = {"$GPGGA,141915.00,4545.0242,N,00306.6039,E,1,05,3.4,499.5,M,,M,,*72"};
|
|
|
|
|
|
|
|
struct position p_1,p_2;
|
|
|
|
p_1.longitude = 0;
|
|
p_1.latitude = 0;
|
|
|
|
p_2.longitude = 0;
|
|
p_2.latitude = 0;
|
|
|
|
|
|
decode_trame(trame1, &p_1.latitude, &p_1.longitude);
|
|
decode_trame(trame2, &p_2.latitude, &p_2.longitude);
|
|
|
|
calcule_distance(p_1, p_2);
|
|
calcule_vitesse(p_1, p_2);
|
|
|
|
|
|
printf("%d\n", p_1.latitude);
|
|
printf("%d\n", p_1.longitude);
|
|
|
|
printf("%d\n", p_2.latitude);
|
|
printf("%d\n", p_2.longitude);
|
|
|
|
return 0;
|
|
}
|