Etienne Stransky / Mbed 2 deprecated Lecture_GPS

ok

Dependencies:   mbed

main.cpp@0:a3beb1f37be6, 2019-02-12 (annotated)

Committer:
stersky
Date:
Tue Feb 12 14:46:28 2019 +0000
Revision:
0:a3beb1f37be6
ok

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stersky 0:a3beb1f37be6 1 #include "mbed.h"
stersky 0:a3beb1f37be6 2
stersky 0:a3beb1f37be6 3
stersky 0:a3beb1f37be6 4 int lecture_GPS(double *ptr_heure,double *ptr_lat,double *ptr_longi,double *ptr_vitesse,double *ptr_route);
stersky 0:a3beb1f37be6 5 double transfo_format(double a);
stersky 0:a3beb1f37be6 6
stersky 0:a3beb1f37be6 7 Serial PC(USBTX, USBRX);
stersky 0:a3beb1f37be6 8 Serial GPS(p9,p10);
stersky 0:a3beb1f37be6 9
stersky 0:a3beb1f37be6 10
stersky 0:a3beb1f37be6 11
stersky 0:a3beb1f37be6 12 int main()
stersky 0:a3beb1f37be6 13 {
stersky 0:a3beb1f37be6 14 double heure,latitude = 0,longitude = 0,vitesse,route,distance_restante,lat_t_m_1 = 0,long_t_m_1 = 0;
stersky 0:a3beb1f37be6 15 double lat_destination = 48.787068,longi_destination = 2.327229;
stersky 0:a3beb1f37be6 16 double *ptr_h=&heure,*ptr_lat=&latitude,*ptr_long=&longitude,*ptr_v=&vitesse,*ptr_rte=&route;
stersky 0:a3beb1f37be6 17 double route_th,route_exp=0,route_exp_t_m_1=0,diff_routes=0,gouvernail=0;
stersky 0:a3beb1f37be6 18 int a;
stersky 0:a3beb1f37be6 19
stersky 0:a3beb1f37be6 20 GPS.baud(9600);
stersky 0:a3beb1f37be6 21 PC.baud(460800);
stersky 0:a3beb1f37be6 22
stersky 0:a3beb1f37be6 23 while(1) {
stersky 0:a3beb1f37be6 24
stersky 0:a3beb1f37be6 25 //Mise en mémoire des anciennes coordonnées gps
stersky 0:a3beb1f37be6 26 lat_t_m_1 = latitude;
stersky 0:a3beb1f37be6 27 long_t_m_1 = longitude;
stersky 0:a3beb1f37be6 28
stersky 0:a3beb1f37be6 29 //Lecture des données du GPS
stersky 0:a3beb1f37be6 30 do
stersky 0:a3beb1f37be6 31 {
stersky 0:a3beb1f37be6 32 a = lecture_GPS(ptr_h,ptr_lat,ptr_long,ptr_v,ptr_rte);
stersky 0:a3beb1f37be6 33 if (a!=1) {PC.printf("Erreur de reception\n\r\n\r");}
stersky 0:a3beb1f37be6 34 }
stersky 0:a3beb1f37be6 35 while(a==2);
stersky 0:a3beb1f37be6 36
stersky 0:a3beb1f37be6 37 latitude = transfo_format(latitude);
stersky 0:a3beb1f37be6 38 longitude = transfo_format(longitude);
stersky 0:a3beb1f37be6 39
stersky 0:a3beb1f37be6 40 PC.printf("LAT : %f, LONGI : %f\n\rLAT_DEST : %f LONGI_DEST : %f\r\n",latitude,longitude,lat_destination,longi_destination);
stersky 0:a3beb1f37be6 41
stersky 0:a3beb1f37be6 42 //Calcul de la route a suivre :
stersky 0:a3beb1f37be6 43 route_th = 57.2956455309649*atan2(longi_destination-longitude,lat_destination-latitude);
stersky 0:a3beb1f37be6 44 if(route_th<0) route_th += 360;//On s'assure que la route est comprise entre 0 et 360°
stersky 0:a3beb1f37be6 45
stersky 0:a3beb1f37be6 46 //Calcul de la route suivie ssi la position a changé
stersky 0:a3beb1f37be6 47 if((lat_t_m_1 > latitude+0.00001)||(lat_t_m_1 < latitude-0.00001)||(long_t_m_1 > longitude+0.00001)||(long_t_m_1 < latitude-0.00001))
stersky 0:a3beb1f37be6 48 {
stersky 0:a3beb1f37be6 49 route_exp = 0.5*(route_exp_t_m_1+57.2956455309649*atan2(longitude-long_t_m_1,latitude-lat_t_m_1));
stersky 0:a3beb1f37be6 50 if(route_exp<0) route_exp += 360;//On s'assure que la route est comprise entre 0 et 360°
stersky 0:a3beb1f37be6 51 }
stersky 0:a3beb1f37be6 52
stersky 0:a3beb1f37be6 53 //Calcul de la distance à parcourir en mètres
stersky 0:a3beb1f37be6 54 distance_restante = sqrt(pow(111111.1*(lat_destination-latitude),2)+pow(75000*(longi_destination-longitude),2));
stersky 0:a3beb1f37be6 55
stersky 0:a3beb1f37be6 56 PC.printf("Route : %lf\n\rRoute theorique : %lf\r\nRoute experimentale : %lf\r\nDistance restance : %lf\r\n",route,route_th,route_exp,distance_restante);
stersky 0:a3beb1f37be6 57
stersky 0:a3beb1f37be6 58
stersky 0:a3beb1f37be6 59 //Calcul, avec la route à suivre et la route suivie, des indications à donner
stersky 0:a3beb1f37be6 60 //On utilise ici une variable "gouvernail", allant de -100 à 100
stersky 0:a3beb1f37be6 61 //-100 correcpond à "à gauche toute" et 100 à "à droite toute"
stersky 0:a3beb1f37be6 62
stersky 0:a3beb1f37be6 63 if(distance_restante<10.0)
stersky 0:a3beb1f37be6 64 {
stersky 0:a3beb1f37be6 65 PC.printf("VOUS ETES ARRIVES\r\n");
stersky 0:a3beb1f37be6 66 }
stersky 0:a3beb1f37be6 67 else
stersky 0:a3beb1f37be6 68 {
stersky 0:a3beb1f37be6 69 if((route<=route_th+5.0)&&(route>=route_th-5))
stersky 0:a3beb1f37be6 70 {
stersky 0:a3beb1f37be6 71 PC.printf("TOUT DROIT\r\n");
stersky 0:a3beb1f37be6 72 gouvernail = 0;
stersky 0:a3beb1f37be6 73 }
stersky 0:a3beb1f37be6 74 else
stersky 0:a3beb1f37be6 75 {
stersky 0:a3beb1f37be6 76 //On calcule la différence d'angle entre la route actuellement suivie
stersky 0:a3beb1f37be6 77 diff_routes = abs(route_th-route);
stersky 0:a3beb1f37be6 78 if(diff_routes > 180.0) diff_routes = 360.0 - diff_routes;
stersky 0:a3beb1f37be6 79
stersky 0:a3beb1f37be6 80 //On vérifie la direction à suivre, s'il faut aller à droite ou à gauche.
stersky 0:a3beb1f37be6 81 if((route_th>=route)&&(route_th<=(route+180)))
stersky 0:a3beb1f37be6 82 {
stersky 0:a3beb1f37be6 83 PC.printf("Direction : droite\r\n");
stersky 0:a3beb1f37be6 84 //On calcule la valeur à donner au gouvernail avec un correcteur proportionnel
stersky 0:a3beb1f37be6 85 //si la différence est sup à 45°, on sature à 100, sinon on a une action
stersky 0:a3beb1f37be6 86 //proportionnelle
stersky 0:a3beb1f37be6 87 if(diff_routes>45.0) gouvernail = 100.0;
stersky 0:a3beb1f37be6 88 else gouvernail = 2.22*diff_routes;
stersky 0:a3beb1f37be6 89 PC.printf("Instruction donnee au gouvernail : %lf\r\n",gouvernail);
stersky 0:a3beb1f37be6 90 }
stersky 0:a3beb1f37be6 91 else
stersky 0:a3beb1f37be6 92 {
stersky 0:a3beb1f37be6 93 PC.printf("Direction : gauche\r\n");
stersky 0:a3beb1f37be6 94 //On fait de même pour tourner à gauche que pour tourner à droite, mais on a des
stersky 0:a3beb1f37be6 95 //coefficients négatifs
stersky 0:a3beb1f37be6 96 if(diff_routes>45.0) gouvernail = -100.0;
stersky 0:a3beb1f37be6 97 else gouvernail = -2.22*diff_routes;
stersky 0:a3beb1f37be6 98 PC.printf("Instruction donnee au gouvernail : %lf\r\n",gouvernail);
stersky 0:a3beb1f37be6 99 }
stersky 0:a3beb1f37be6 100 }
stersky 0:a3beb1f37be6 101 }
stersky 0:a3beb1f37be6 102
stersky 0:a3beb1f37be6 103 PC.printf("\n\r");
stersky 0:a3beb1f37be6 104 route_exp_t_m_1 = route_exp;
stersky 0:a3beb1f37be6 105 }
stersky 0:a3beb1f37be6 106 }
stersky 0:a3beb1f37be6 107
stersky 0:a3beb1f37be6 108 //
stersky 0:a3beb1f37be6 109
stersky 0:a3beb1f37be6 110 //transforme les coordonnees au format dd.mmssss au lieu de ddmm.mmmm
stersky 0:a3beb1f37be6 111 double transfo_format(double a)
stersky 0:a3beb1f37be6 112 {
stersky 0:a3beb1f37be6 113 double e;
stersky 0:a3beb1f37be6 114 int c,b,d;
stersky 0:a3beb1f37be6 115 d = a/100;
stersky 0:a3beb1f37be6 116 c = a*10000;
stersky 0:a3beb1f37be6 117 b = c%1000000;
stersky 0:a3beb1f37be6 118 e = d+b/(60.0*10000.0);
stersky 0:a3beb1f37be6 119 return e;
stersky 0:a3beb1f37be6 120 }
stersky 0:a3beb1f37be6 121
stersky 0:a3beb1f37be6 122
stersky 0:a3beb1f37be6 123
stersky 0:a3beb1f37be6 124 int lecture_GPS(double *ptr_heure,double *ptr_lat,double *ptr_longi,double *ptr_vitesse,double *ptr_route)
stersky 0:a3beb1f37be6 125 {
stersky 0:a3beb1f37be6 126 char i=2;
stersky 0:a3beb1f37be6 127 char buffer[120];
stersky 0:a3beb1f37be6 128 int test_reception = 0;
stersky 0:a3beb1f37be6 129 char a, etat=0;
stersky 0:a3beb1f37be6 130 double h,lat,lng,v,dir;
stersky 0:a3beb1f37be6 131
stersky 0:a3beb1f37be6 132 while(test_reception == 0)
stersky 0:a3beb1f37be6 133 {
stersky 0:a3beb1f37be6 134 if (GPS.readable()) { // attention PC.readable reste à 1 tant qu'il n'y a pas eu de getc qui vide le buffer
stersky 0:a3beb1f37be6 135 a=GPS.getc();
stersky 0:a3beb1f37be6 136
stersky 0:a3beb1f37be6 137 switch(etat)
stersky 0:a3beb1f37be6 138 {
stersky 0:a3beb1f37be6 139 case 0 :
stersky 0:a3beb1f37be6 140 if(a == '$') etat = 1; //On détecte le début de la trame
stersky 0:a3beb1f37be6 141 break;
stersky 0:a3beb1f37be6 142 case 1 :
stersky 0:a3beb1f37be6 143 if(a != '$') //On s'assure que l'on reçoit bien un autre carcatère que celui de début de trame
stersky 0:a3beb1f37be6 144 {
stersky 0:a3beb1f37be6 145 etat = 2;
stersky 0:a3beb1f37be6 146 buffer[0] = '$';//On stocke le caractère de début de trame dans la chaine (mais c'est juste pour faire joli en fait)
stersky 0:a3beb1f37be6 147 buffer[1] = a;//On stocke le premier caractère utile de la trame
stersky 0:a3beb1f37be6 148 }
stersky 0:a3beb1f37be6 149 break;
stersky 0:a3beb1f37be6 150
stersky 0:a3beb1f37be6 151 case 2 :
stersky 0:a3beb1f37be6 152 buffer[i] = a;//On stocke l'octet reçu dans la trame
stersky 0:a3beb1f37be6 153 i++;
stersky 0:a3beb1f37be6 154 if(a == 'C')//Si un caractère entré est un 'C', alors la trame nous intéresse : on poursuit
stersky 0:a3beb1f37be6 155 {
stersky 0:a3beb1f37be6 156 etat = 3;
stersky 0:a3beb1f37be6 157 }
stersky 0:a3beb1f37be6 158 else if(i>=7)//S'il n'y a pas de 'C' au 7ème caractère utile, la trame ne nous intéresse pas : on arrête l'acquisition
stersky 0:a3beb1f37be6 159 {
stersky 0:a3beb1f37be6 160 etat = 5;
stersky 0:a3beb1f37be6 161 }
stersky 0:a3beb1f37be6 162 break;
stersky 0:a3beb1f37be6 163
stersky 0:a3beb1f37be6 164 case 3 :
stersky 0:a3beb1f37be6 165
stersky 0:a3beb1f37be6 166 if(a=='$')//Si on revoit le caractère de début de trame, on s'arrête
stersky 0:a3beb1f37be6 167 {
stersky 0:a3beb1f37be6 168 etat = 4;
stersky 0:a3beb1f37be6 169 }
stersky 0:a3beb1f37be6 170 else
stersky 0:a3beb1f37be6 171 {
stersky 0:a3beb1f37be6 172 buffer[i]=a;//Sinon on stocke le caractère entré dans la chaîne de caractères
stersky 0:a3beb1f37be6 173 i++;
stersky 0:a3beb1f37be6 174 }
stersky 0:a3beb1f37be6 175 break;
stersky 0:a3beb1f37be6 176
stersky 0:a3beb1f37be6 177 case 4 :
stersky 0:a3beb1f37be6 178
stersky 0:a3beb1f37be6 179 if((buffer[17]=='A')||(buffer[18]=='A'))
stersky 0:a3beb1f37be6 180 {
stersky 0:a3beb1f37be6 181 sscanf(buffer,"$GPRMC,%lf,A,%lf,N,%lf,E,%lf,%lf",&h,&lat,&lng,&v,&dir);
stersky 0:a3beb1f37be6 182 *ptr_heure = h;
stersky 0:a3beb1f37be6 183 *ptr_lat = lat;
stersky 0:a3beb1f37be6 184 *ptr_longi = lng;
stersky 0:a3beb1f37be6 185 *ptr_vitesse = v;
stersky 0:a3beb1f37be6 186 *ptr_route = dir;
stersky 0:a3beb1f37be6 187 test_reception = 1;
stersky 0:a3beb1f37be6 188 }
stersky 0:a3beb1f37be6 189 else
stersky 0:a3beb1f37be6 190 {
stersky 0:a3beb1f37be6 191 test_reception = 2;
stersky 0:a3beb1f37be6 192 }
stersky 0:a3beb1f37be6 193 PC.printf("%s",buffer);//On affiche la chaîne
stersky 0:a3beb1f37be6 194 etat = 5;
stersky 0:a3beb1f37be6 195 break;
stersky 0:a3beb1f37be6 196
stersky 0:a3beb1f37be6 197 case 5 :
stersky 0:a3beb1f37be6 198 etat = 0;//On réinitialise l'acquisition
stersky 0:a3beb1f37be6 199 for(i=0;i<120;i++)//On réinitialise la chaîne de caractères
stersky 0:a3beb1f37be6 200 {
stersky 0:a3beb1f37be6 201 buffer[i] = 0;
stersky 0:a3beb1f37be6 202 }
stersky 0:a3beb1f37be6 203 i=2;
stersky 0:a3beb1f37be6 204 break;
stersky 0:a3beb1f37be6 205
stersky 0:a3beb1f37be6 206 default :
stersky 0:a3beb1f37be6 207 etat = 0;
stersky 0:a3beb1f37be6 208 break;
stersky 0:a3beb1f37be6 209 }
stersky 0:a3beb1f37be6 210 }
stersky 0:a3beb1f37be6 211
stersky 0:a3beb1f37be6 212 }
stersky 0:a3beb1f37be6 213 return test_reception;
stersky 0:a3beb1f37be6 214 }