|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <strings.h>
|
|
#include "trame.h"
|
|
#include <math.h>
|
|
#define rayonter 6367.445
|
|
//Trames de tests ? modifier si n?cessaire.
|
|
char * trames[]= {"$GPGSV,3,2,10,15,03,077,,18,04,041,42,19,85,271,,20,08,214,*7C",
|
|
"$GPGSV,3,3,10,22,39,053,50,28,15,320,*7E",
|
|
"$GPRMC,141914.00,A,4545.6424,N,00306.6036,E,0.4,99.4,010206,,*0C",
|
|
"$GPGLL,4545.6424,N,00306.6036,E,141914.00,A*0E",
|
|
"$GPGGA,141914.00,4545.0000,N,00306.6036,E,1,05,3.4,499.3,M,,M,,*7D",
|
|
"$GPGSA,A,3,,03,,22,14,,01,,18,,,,3.9,3.4,1.9*39",
|
|
"$GPVTG,99.4,T,,M,0.4,N,0.7,K*57",
|
|
"$GPZDA,141914.00,01,02,2006,00,00*69",
|
|
0};
|
|
|
|
typedef struct //Structure contenant la latitude et la longitude
|
|
{
|
|
float latitude;
|
|
|
|
float longitude;
|
|
|
|
}Position ;
|
|
|
|
typedef struct //Structure contenant la position et la vitesse maximale
|
|
{
|
|
Position rpos;
|
|
|
|
float vitmax;
|
|
}Zone;
|
|
|
|
Zone zones[]={
|
|
{{44.7887762, -3.012}, 50},
|
|
{{44.7891220, -3.013}, 70},
|
|
{{45.3456220, -3.045}, 80},
|
|
{{46.7350220, -3.023}, 90},
|
|
};
|
|
|
|
int trame_cmp(char* trame,char *type)
|
|
/*fonction qui renvoie 1 si la trame commence
|
|
par la cha?ne de caract?re type et z?ro dans les autres cas*/
|
|
{
|
|
int i, ok =1 ;
|
|
for(i=0 ;i<=4 ;i=i+1)
|
|
{
|
|
if (trame[i+1] !=type[i])
|
|
/*comparaison de la premi?re chaine (sans le '$')
|
|
et de la deuxi?me chaine pass?es en param?tre*/
|
|
{
|
|
ok=0;
|
|
/*si des caract?res diff?rent lors de la comparaison ok=0
|
|
et on en conclut que les deux chaines compar?es ne sont pas pareils*/
|
|
}
|
|
}
|
|
return ok;
|
|
}
|
|
|
|
int decode_int(char car)
|
|
{ //qui renvoie la valeur d?cimale associ?e ? un caract?re donn? en param?tre
|
|
int entier;
|
|
if(car<='9' && car>='0')
|
|
{
|
|
entier= car-48; //on soustrait au code ascii du caract?re le code ascii du caract?re '0'
|
|
}
|
|
else
|
|
{
|
|
entier=-1; // si le caractere entr? ne correspond pas ? un chiffre on renvoie -1
|
|
}
|
|
return entier;
|
|
}
|
|
|
|
|
|
|
|
int decode_nombre (char *ch,int n)
|
|
/*fonction qui renvoie qui renvoie la valeur d?cimale
|
|
des n premiers caract?res de la cha?ne ch*/
|
|
{
|
|
int i,s=0;
|
|
for(i=0;i<n;i++)
|
|
{
|
|
s=s + decode_int(ch[i])*pow(10,n-1-i);
|
|
}
|
|
return s;
|
|
}
|
|
|
|
|
|
float conv_lat_flottant(char * ch)
|
|
// fonction qui convertit les latitudes en flottant
|
|
{
|
|
float latitude;
|
|
|
|
int val_degree,valEnt_min,valDec_min;
|
|
|
|
val_degree=decode_nombre(ch,2); //Recuperation de la partie entiere correspondant aux degrees
|
|
|
|
valEnt_min=decode_nombre(&ch[2],2);//Recuperation de la partie entiere des minututes
|
|
|
|
valDec_min=decode_nombre(&ch[5],4); // recuperation de la partie decimale des minututes
|
|
|
|
latitude=val_degree + ((valEnt_min+valDec_min*0.0001)/60); // conversion en flottant
|
|
|
|
return latitude;
|
|
}
|
|
|
|
|
|
float conv_long_flottant(char * ch)
|
|
/*fonction convertissant la cha?ne de caract?re contenant
|
|
la longitude en nombre flottant et son test unitaire.*/
|
|
{
|
|
float longitude;
|
|
int val_degree,valEnt_min,valDec_min;
|
|
|
|
val_degree=decode_nombre(ch,3); /* Recuperation de la partie
|
|
entiere correspondant aux degrees*/
|
|
valEnt_min=decode_nombre(ch+3,2); //Recuperation de la partie entiere des minututes
|
|
|
|
valDec_min=decode_nombre(ch+6,4); //Recuperation de la partie decimale des minututes
|
|
|
|
longitude=val_degree + ((valEnt_min+valDec_min*0.0001)/60); //conversion en flottant
|
|
|
|
return longitude;
|
|
}
|
|
|
|
|
|
float conv_sexa_flottant(char * ch)
|
|
// Qui d?code indiff?remment une latitude ou une longitude.
|
|
{
|
|
float coordonnee;
|
|
|
|
if (ch[4]!='.')
|
|
// si la chaine contient 5 caract?res avant le point on consid?re que c'est une longitude
|
|
{
|
|
coordonnee=conv_long_flottant(ch);
|
|
}
|
|
else
|
|
// sinon on consid?re que c'est une latitude
|
|
{
|
|
coordonnee=conv_lat_flottant(ch);
|
|
}
|
|
return coordonnee;
|
|
}
|
|
|
|
|
|
int decode_trame(char* t, Position* p){
|
|
int a,i; //a=1 si la trame est valide ; a=0 si la trame n'est pas valide
|
|
|
|
char copie_lat[9]; //chaine dans laquelle on copiera la latitude
|
|
|
|
char copie_long[10]; //chaine dans laquelle on copiera la longitude
|
|
|
|
a=trame_cmp(t,"GPGGA"); //comparaison pour voir si la trame est valide
|
|
|
|
if(a==1)
|
|
{
|
|
for(i=0;i<9;++i)
|
|
{
|
|
copie_lat[i]=t[17+i]; //on extrait la latitude
|
|
}
|
|
|
|
(*p).latitude= conv_lat_flottant(copie_lat); //on convertit la laatitude en flottant
|
|
|
|
for(i=0;i<10;++i)
|
|
{
|
|
copie_long[i]=t[29+i]; //on extrait la longitude
|
|
}
|
|
(*p).longitude=conv_long_flottant(copie_long); //on convertit la longitude en flottant
|
|
}
|
|
return a;
|
|
}
|
|
|
|
float calcul_distance(Position pos, Position pos_prec){
|
|
float distance,A1,A2,B1,B2,C1;
|
|
A1=pos_prec.latitude;//Latitude du premier point
|
|
|
|
A2=pos.latitude;//Latitude du second point
|
|
|
|
B1=pos_prec.longitude;//Longitude du premier point
|
|
|
|
B2=pos_prec.longitude;//Longitude du second point
|
|
|
|
C1=B1-B2;//Difference entre les 2 longitudes
|
|
|
|
distance=(acosf(sinf(A1)*sinf(A2)+cosf(B1)*cosf(B2)*cosf(C1))*rayonter);
|
|
|
|
return distance; //on renvoie la distance en km
|
|
|
|
}
|
|
|
|
float calcul_vitesse(Position pos, Position pos_prec)
|
|
{
|
|
float vitesse,distance;
|
|
|
|
distance=calcul_distance(pos,pos_prec);
|
|
|
|
vitesse=distance*3600;
|
|
/*le temps ?tant de 1s, on a directement la vitessse en m/s ,
|
|
on la convertit en km/h en multipliant par 3,6*/
|
|
|
|
return vitesse;
|
|
|
|
}
|
|
|
|
int distance_a_la_plus_proche_zone(Position p,Zone r[],int nb_zones,float *d)
|
|
{
|
|
float d1;
|
|
|
|
int a=0,i;
|
|
|
|
*d=calcul_distance(p, r[0].rpos);
|
|
/*on calcule la distance entre la position actuelle
|
|
et la premiere zone dangereuse du tableau*/
|
|
if(nb_zones==0)
|
|
a=-1;
|
|
for(i=1;i<nb_zones;i++)
|
|
{
|
|
d1=calcul_distance(p, r[i].rpos);
|
|
/*a chaque it?ration on calcule la distance entre la position actuelle
|
|
et une zone dangeureuse*/
|
|
if(d1<(*d))
|
|
/*si cette distance est inf?rieure ? la distance d calcul?e
|
|
,on remplace d par cette nouvelle distance*/
|
|
|
|
{
|
|
(*d)=d1;
|
|
a=i; // a prend la valeur de l'indice correspondant ? cette zone dangereuse
|
|
}
|
|
}
|
|
|
|
return a; // on revoie l'indice de la zone dangereuse la plus proche
|
|
}
|
|
|
|
//Fonction ? modifier !!!!!
|
|
void traitement(char * trame)
|
|
{
|
|
static int cpt=0;
|
|
|
|
cpt++;
|
|
|
|
float d,vitesse;//d=distance
|
|
|
|
int a,b;
|
|
|
|
int nb_zones=4;
|
|
|
|
Position p;
|
|
|
|
b=decode_trame(trame,&p);
|
|
|
|
if(b==1)
|
|
// si la trame est valide
|
|
{
|
|
printf("%s\n", trame); // on affiche juste pour v?rifier que la trame est valide
|
|
|
|
a=distance_a_la_plus_proche_zone(p,zones,nb_zones,&d);
|
|
/*on recupere l'indice de la zone dangereuse et
|
|
la distance entre la zone et la position actuelle*/
|
|
|
|
vitesse=calcul_vitesse(p,zones[a].rpos);
|
|
// on calcule la vitesse
|
|
|
|
if(d>680 && vitesse>2464890)
|
|
// 680=seuil de la distance et 2464890=seuil dela vitesse
|
|
{
|
|
printf("alarme=on\n"); // si les conditions sont respect?es alarme=on
|
|
|
|
printf("distance: %f ", d);
|
|
|
|
printf("vitesse;%f\n",vitesse);
|
|
}
|
|
else
|
|
{
|
|
printf("alarme=off\n"); // sinon alarme=off
|
|
}
|
|
}
|
|
}
|
|
|
|
//Ajouter vos tests unitaires dans cette fonction.
|
|
void tests_unitaires(void){
|
|
Position a={0.0,0.0};
|
|
// on initialise 2 struxtrures afin d'effecuer les test unitaires de decode_trame
|
|
Position b={0.0,0.0};
|
|
|
|
Position p1={45.750000,3.11006};
|
|
|
|
Position p3={45.750403,3.11007};
|
|
|
|
Position* p;
|
|
|
|
Position* p2; // pour tester decode_trame
|
|
|
|
p=&a; // on affecte ? chache pointeur l'adresse d'une stru
|
|
|
|
p2=&b;
|
|
|
|
float d;
|
|
|
|
int nb_zones=4;
|
|
|
|
if (trame_cmp("$GPGGA suite chaine","GPGGA")!=1)
|
|
{
|
|
printf ("Erreur Test unitaire trame_cmp.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
if (trame_cmp("$GPRMC suite chaine","GPGGA")!=0)
|
|
{
|
|
printf ("Erreur Test unitaire trame_cmp.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
if (trame_cmp("$GPRMC... ", "GPRMC" )!=1)
|
|
{
|
|
printf ("Erreur Test unitaire trame_cmp.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
|
|
if (trame_cmp("$APRMC...", "GPGGA")!=0)
|
|
{
|
|
printf ("Erreur Test unitaire trame_cmp.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
if (trame_cmp("$GPGGA suite chaine","GPGGA")!=1)
|
|
{
|
|
printf ("Erreur Test unitaire trame_cmp.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
if(decode_int('0')!=0)
|
|
//tests decode_int
|
|
{
|
|
printf ("Erreur Test unitaire decode_int.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
if(decode_int('A')!=-1){
|
|
printf ("Erreur Test unitaire decode_int.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
if(decode_nombre("5145",2)!=51)
|
|
//test decode_nombre
|
|
{
|
|
printf ("Erreur Test unitaire decode_nombre.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
if(decode_nombre("1234",2)!=12)
|
|
{
|
|
printf ("Erreur Test unitaire decode_nombre.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
if(conv_lat_flottant("3723.2475")-37.387458>0.0001)
|
|
/*test de la fonction de conversion d'une latitude en flottant/
|
|
prise en compte de l'erreur sur la pr?cision*/
|
|
{
|
|
printf ("Erreur Test unitaire conv_lat_flottant.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
if(conv_lat_flottant("4557.5679")-45.959465>0.0001)
|
|
// prise en compte de l'erreur sur la pr?cision
|
|
{
|
|
printf ("Erreur Test unitaire conv_lat_flottant.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
if(conv_long_flottant("00306.6036")-3.11006>0.0001)
|
|
/*test de la fonction de conversion d'une longitude en flottant/
|
|
prise en compte de l'erreur sur la pr?cision*/
|
|
{
|
|
printf ("Erreur Test unitaire conv_long_flottant.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
if(conv_long_flottant("00405.3567")-4.089227>0.0001)
|
|
// prise en compte de l'erreur sur la pr?cision
|
|
{
|
|
printf ("Erreur Test unitaire conv_long_flottant.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
if(conv_sexa_flottant("3723.2475")-37.387458>0.0001)
|
|
/*test de la fonction de conversion d'une coordonn?e sexag?simale en flottant
|
|
/prise en compte de l'erreur sur la pr?cision*/
|
|
{
|
|
printf ("Erreur Test unitaire conv_sexa_flottant.\n");
|
|
exit(-1);
|
|
}
|
|
if(conv_sexa_flottant("00306.6036")-3.11006>0.0001)
|
|
// prise en compte de l'erreur sur la pr?cision
|
|
{
|
|
printf ("Erreur Test unitaire conv_sexa_flottant.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
if(decode_trame("$GPGGA,141915.00,4545.0242,N,00306.6039,E,1,05,3.4,499.5,M,,M,,*72",p2)==0)
|
|
// test pour voir si la comparaison marche(trame valide ou non)
|
|
{
|
|
printf ("Erreur Test unitaire decode_trame.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
decode_trame("$GPGGA,141915.00,3723.2475,N,00306.6036,E,1,05,3.4,499.5,M,,M,,*72",p);
|
|
if((*p).longitude-3.11006>0.0001)
|
|
//test pour voir si le calcul de la longitude extraite de la trame est bon
|
|
{
|
|
printf ("Erreur Test unitaire decode_trame.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
if((*p).latitude-37.387458>0.0001)
|
|
//test pour voir si le calcul de la latitude extraite de la trame est bon
|
|
{
|
|
printf ("Erreur Test unitaire decode_trame.\n");
|
|
|
|
exit(-1);
|
|
|
|
}
|
|
if(calcul_distance(p1,p3)-0.044812>0.001)
|
|
//test calcul_distance (prise en com^te de l'erreur d'1m)
|
|
{
|
|
printf ("Erreur Test unitaire calcul_distance.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
if(calcul_vitesse(p1,p3)-1613.232>0.01)
|
|
//test calcul_vitesse (prise en compte de l'erreur)
|
|
{
|
|
printf ("Erreur Test unitaire calcul_vitesse.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
if(distance_a_la_plus_proche_zone(p1,zones,nb_zones,&d)!=2)
|
|
//test distance_a_la_plus_proche (prise en compte de l'erreur)
|
|
{
|
|
printf ("Erreur Test unitaire calcul_distance_plus_proche.\n");
|
|
|
|
exit(-1);
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
// Ne pas modifier cette fonction
|
|
int main(int argc,char ** argv)
|
|
{
|
|
tests_unitaires();
|
|
|
|
// Affichage des trames definies dans la table trames.
|
|
printf ("Trames de tests tableau trames:\n");
|
|
int i=0;
|
|
while (trames[i])
|
|
traitement(trames[i++]);
|
|
|
|
if (!trame_init())
|
|
exit(-1);
|
|
// Affichage des trames du fichier gps.log
|
|
char *trame;
|
|
printf ("Trames de tests du fichier gps.log\n");
|
|
while ((trame = trame_suivante()))
|
|
traitement(trame);
|
|
|
|
|
|
return 0;
|
|
}
|