root/branch/GUIFO/sp4a12/main.c @ 671
| 1 | jalaffon | #include <stdio.h>
|
|
#include <stdlib.h>
|
|||
#include <strings.h>
|
|||
| 272 | jcguifodjo | #include "trame.h"
|
|
| 522 | jcguifodjo | #include "math.h"
|
|
| 671 | jcguifodjo | #define k 111.120 //Constante a utiliser pour calculer la distance en utilisant la m?thode de Pythagore
|
|
| 272 | jcguifodjo | #define N 200
|
|
| 671 | jcguifodjo | #define rads 0.01745329252 //rads=pie/180 puisque le langage C utilise les angles en radians
|
|
| 1 | jalaffon | ||
//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",
|
|||
| 671 | jcguifodjo | ||
| 632 | jcguifodjo | 0};
|
|
| 671 | jcguifodjo | ||
| 632 | jcguifodjo | ||
| 272 | jcguifodjo | typedef struct {
|
|
float latitude;
|
|||
float longitude;
|
|||
} Position ;
|
|||
| 643 | jcguifodjo | ||
| 671 | jcguifodjo | ||
| 522 | jcguifodjo | //Comparaison des trames pour savoir si la trame est du bon type
|
|
| 114 | jcguifodjo | int trame_cmp(char* trame,char* type)
|
|
{
|
|||
int i=0,d=1;
|
|||
while(type[i]!='\0')
|
|||
{
|
|||
if(trame[i+1]!=type[i] )
|
|||
{
|
|||
d=0;
|
|||
}
|
|||
i++;
|
|||
}
|
|||
return d;
|
|||
| 3 | jalaffon | }
|
|
| 671 | jcguifodjo | ||
| 522 | jcguifodjo | //Conversion d'un caract?re en decimal
|
|
| 272 | jcguifodjo | int decode_int(char c)
|
|
{
|
|||
int d;
|
|||
if(c>='0'&& c<='9')
|
|||
{
|
|||
d=c-48;
|
|||
}
|
|||
else
|
|||
{
|
|||
d=-1;
|
|||
}
|
|||
return d;
|
|||
}
|
|||
| 671 | jcguifodjo | ||
| 522 | jcguifodjo | //Renvoi la valeur decimal des n premiers caract?res entr?e en param?tre
|
|
int decode_nombre(char *c, int n)
|
|||
| 272 | jcguifodjo | {
|
|
| 522 | jcguifodjo | int i,valeur=0;
|
|
for(i=0;i<n;i++){
|
|||
valeur+=decode_int(c[i])*pow(10,n-1-i);
|
|||
}
|
|||
| 632 | jcguifodjo | return valeur;
|
|
| 272 | jcguifodjo | }
|
|
| 671 | jcguifodjo | ||
| 522 | jcguifodjo | //Conversion de la latitude et longitude en degree floatant
|
|
float conversion(char *c)
|
|||
| 272 | jcguifodjo | {
|
|
| 651 | jcguifodjo | int i=0;
|
|
| 522 | jcguifodjo | float deg_lat,deg_long;
|
|
| 671 | jcguifodjo | ||
| 522 | jcguifodjo | while(c[i]!='\0'){
|
|
i++;
|
|||
| 272 | jcguifodjo | }
|
|
| 651 | jcguifodjo | if(i==9){
|
|
| 522 | jcguifodjo | deg_lat=decode_nombre(c,2);
|
|
| 651 | jcguifodjo | ||
deg_lat+=(c[2]-'0')*0.16666666666666;
|
|||
deg_lat+=(c[3]-'0')*0.016666666666666;
|
|||
| 522 | jcguifodjo | for(i=5;i<9;i++){
|
|
deg_lat+=((c[i]-'0')*pow(10,3-i))/6;
|
|||
}
|
|||
return deg_lat;
|
|||
| 272 | jcguifodjo | }
|
|
| 651 | jcguifodjo | if(i==10){
|
|
| 272 | jcguifodjo | ||
| 522 | jcguifodjo | deg_long=decode_nombre(c,3);
|
|
| 651 | jcguifodjo | deg_long+=(c[3]-'0')*0.166666666666666;
|
|
deg_long+=(c[4]-'0')*0.0166666666666666;
|
|||
| 522 | jcguifodjo | for(i=6;i<10;i++){
|
|
| 651 | jcguifodjo | deg_long+=((c[i]-'0')*pow(10,4-i))*0.166666666666666;
|
|
| 522 | jcguifodjo | }
|
|
return deg_long;
|
|||
}
|
|||
| 272 | jcguifodjo | ||
| 522 | jcguifodjo | }
|
|
| 272 | jcguifodjo | ||
| 522 | jcguifodjo | ||
//Decodage d'une trame
|
|||
| 637 | jcguifodjo | void decode_trame(char *trame, Position* p)
|
|
| 522 | jcguifodjo | {
|
|
int i,j,d;
|
|||
| 651 | jcguifodjo | char lat[]={'0','0','0','0','0','0','0','0','0','\0'},lon[]={'0','0','0','0','0','0','0','0','0','0','\0'},indx_lat[]={'0','\0'},indx_lon[]={'0','\0'};
|
|
| 522 | jcguifodjo | ||
if(trame_cmp(trame,"GPGGA")==1){
|
|||
d=1;
|
|||
| 651 | jcguifodjo | for (i=0; i<9; i++){
|
|
| 522 | jcguifodjo | lat[i]=trame[i+17];
|
|
}
|
|||
| 651 | jcguifodjo | for (j=0; j<10;j++){
|
|
lon[j]=trame[j+29];
|
|||
| 522 | jcguifodjo | }
|
|
indx_lat[0]=trame[27];
|
|||
indx_lon[0]=trame[40];
|
|||
| 637 | jcguifodjo | (*p).latitude= conversion(lat);
|
|
(*p).longitude=conversion(lon);
|
|||
| 522 | jcguifodjo | ||
if(indx_lat[0]=='S'){
|
|||
| 637 | jcguifodjo | (*p).latitude=-(*p).latitude;
|
|
| 522 | jcguifodjo | }
|
|
| 632 | jcguifodjo | if(indx_lon[0]=='W'){
|
|
| 637 | jcguifodjo | (*p).longitude=-(*p).longitude;
|
|
| 522 | jcguifodjo | }
|
|
| 632 | jcguifodjo | }
|
|
| 272 | jcguifodjo | else{
|
|
d=0;
|
|||
| 671 | jcguifodjo | printf("\nNe peut decode cette trame");
|
|
| 522 | jcguifodjo | ||
}
|
|||
| 637 | jcguifodjo | printf("\nTrames: %s", trame);
|
|
| 651 | jcguifodjo | printf("\nLatitude: %2.7f", (*p).latitude);
|
|
printf("\nLongitude: %2.7f", (*p).longitude);
|
|||
| 637 | jcguifodjo | ||
| 272 | jcguifodjo | }
|
|
| 632 | jcguifodjo | ||
| 671 | jcguifodjo | // calcule la racine carr?e en se basant sur l'algorithme de Heron
|
|
float racine_carree(float nb){
|
|||
float a,fa,e;
|
|||
e=1;
|
|||
a=1;
|
|||
while(e>0.0001){
|
|||
fa=((nb/a)+a)/2;
|
|||
if(fa>a)
|
|||
{
|
|||
e=fa-a;
|
|||
}
|
|||
else
|
|||
e=a-fa;
|
|||
a=fa;
|
|||
}
|
|||
return a;
|
|||
}
|
|||
| 632 | jcguifodjo | //Fonction qui calcule la distance entre deux positions
|
|
float calcul_distance(Position p1,Position p2){
|
|||
float x,a,y,z,distance,b;
|
|||
a=(p2.longitude-p1.longitude);
|
|||
b=(p1.latitude+p2.latitude)/2;
|
|||
x=a*cos(b*rads);
|
|||
y=p2.latitude-p1.latitude;
|
|||
| 671 | jcguifodjo | z=racine_carree((x*x)+(y*y));
|
|
| 632 | jcguifodjo | distance=k*z;
|
|
return distance;
|
|||
}
|
|||
//Fonction qui calcule la vitesse du vehicule par seconde
|
|||
| 671 | jcguifodjo | float calcule_vitesse(Position p1, Position p2){
|
|
| 632 | jcguifodjo | ||
float distance,vitesse;
|
|||
| 671 | jcguifodjo | distance=calcul_distance( p1, p2);
|
|
| 632 | jcguifodjo | vitesse=3.6*distance;
|
|
| 671 | jcguifodjo | return vitesse;
|
|
| 632 | jcguifodjo | ||
| 671 | jcguifodjo | }
|
|
typedef struct {
|
|||
Position rpos;
|
|||
float vitmax;
|
|||
} Zone ;
|
|||
Zone zones[] = {
|
|||
{{44.7887762, -3.012}, 50}, /* Descripteur de la premi?re zone */
|
|||
{{44.7891220, -3,013}, 70},
|
|||
};
|
|||
//Calcul de la distance ? la zone dangereuse la plus proche
|
|||
int distance_a_la_proche_zone(Position p, Zone r[],int nb_zones, float *d){
|
|||
int i;
|
|||
float dist;
|
|||
if(nb_zones!=0){
|
|||
(*d)=calcul_distance(p,r[0].rpos);
|
|||
for(i=1;i<nb_zones;i++){
|
|||
dist=calcul_distance(p,r[i].rpos);
|
|||
if(dist<(*d)){
|
|||
(*d)=dist;
|
|||
}
|
|||
}
|
|||
}
|
|||
return i;
|
|||
}
|
|||
| 1 | jalaffon | //Fonction ? modifier !!!!!
|
|
void traitement(char * trame)
|
|||
| 107 | jcguifodjo | {
|
|
| 637 | jcguifodjo | Position p;
|
|
| 671 | jcguifodjo | ||
| 107 | jcguifodjo | static int cpt=0 ;
|
|
| 121 | jcguifodjo | cpt++ ;
|
|
if (trame_cmp(trame,"GPGGA")==1)
|
|||
{
|
|||
| 651 | jcguifodjo | decode_trame(trame,&p);
|
|
| 632 | jcguifodjo | ||
| 637 | jcguifodjo | ||
| 121 | jcguifodjo | }
|
|
| 1 | jalaffon | }
|
|
| 522 | jcguifodjo | //Ajouter vos tests unitaires dans cette fonction.
|
|
//test unitaire decode_int
|
|||
void test_decode_int(void){
|
|||
if (decode_int('0')!=0){
|
|||
printf ("Erreur Test unitaire basique decode int.\n");
|
|||
exit(-1);
|
|||
}
|
|||
if (decode_int('1')!=1){
|
|||
printf ("Erreur Test unitaire basique decode int.\n");
|
|||
exit(-1);
|
|||
}
|
|||
if (decode_int('2')!=2){
|
|||
printf ("Erreur Test unitaire basique decode int.\n");
|
|||
exit(-1);
|
|||
}
|
|||
if (decode_int('3')!=3){
|
|||
printf ("Erreur Test unitaire basique decode int.\n");
|
|||
exit(-1);
|
|||
}
|
|||
if (decode_int('4')!=4){
|
|||
printf ("Erreur Test unitaire basique decode int.\n");
|
|||
exit(-1);
|
|||
}
|
|||
if (decode_int('5')!=5){
|
|||
printf ("Erreur Test unitaire basique decode int.\n");
|
|||
exit(-1);
|
|||
}
|
|||
if (decode_int('A')!=-1){
|
|||
printf ("Erreur Test unitaire basique decode int.\n");
|
|||
exit(-1);
|
|||
}
|
|||
}
|
|||
//Test unitaire decode_nombre
|
|||
void test_decode_nombre(void){
|
|||
if (decode_nombre("632",2)!=63){
|
|||
| 643 | jcguifodjo | printf ("Erreur Test unitaire basique decode nombre1.\n");
|
|
| 522 | jcguifodjo | exit(-1);
|
|
}
|
|||
| 632 | jcguifodjo | if (decode_nombre("400",2)!=40){
|
|
| 643 | jcguifodjo | printf ("Erreur Test unitaire basique decode nombre2.\n");
|
|
| 522 | jcguifodjo | exit(-1);
|
|
}
|
|||
if (decode_nombre("1204",3)!=120){
|
|||
| 643 | jcguifodjo | printf ("Erreur Test unitaire basique decode nombre3.\n");
|
|
| 522 | jcguifodjo | exit(-1);
|
|
}
|
|||
if (decode_nombre("10",1)!=1){
|
|||
| 643 | jcguifodjo | printf ("Erreur Test unitaire basique decode nombre4.\n");
|
|
| 522 | jcguifodjo | exit(-1);
|
|
}
|
|||
if (decode_nombre("7865",3)!=786){
|
|||
| 643 | jcguifodjo | printf ("Erreur Test unitaire basique decode nombre5.\n");
|
|
| 522 | jcguifodjo | exit(-1);
|
|
}
|
|||
}
|
|||
void tests_unitaires(void){
|
|||
| 1 | jalaffon | if (5!=5){
|
|
printf ("Erreur Test unitaire basique.\n");
|
|||
exit(-1);
|
|||
}
|
|||
| 114 | jcguifodjo | if (trame_cmp("$GPGGA suite chaine","GPGGA")!=1){
|
|
| 1 | jalaffon | 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");
|
|||
| 522 | jcguifodjo | exit(-1);//Fin
|
|
}
|
|||
test_decode_int();
|
|||
test_decode_nombre();
|
|||
}
|
|||
| 1 | jalaffon | ||
| 522 | jcguifodjo | ||
| 1 | jalaffon | // 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;
|
|||
| 637 | jcguifodjo | printf ("\nTrames de tests du fichier gps.log\n");
|
|
| 1 | jalaffon | while ((trame = trame_suivante()))
|
|
traitement(trame);
|
|||
return 0;
|
|||
}
|