Révision 696
Ajouté par rademagalh il y a presque 4 ans
| SP4b3.c | ||
|---|---|---|
|
/* NOTE:THIS IS A TYPICAL EXAMPLE. */
|
||
|
/***********************************************************************/
|
||
|
|
||
|
typedef struct{
|
||
|
float latitude;
|
||
|
float longitude;
|
||
|
}Position;
|
||
|
|
||
|
|
||
|
typedef struct {
|
||
|
Position rpos;
|
||
|
float vitmax;
|
||
|
} Zone;
|
||
|
|
||
|
float CharVersDegre(char * ch);
|
||
|
Position decode_trame(char * ch);
|
||
|
float calcul_distance(Position p1,Position p2);
|
||
|
float calcul_vitesse(float distance);
|
||
|
float distance_a_la_plus_proche_zone(Position pos_voiture, Zone r[], int nb_zones);
|
||
|
|
||
|
void main(void)
|
||
|
{
|
||
|
|
||
|
Zone zones[] = {
|
||
|
{{44.7887762, -3.012}, 50}, /* Descripteur de la premi?re zone */
|
||
|
{{44.7891220, -3.013}, 70},
|
||
|
};
|
||
|
while (1)
|
||
|
{
|
||
|
float distance=0;
|
||
|
Position pos1=decode_trame("$GPGGA,141914.00,4545.0000,N,00306.6036,E,1,05,3.4,499.3,M,,M,,*7D");
|
||
|
Position pos2=decode_trame("$GPGGA,141925.00,4545.2410,N,00306.6046,E,1,05,3.4,501.4,M,,M,,*7D");
|
||
|
distance = calcul_distance(pos1,pos2);
|
||
|
calcul_vitesse(distance);
|
||
|
distance_a_la_plus_proche_zone(pos1, zones, 2);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
float distance_a_la_plus_proche_zone(Position pos_voiture, Zone r[], int nb_zones)
|
||
|
{
|
||
|
int i;
|
||
|
int indice_rechercher=0;
|
||
|
float d_min=0,d=0;
|
||
|
Position t[10];
|
||
|
for (i = 0; i < nb_zones; i++)
|
||
|
{
|
||
|
Position pos=r[i].rpos;
|
||
|
t[i]=pos;
|
||
|
}
|
||
|
d_min=calcul_distance(pos_voiture,t[1]);
|
||
|
for (i = 0; i < nb_zones; i++)
|
||
|
{
|
||
|
d=calcul_distance(pos_voiture,t[i]);
|
||
|
if (d<d_min)
|
||
|
{
|
||
|
d_min=d;
|
||
|
indice_rechercher=i;
|
||
|
}
|
||
|
if (nb_zones==0)
|
||
|
{
|
||
|
return -1;
|
||
|
}
|
||
|
}
|
||
|
return d_min;
|
||
|
}
|
||
|
|
||
|
float calcul_vitesse(float distance)
|
||
|
{
|
||
|
return distance*3600;
|
||
|
}
|
||
|
|
||
|
float calcul_distance(Position p1,Position p2)
|
||
|
{
|
||
|
float R_terre=6378.137;
|
||
|
float distance = R_terre*(p1.latitude-p2.latitude)*(3.14/180);
|
||
|
return distance;
|
||
|
}
|
||
|
|
||
|
Position decode_trame(char * ch)
|
||
|
{
|
||
|
Position pos;
|
||
|
int i,indice=0;
|
||
|
char longitude[10]="";
|
||
|
char latitude[10]="";
|
||
|
int compteur_de_virgule = 0;
|
||
|
if (trame_cmp(ch,"GPGGA")==0)
|
||
|
{
|
||
|
exit(-1);
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
|
||
|
do
|
||
|
{
|
||
|
if (ch[indice]==',')
|
||
|
{
|
||
|
compteur_de_virgule++;
|
||
|
}
|
||
|
|
||
|
if (compteur_de_virgule==2)
|
||
|
{
|
||
|
for (i = 0; i < 9; i++)
|
||
|
{
|
||
|
latitude[i]=ch[indice+1];
|
||
|
indice++;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if (compteur_de_virgule==4)
|
||
|
{
|
||
|
for (i = 0; i < 9; i++)
|
||
|
{
|
||
|
longitude[i]=ch[indice+1];
|
||
|
indice++;
|
||
|
}
|
||
|
break;
|
||
|
}
|
||
|
indice++;
|
||
|
} while (ch[indice]!='\0');
|
||
|
pos.latitude = CharVersDegre(latitude);
|
||
|
pos.longitude = CharVersDegre(longitude);
|
||
|
}
|
||
|
return pos;
|
||
|
}
|
||
|
|
||
|
float CharVersDegre(char * ch)
|
||
|
{
|
||
|
char * nombre=ch;
|
||
|
float dec;
|
||
|
int compte=0,n,degre;
|
||
|
int virguleVu=0;
|
||
|
float res = 0, fact = 1, d=0;
|
||
|
if (*ch == '-')
|
||
|
{
|
||
|
ch++;
|
||
|
fact = -1;
|
||
|
}
|
||
|
for (virguleVu = 0; *ch; ch++)
|
||
|
{
|
||
|
compte++;
|
||
|
if (*ch == '.')
|
||
|
{
|
||
|
virguleVu = 1;
|
||
|
n=compte-1;
|
||
|
continue;
|
||
|
}
|
||
|
d = *ch - '0';
|
||
|
if (d >= 0 && d <= 9)
|
||
|
{
|
||
|
if (virguleVu) fact /= 10.0;
|
||
|
res = res * 10.0 + d;
|
||
|
}
|
||
|
if (*ch == ',')
|
||
|
{
|
||
|
break;
|
||
|
}
|
||
|
}
|
||
|
dec=(res * fact);
|
||
|
if (n==4)
|
||
|
{
|
||
|
degre=decode_nombre(nombre,2);
|
||
|
dec=(dec-degre*100)/60;
|
||
|
}
|
||
|
else if (n==5)
|
||
|
{
|
||
|
degre=decode_nombre(nombre,3);
|
||
|
dec=(dec-degre*100)/60;
|
||
|
}
|
||
|
dec = degre + dec;
|
||
|
return dec;
|
||
|
}
|
||
|
|
||
|
int decode_nombre(char * ch, int n)
|
||
|
{
|
||
|
int longueurMot=0;
|
||
|
char caratereCourrant=0;
|
||
|
//Obtention de la longueur du Mot extrait entre le symbole $ et la virgule
|
||
|
do
|
||
|
{
|
||
|
caratereCourrant=ch[longueurMot];
|
||
|
longueurMot++;
|
||
|
}
|
||
|
while (caratereCourrant != '\0');
|
||
|
longueurMot--;
|
||
|
if (longueurMot < n ||n==0){
|
||
|
return 0;
|
||
|
}
|
||
|
else{
|
||
|
int j=0,sum=0;
|
||
|
for(j=0; j<n; j++)
|
||
|
{
|
||
|
if(ch[j] < 48 || ch[j] > 57)
|
||
|
{
|
||
|
return 0;
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
sum = sum*10 + (ch[j] - 48);
|
||
|
}
|
||
|
}
|
||
|
return sum;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
int decode_int(char c)
|
||
|
{
|
||
|
int code;
|
||
|
char i;
|
||
|
if(c >= '0' && c <= '9')
|
||
|
// Verification que le caract?re 'c' est un caract?re entre 0 et 9 dans la table ascii
|
||
|
{
|
||
|
for (i= '0'; i <='9'; i++)
|
||
|
//Parcourir la table ascii ? la recherche du carac. c
|
||
|
{
|
||
|
if (i==c)
|
||
|
// Identification du carac. c dans la table ascii
|
||
|
{
|
||
|
code=i-48; // On ram?ne le code ascii de c entre 0 et 9
|
||
|
return code;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
// le caract?re 'c' n'est pas entre '0' et '9' dans la table ascii donc on renvoie -1
|
||
|
return -1;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
int trame_cmp(char * trame, char * type)
|
||
|
{
|
||
|
int indice=1;
|
||
|
char mot[10]="";
|
||
|
int longueurMot=0;
|
||
|
int longueurType=0;
|
||
|
char caratereCourrant=0;
|
||
|
//Extraction du mot entre le symbole $ et la virgule
|
||
|
do
|
||
|
{
|
||
|
mot[indice-1]=trame[indice];
|
||
|
indice++;
|
||
|
}
|
||
|
while (trame[indice]!=',');
|
||
|
|
||
|
//Obtention de la longueur du Mot extrait entre le symbole $ et la virgule
|
||
|
do
|
||
|
{
|
||
|
caratereCourrant=mot[longueurMot];
|
||
|
longueurMot++;
|
||
|
}
|
||
|
while (caratereCourrant != '\0');
|
||
|
longueurMot--;
|
||
|
|
||
|
caratereCourrant=0;
|
||
|
////Obtention de la longueur du type
|
||
|
do
|
||
|
{
|
||
|
caratereCourrant=type[longueurType];
|
||
|
longueurType++;
|
||
|
}
|
||
|
while (caratereCourrant != '\0');
|
||
|
longueurType--;
|
||
|
|
||
|
if (longueurType==longueurMot) //Verification si le type est de m?me taille que le mot extrait
|
||
|
{
|
||
|
int verificateur=0; //varaiable de validation
|
||
|
int i;
|
||
|
for (i = 0; i < longueurMot; i++)
|
||
|
{
|
||
|
if (mot[i]==type[i])
|
||
|
{
|
||
|
verificateur=1; // Valide la comparaison entre deux lettres identiques
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
verificateur=0; // Si la comparaison n'est pas bonne, on arr?te la comparaison
|
||
|
break;
|
||
|
}
|
||
|
}
|
||
|
if (verificateur==1 && type=="GPGGA")
|
||
|
{
|
||
|
// Condition de renvoie si le type est bien GPGGA
|
||
|
return 1;
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
// return 0 pour tout les test qui ne seraient pas du type GPGGA
|
||
|
return 0;
|
||
|
}
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
// return 0 si aucune comparaison n'est bonne
|
||
|
return 0;
|
||
|
}
|
||
|
}
|
||
Formats disponibles : Unified diff
Mise a jour tp SPb3 avant modification des fonctions flottantes