etienne maillet / Mbed 2 deprecated GNSS1A1_FINAL

GNSS1A1 polytech grenoble

Dependencies:   mbed mbed-rtos

main.cpp@1:2f8deb2a7fe1, 2019-02-28 (annotated)

Committer:
etiene32
Date:
Thu Feb 28 18:41:10 2019 +0000
Revision:
1:2f8deb2a7fe1
Parent:
0:4e088cbb2dbf 
polytech grenoble etienne

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nirnakern 0:4e088cbb2dbf 1 #include "mbed.h"
nirnakern 0:4e088cbb2dbf 2 #include "rtos/rtos.h"
nirnakern 0:4e088cbb2dbf 3 #include "Teseo-LIV3F.h"
nirnakern 0:4e088cbb2dbf 4 #include <cstdlib>
nirnakern 0:4e088cbb2dbf 5 #include <vector>
etiene32 1:2f8deb2a7fe1 6 #include <sstream>
etiene32 1:2f8deb2a7fe1 7 #include <stdio.h>
etiene32 1:2f8deb2a7fe1 8
etiene32 1:2f8deb2a7fe1 9 int time_between=3000; //Temps entre 2 mesures
etiene32 1:2f8deb2a7fe1 10 int unite=2; // 1 = DM ; 2 = DMS ; 3 = Décimal
etiene32 1:2f8deb2a7fe1 11 float sensibilite=0.00001; //Valeur en dessous de laquelle on considere aucun changement de position
etiene32 1:2f8deb2a7fe1 12 int nbre_iteration = 2; //nombre de meme position avant stoppage du gps car certain de la position
etiene32 1:2f8deb2a7fe1 13
etiene32 1:2f8deb2a7fe1 14 //Def fonction
etiene32 1:2f8deb2a7fe1 15 void Start_GPS();
etiene32 1:2f8deb2a7fe1 16 void f1 ();
etiene32 1:2f8deb2a7fe1 17 void f2 ();
etiene32 1:2f8deb2a7fe1 18 void conversion_decimal(float coord, int indice);
etiene32 1:2f8deb2a7fe1 19 void decimal_to_DM(float latitude, float longitude);
etiene32 1:2f8deb2a7fe1 20 void decimal_to_DMS(float latitude, float longitude);
etiene32 1:2f8deb2a7fe1 21
etiene32 1:2f8deb2a7fe1 22 char buffer[20];
etiene32 1:2f8deb2a7fe1 23 char buffer1[20];
nirnakern 0:4e088cbb2dbf 24
nirnakern 0:4e088cbb2dbf 25 /*cose per gps*/
nirnakern 0:4e088cbb2dbf 26 static Thread t1;
nirnakern 0:4e088cbb2dbf 27 static Thread t2;
nirnakern 0:4e088cbb2dbf 28
nirnakern 0:4e088cbb2dbf 29 static Mutex data_access;
nirnakern 0:4e088cbb2dbf 30
nirnakern 0:4e088cbb2dbf 31 static Timer system_timer;
etiene32 1:2f8deb2a7fe1 32 static Serial usb_serial(USBTX, USBRX, 115200);
nirnakern 0:4e088cbb2dbf 33 static Serial *serial_debug= &usb_serial;
nirnakern 0:4e088cbb2dbf 34
nirnakern 0:4e088cbb2dbf 35
etiene32 1:2f8deb2a7fe1 36 static Teseo_LIV3F Teseo(D7,D13,D6,D2,D8,&usb_serial);
nirnakern 0:4e088cbb2dbf 37
nirnakern 0:4e088cbb2dbf 38
nirnakern 0:4e088cbb2dbf 39 struct gps_coordinate{
nirnakern 0:4e088cbb2dbf 40 float lat;
nirnakern 0:4e088cbb2dbf 41 char lat_cardinal;
nirnakern 0:4e088cbb2dbf 42 float longitud;
nirnakern 0:4e088cbb2dbf 43 char long_cardinal;
etiene32 1:2f8deb2a7fe1 44 bool valid;
nirnakern 0:4e088cbb2dbf 45 };
nirnakern 0:4e088cbb2dbf 46
etiene32 1:2f8deb2a7fe1 47 struct gps_DM_coordinate{
etiene32 1:2f8deb2a7fe1 48 int degres_lat;
etiene32 1:2f8deb2a7fe1 49 float minute_lat;
etiene32 1:2f8deb2a7fe1 50 char lat_card;
etiene32 1:2f8deb2a7fe1 51 int degres_lon;
etiene32 1:2f8deb2a7fe1 52 float minute_lon;
etiene32 1:2f8deb2a7fe1 53 char lon_card;
etiene32 1:2f8deb2a7fe1 54 };
nirnakern 0:4e088cbb2dbf 55
etiene32 1:2f8deb2a7fe1 56 struct gps_DMS_coordinate{
etiene32 1:2f8deb2a7fe1 57 int degres_lat;
etiene32 1:2f8deb2a7fe1 58 int minute_lat;
etiene32 1:2f8deb2a7fe1 59 float seconde_lat;
etiene32 1:2f8deb2a7fe1 60 char lat_card;
etiene32 1:2f8deb2a7fe1 61 int degres_lon;
etiene32 1:2f8deb2a7fe1 62 int minute_lon;
etiene32 1:2f8deb2a7fe1 63 float seconde_lon;
etiene32 1:2f8deb2a7fe1 64 char lon_card;
etiene32 1:2f8deb2a7fe1 65 };
nirnakern 0:4e088cbb2dbf 66
etiene32 1:2f8deb2a7fe1 67 struct gps_DD_coordinate{
etiene32 1:2f8deb2a7fe1 68 float latitude;
etiene32 1:2f8deb2a7fe1 69 float longitude;
etiene32 1:2f8deb2a7fe1 70 };
etiene32 1:2f8deb2a7fe1 71
etiene32 1:2f8deb2a7fe1 72 struct last_coordinate{
etiene32 1:2f8deb2a7fe1 73 float latitude;
etiene32 1:2f8deb2a7fe1 74 float longitude;
etiene32 1:2f8deb2a7fe1 75 };
etiene32 1:2f8deb2a7fe1 76
etiene32 1:2f8deb2a7fe1 77 gps_coordinate myPosition;
etiene32 1:2f8deb2a7fe1 78
etiene32 1:2f8deb2a7fe1 79 gps_DM_coordinate myPosition_DM;
etiene32 1:2f8deb2a7fe1 80 gps_DMS_coordinate myPosition_DMS;
etiene32 1:2f8deb2a7fe1 81 gps_DD_coordinate myPosition_DD;
etiene32 1:2f8deb2a7fe1 82 last_coordinate last_position;
etiene32 1:2f8deb2a7fe1 83 last_coordinate last_iteration_position;
nirnakern 0:4e088cbb2dbf 84
nirnakern 0:4e088cbb2dbf 85 void f1 (){
nirnakern 0:4e088cbb2dbf 86 enum nmea_msg_id id;
etiene32 1:2f8deb2a7fe1 87 last_position.latitude=0;
etiene32 1:2f8deb2a7fe1 88 last_position.longitude=0;
nirnakern 0:4e088cbb2dbf 89 Teseo.Reset(serial_debug);
nirnakern 0:4e088cbb2dbf 90 Teseo.startListener(serial_debug);
nirnakern 0:4e088cbb2dbf 91 while(1) {
nirnakern 0:4e088cbb2dbf 92 osEvent evt = Teseo.queue.get();
nirnakern 0:4e088cbb2dbf 93 if (evt.status == osEventMessage) {
nirnakern 0:4e088cbb2dbf 94 struct teseo_msg *message = (struct teseo_msg *)evt.value.p;
nirnakern 0:4e088cbb2dbf 95 if (message->len){
etiene32 1:2f8deb2a7fe1 96 id = Teseo.MsgDetect(message->buf, message->len, serial_debug);
nirnakern 0:4e088cbb2dbf 97 }
nirnakern 0:4e088cbb2dbf 98 if (id==1){
nirnakern 0:4e088cbb2dbf 99 //da qui converto il messaggio in coordinate//
nirnakern 0:4e088cbb2dbf 100 data_access.lock();
nirnakern 0:4e088cbb2dbf 101 vector<char> pos;
nirnakern 0:4e088cbb2dbf 102 //latitudine
etiene32 1:2f8deb2a7fe1 103 //printf ("latitudine=");
nirnakern 0:4e088cbb2dbf 104 int cursor;
nirnakern 0:4e088cbb2dbf 105 for (int i=7;;i++){
nirnakern 0:4e088cbb2dbf 106 cursor=i;
nirnakern 0:4e088cbb2dbf 107 if (message->buf[i] == ',')
nirnakern 0:4e088cbb2dbf 108 break;
etiene32 1:2f8deb2a7fe1 109 pos.push_back(message->buf[i]);
etiene32 1:2f8deb2a7fe1 110 }
etiene32 1:2f8deb2a7fe1 111 //ma quanto è cool? conversione da vector ad array e poi a float
etiene32 1:2f8deb2a7fe1 112 myPosition.lat= atof (&pos[0]);
etiene32 1:2f8deb2a7fe1 113 //printf ("%f",myPosition.lat);
etiene32 1:2f8deb2a7fe1 114 //printf (" ");
etiene32 1:2f8deb2a7fe1 115 myPosition.lat_cardinal= message->buf[cursor+1];
etiene32 1:2f8deb2a7fe1 116 //printf ("%c ",myPosition.lat_cardinal);
etiene32 1:2f8deb2a7fe1 117 pos.clear();
etiene32 1:2f8deb2a7fe1 118 //printf ("longitudine=");
nirnakern 0:4e088cbb2dbf 119 for (int i=cursor+3;;i++){
nirnakern 0:4e088cbb2dbf 120 cursor=i;
nirnakern 0:4e088cbb2dbf 121 if (message->buf[i] == ',')
nirnakern 0:4e088cbb2dbf 122 break;
etiene32 1:2f8deb2a7fe1 123 pos.push_back(message->buf[i]);
nirnakern 0:4e088cbb2dbf 124 }
nirnakern 0:4e088cbb2dbf 125 myPosition.longitud= atof (&pos[0]);
etiene32 1:2f8deb2a7fe1 126 //printf ("%f",myPosition.longitud);
etiene32 1:2f8deb2a7fe1 127 //printf (" ");
nirnakern 0:4e088cbb2dbf 128 myPosition.long_cardinal= message->buf[cursor+1];
etiene32 1:2f8deb2a7fe1 129 // printf ("%c ",myPosition.long_cardinal);
nirnakern 0:4e088cbb2dbf 130 for (int i=cursor+3;;i++){
nirnakern 0:4e088cbb2dbf 131 cursor=i;
nirnakern 0:4e088cbb2dbf 132 if (message->buf[i] == 'A' ){
nirnakern 0:4e088cbb2dbf 133 myPosition.valid=true;
nirnakern 0:4e088cbb2dbf 134 break;
nirnakern 0:4e088cbb2dbf 135 }else if (message->buf[i] == 'V' ) {
nirnakern 0:4e088cbb2dbf 136 myPosition.valid=false;
etiene32 1:2f8deb2a7fe1 137 break;
etiene32 1:2f8deb2a7fe1 138 }
nirnakern 0:4e088cbb2dbf 139 }
nirnakern 0:4e088cbb2dbf 140 if (myPosition.valid==true){
etiene32 1:2f8deb2a7fe1 141 //printf ("Valid");
etiene32 1:2f8deb2a7fe1 142 }
nirnakern 0:4e088cbb2dbf 143 if (myPosition.valid==false){
nirnakern 0:4e088cbb2dbf 144 //printf ("Not Valid");
nirnakern 0:4e088cbb2dbf 145 }
nirnakern 0:4e088cbb2dbf 146 data_access.unlock();
nirnakern 0:4e088cbb2dbf 147 // printf ("\n\r");
nirnakern 0:4e088cbb2dbf 148 }
nirnakern 0:4e088cbb2dbf 149 Teseo.mpool.free(message);
nirnakern 0:4e088cbb2dbf 150 }
nirnakern 0:4e088cbb2dbf 151 }
nirnakern 0:4e088cbb2dbf 152 }
nirnakern 0:4e088cbb2dbf 153
etiene32 1:2f8deb2a7fe1 154 void decimal_to_DM(float latitude, float longitude){
etiene32 1:2f8deb2a7fe1 155 int degres;
etiene32 1:2f8deb2a7fe1 156 float minutes;
etiene32 1:2f8deb2a7fe1 157 //Latitude
etiene32 1:2f8deb2a7fe1 158 degres = latitude;
etiene32 1:2f8deb2a7fe1 159 minutes = (latitude - degres)*60;
etiene32 1:2f8deb2a7fe1 160 myPosition_DM.degres_lat = degres;
etiene32 1:2f8deb2a7fe1 161 myPosition_DM.minute_lat = minutes;
etiene32 1:2f8deb2a7fe1 162 myPosition_DM.lat_card = myPosition.lat_cardinal;
etiene32 1:2f8deb2a7fe1 163 //Longitude
etiene32 1:2f8deb2a7fe1 164 degres = longitude;
etiene32 1:2f8deb2a7fe1 165 minutes = (longitude - degres)*60;
etiene32 1:2f8deb2a7fe1 166 myPosition_DM.degres_lon = degres;
etiene32 1:2f8deb2a7fe1 167 myPosition_DM.minute_lon = minutes;
etiene32 1:2f8deb2a7fe1 168 myPosition_DM.lon_card = myPosition.long_cardinal;
etiene32 1:2f8deb2a7fe1 169 }
nirnakern 0:4e088cbb2dbf 170
etiene32 1:2f8deb2a7fe1 171 void decimal_to_DMS(float latitude, float longitude){
etiene32 1:2f8deb2a7fe1 172 int degres, minutes;
etiene32 1:2f8deb2a7fe1 173 float seconde, minutes_float;
etiene32 1:2f8deb2a7fe1 174 //Latitude
etiene32 1:2f8deb2a7fe1 175 degres = latitude;
etiene32 1:2f8deb2a7fe1 176 minutes_float = (latitude - degres)*60;
etiene32 1:2f8deb2a7fe1 177 minutes = minutes_float;
etiene32 1:2f8deb2a7fe1 178 seconde = (minutes_float - minutes) * 60;
etiene32 1:2f8deb2a7fe1 179 myPosition_DMS.degres_lat = degres;
etiene32 1:2f8deb2a7fe1 180 myPosition_DMS.minute_lat = minutes;
etiene32 1:2f8deb2a7fe1 181 myPosition_DMS.seconde_lat = seconde;
etiene32 1:2f8deb2a7fe1 182 myPosition_DMS.lat_card = myPosition.lat_cardinal;
etiene32 1:2f8deb2a7fe1 183 //Longitude
etiene32 1:2f8deb2a7fe1 184 degres = longitude;
etiene32 1:2f8deb2a7fe1 185 minutes_float = (longitude - degres)*60;
etiene32 1:2f8deb2a7fe1 186 minutes = minutes_float;
etiene32 1:2f8deb2a7fe1 187 seconde = (minutes_float - minutes) * 60;
etiene32 1:2f8deb2a7fe1 188 myPosition_DMS.degres_lon = degres;
etiene32 1:2f8deb2a7fe1 189 myPosition_DMS.minute_lon = minutes;
etiene32 1:2f8deb2a7fe1 190 myPosition_DMS.seconde_lon = seconde;
etiene32 1:2f8deb2a7fe1 191 myPosition_DMS.lon_card = myPosition.long_cardinal;
nirnakern 0:4e088cbb2dbf 192 }
nirnakern 0:4e088cbb2dbf 193
etiene32 1:2f8deb2a7fe1 194 void conversion_decimal(float coord, int indice){ //indice -> 1=latitude 2=longitude
etiene32 1:2f8deb2a7fe1 195 int degres, minute_int;
etiene32 1:2f8deb2a7fe1 196 double minute, coord_d, seconde;
etiene32 1:2f8deb2a7fe1 197 //Séparation degres, minute, seconde mathématiquement
etiene32 1:2f8deb2a7fe1 198 coord_d = coord / 100.0;
etiene32 1:2f8deb2a7fe1 199 degres = coord_d; //float vers int, on garde donc que la partie décimale donc degres ici
etiene32 1:2f8deb2a7fe1 200 minute = coord_d - degres;
etiene32 1:2f8deb2a7fe1 201 minute = minute * 100.0;
etiene32 1:2f8deb2a7fe1 202 minute_int = minute;
etiene32 1:2f8deb2a7fe1 203 seconde = (minute-minute_int)*60;
etiene32 1:2f8deb2a7fe1 204 //Calcul position decimal (toujours calculer pour comparaison avec last_position)
etiene32 1:2f8deb2a7fe1 205 if(indice==1){
etiene32 1:2f8deb2a7fe1 206 if(degres>=0){
etiene32 1:2f8deb2a7fe1 207 myPosition_DD.latitude = (seconde/3600.0)+(minute_int/60.0)+degres*1.0;
etiene32 1:2f8deb2a7fe1 208 }
etiene32 1:2f8deb2a7fe1 209 else{
etiene32 1:2f8deb2a7fe1 210 myPosition_DD.latitude = -(seconde/3600.0)-(minute_int/60.0)+degres*1.0;
etiene32 1:2f8deb2a7fe1 211 }
nirnakern 0:4e088cbb2dbf 212 }
etiene32 1:2f8deb2a7fe1 213 if(indice==2){
etiene32 1:2f8deb2a7fe1 214 if(degres>=0){
etiene32 1:2f8deb2a7fe1 215 myPosition_DD.longitude = (seconde/3600.0)+(minute_int/60.0)+degres*1.0;
etiene32 1:2f8deb2a7fe1 216 }
etiene32 1:2f8deb2a7fe1 217 else{
etiene32 1:2f8deb2a7fe1 218 myPosition_DD.longitude = -(seconde/3600.0)-(minute_int/60.0)+degres*1.0;
etiene32 1:2f8deb2a7fe1 219 }
etiene32 1:2f8deb2a7fe1 220 }
nirnakern 0:4e088cbb2dbf 221 }
nirnakern 0:4e088cbb2dbf 222
etiene32 1:2f8deb2a7fe1 223 void f2 (){
etiene32 1:2f8deb2a7fe1 224 int stop = 0;
etiene32 1:2f8deb2a7fe1 225 int compteur=0;
etiene32 1:2f8deb2a7fe1 226 Thread::wait (1000);
etiene32 1:2f8deb2a7fe1 227 Thread::wait (2000);
etiene32 1:2f8deb2a7fe1 228 while (stop==0){
etiene32 1:2f8deb2a7fe1 229 Thread::wait (time_between);
etiene32 1:2f8deb2a7fe1 230 data_access.lock();
etiene32 1:2f8deb2a7fe1 231 conversion_decimal(myPosition.lat,1);
etiene32 1:2f8deb2a7fe1 232 conversion_decimal(myPosition.longitud,2);
etiene32 1:2f8deb2a7fe1 233 if(fabs(last_position.latitude-myPosition_DD.latitude)<sensibilite || fabs(last_position.longitude-myPosition_DD.longitude)<sensibilite){
etiene32 1:2f8deb2a7fe1 234 //printf("Meme position \r\n");
etiene32 1:2f8deb2a7fe1 235 compteur++;
etiene32 1:2f8deb2a7fe1 236 }
etiene32 1:2f8deb2a7fe1 237 else{
etiene32 1:2f8deb2a7fe1 238 last_position.latitude=myPosition_DD.latitude;
etiene32 1:2f8deb2a7fe1 239 last_position.longitude=myPosition_DD.longitude;
etiene32 1:2f8deb2a7fe1 240 compteur=0;
etiene32 1:2f8deb2a7fe1 241 }
etiene32 1:2f8deb2a7fe1 242 data_access.unlock();
etiene32 1:2f8deb2a7fe1 243 if(compteur==nbre_iteration){
etiene32 1:2f8deb2a7fe1 244 stop=1;
etiene32 1:2f8deb2a7fe1 245 }
etiene32 1:2f8deb2a7fe1 246 }
etiene32 1:2f8deb2a7fe1 247 if(fabs(last_position.latitude-last_iteration_position.latitude)>sensibilite || fabs(last_position.longitude-last_iteration_position.longitude)>sensibilite){
etiene32 1:2f8deb2a7fe1 248 last_iteration_position.latitude = last_position.latitude;
etiene32 1:2f8deb2a7fe1 249 last_iteration_position.longitude = last_position.longitude;
etiene32 1:2f8deb2a7fe1 250 }
etiene32 1:2f8deb2a7fe1 251 if(unite==1){ //DM
etiene32 1:2f8deb2a7fe1 252 decimal_to_DM(last_iteration_position.latitude, last_iteration_position.longitude);
etiene32 1:2f8deb2a7fe1 253 printf("Votre position est : \r\n\t latitude : %dd %fm %c\r\n\t longitude : %dd %fm %c\r\n",myPosition_DM.degres_lat,myPosition_DM.minute_lat,myPosition_DM.lat_card,myPosition_DM.degres_lon,myPosition_DM.minute_lon, myPosition_DM.lon_card);
etiene32 1:2f8deb2a7fe1 254 }
etiene32 1:2f8deb2a7fe1 255 if(unite==2){ //DMS
etiene32 1:2f8deb2a7fe1 256 decimal_to_DMS(last_iteration_position.latitude, last_iteration_position.longitude);
etiene32 1:2f8deb2a7fe1 257 printf("Votre position est : \r\n\t latitude : %dd %dm %fs %c \r\n\t longitude : %dd %dm %fs %c \r\n",myPosition_DMS.degres_lat,myPosition_DMS.minute_lat,myPosition_DMS.seconde_lat,myPosition_DMS.lat_card,myPosition_DMS.degres_lon,myPosition_DMS.minute_lon, myPosition_DMS.seconde_lon, myPosition_DMS.lon_card);
etiene32 1:2f8deb2a7fe1 258 }
etiene32 1:2f8deb2a7fe1 259 if(unite==3){ //Décimal
etiene32 1:2f8deb2a7fe1 260 printf("Votre position est : \r\n\t latitude : %f \r\n\t longitude : %f \r\n",last_iteration_position.latitude, last_iteration_position.longitude);
etiene32 1:2f8deb2a7fe1 261 }
etiene32 1:2f8deb2a7fe1 262 }
nirnakern 0:4e088cbb2dbf 263
etiene32 1:2f8deb2a7fe1 264 void Start_GPS(){
etiene32 1:2f8deb2a7fe1 265 printf(" Start !\r\n");
etiene32 1:2f8deb2a7fe1 266 t1.start(f1);
etiene32 1:2f8deb2a7fe1 267 t2.start(f2);
etiene32 1:2f8deb2a7fe1 268 }
nirnakern 0:4e088cbb2dbf 269
nirnakern 0:4e088cbb2dbf 270 int main() {
etiene32 1:2f8deb2a7fe1 271 Start_GPS();
etiene32 1:2f8deb2a7fe1 272 }