etienne maillet / Mbed 2 deprecated GNSS1A1_sansmenu

hue

Dependencies:   mbed mbed-rtos

main.cpp@1:d6e2c8220523, 2019-02-19 (annotated)

Committer:
etiene32
Date:
Tue Feb 19 13:12:21 2019 +0000
Revision:
1:d6e2c8220523
Parent:
0:4e088cbb2dbf 
test

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:d6e2c8220523 6 #include <sstream>
etiene32 1:d6e2c8220523 7 #include <stdio.h>
etiene32 1:d6e2c8220523 8
etiene32 1:d6e2c8220523 9 int time_between=3000; //Temps entre 2 mesures
etiene32 1:d6e2c8220523 10 int unite=3; // 1 = DM ; 2 = DMS ; 3 = Décimal
etiene32 1:d6e2c8220523 11 float sensibilite=0.00001; //Valeur en dessous de laquelle on considere aucun changement de position
etiene32 1:d6e2c8220523 12 int nbre_iteration = 2; //nombre de meme position avant stoppage du gps car certain de la position
etiene32 1:d6e2c8220523 13
etiene32 1:d6e2c8220523 14 //Def fonction
etiene32 1:d6e2c8220523 15 void Start_GPS();
etiene32 1:d6e2c8220523 16 void f1 ();
etiene32 1:d6e2c8220523 17 void f2 ();
etiene32 1:d6e2c8220523 18 void conversion_decimal(float coord, int indice);
etiene32 1:d6e2c8220523 19 void decimal_to_DM(float latitude, float longitude);
etiene32 1:d6e2c8220523 20 void decimal_to_DMS(float latitude, float longitude);
etiene32 1:d6e2c8220523 21
etiene32 1:d6e2c8220523 22 char buffer[20];
etiene32 1:d6e2c8220523 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:d6e2c8220523 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:d6e2c8220523 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:d6e2c8220523 44 bool valid;
nirnakern 0:4e088cbb2dbf 45 };
nirnakern 0:4e088cbb2dbf 46
etiene32 1:d6e2c8220523 47 struct gps_DM_coordinate{
etiene32 1:d6e2c8220523 48 int degres_lat;
etiene32 1:d6e2c8220523 49 float minute_lat;
etiene32 1:d6e2c8220523 50 char lat_card;
etiene32 1:d6e2c8220523 51 int degres_lon;
etiene32 1:d6e2c8220523 52 float minute_lon;
etiene32 1:d6e2c8220523 53 char lon_card;
etiene32 1:d6e2c8220523 54 };
nirnakern 0:4e088cbb2dbf 55
etiene32 1:d6e2c8220523 56 struct gps_DMS_coordinate{
etiene32 1:d6e2c8220523 57 int degres_lat;
etiene32 1:d6e2c8220523 58 int minute_lat;
etiene32 1:d6e2c8220523 59 float seconde_lat;
etiene32 1:d6e2c8220523 60 char lat_card;
etiene32 1:d6e2c8220523 61 int degres_lon;
etiene32 1:d6e2c8220523 62 int minute_lon;
etiene32 1:d6e2c8220523 63 float seconde_lon;
etiene32 1:d6e2c8220523 64 char lon_card;
etiene32 1:d6e2c8220523 65 };
nirnakern 0:4e088cbb2dbf 66
etiene32 1:d6e2c8220523 67 struct gps_DD_coordinate{
etiene32 1:d6e2c8220523 68 float latitude;
etiene32 1:d6e2c8220523 69 float longitude;
etiene32 1:d6e2c8220523 70 };
etiene32 1:d6e2c8220523 71
etiene32 1:d6e2c8220523 72 struct last_coordinate{
etiene32 1:d6e2c8220523 73 float latitude;
etiene32 1:d6e2c8220523 74 float longitude;
etiene32 1:d6e2c8220523 75 };
etiene32 1:d6e2c8220523 76
etiene32 1:d6e2c8220523 77 gps_coordinate myPosition;
etiene32 1:d6e2c8220523 78
etiene32 1:d6e2c8220523 79 gps_DM_coordinate myPosition_DM;
etiene32 1:d6e2c8220523 80 gps_DMS_coordinate myPosition_DMS;
etiene32 1:d6e2c8220523 81 gps_DD_coordinate myPosition_DD;
etiene32 1:d6e2c8220523 82 last_coordinate last_position;
etiene32 1:d6e2c8220523 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:d6e2c8220523 87 last_position.latitude=0;
etiene32 1:d6e2c8220523 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:d6e2c8220523 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:d6e2c8220523 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:d6e2c8220523 109 pos.push_back(message->buf[i]);
etiene32 1:d6e2c8220523 110 }
etiene32 1:d6e2c8220523 111 //ma quanto è cool? conversione da vector ad array e poi a float
etiene32 1:d6e2c8220523 112 myPosition.lat= atof (&pos[0]);
etiene32 1:d6e2c8220523 113 //printf ("%f",myPosition.lat);
etiene32 1:d6e2c8220523 114 //printf (" ");
etiene32 1:d6e2c8220523 115 myPosition.lat_cardinal= message->buf[cursor+1];
etiene32 1:d6e2c8220523 116 //printf ("%c ",myPosition.lat_cardinal);
etiene32 1:d6e2c8220523 117 pos.clear();
etiene32 1:d6e2c8220523 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:d6e2c8220523 123 pos.push_back(message->buf[i]);
nirnakern 0:4e088cbb2dbf 124 }
nirnakern 0:4e088cbb2dbf 125 myPosition.longitud= atof (&pos[0]);
etiene32 1:d6e2c8220523 126 //printf ("%f",myPosition.longitud);
etiene32 1:d6e2c8220523 127 //printf (" ");
nirnakern 0:4e088cbb2dbf 128 myPosition.long_cardinal= message->buf[cursor+1];
etiene32 1:d6e2c8220523 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:d6e2c8220523 137 break;
etiene32 1:d6e2c8220523 138 }
nirnakern 0:4e088cbb2dbf 139 }
nirnakern 0:4e088cbb2dbf 140 if (myPosition.valid==true){
etiene32 1:d6e2c8220523 141 //printf ("Valid");
etiene32 1:d6e2c8220523 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:d6e2c8220523 154 void decimal_to_DM(float latitude, float longitude){
etiene32 1:d6e2c8220523 155 int degres;
etiene32 1:d6e2c8220523 156 float minutes;
etiene32 1:d6e2c8220523 157 //Latitude
etiene32 1:d6e2c8220523 158 degres = latitude;
etiene32 1:d6e2c8220523 159 minutes = (latitude - degres)*60;
etiene32 1:d6e2c8220523 160 myPosition_DM.degres_lat = degres;
etiene32 1:d6e2c8220523 161 myPosition_DM.minute_lat = minutes;
etiene32 1:d6e2c8220523 162 myPosition_DM.lat_card = myPosition.lat_cardinal;
etiene32 1:d6e2c8220523 163 //Longitude
etiene32 1:d6e2c8220523 164 degres = longitude;
etiene32 1:d6e2c8220523 165 minutes = (longitude - degres)*60;
etiene32 1:d6e2c8220523 166 myPosition_DM.degres_lon = degres;
etiene32 1:d6e2c8220523 167 myPosition_DM.minute_lon = minutes;
etiene32 1:d6e2c8220523 168 myPosition_DM.lon_card = myPosition.long_cardinal;
etiene32 1:d6e2c8220523 169 }
nirnakern 0:4e088cbb2dbf 170
etiene32 1:d6e2c8220523 171 void decimal_to_DMS(float latitude, float longitude){
etiene32 1:d6e2c8220523 172 int degres, minutes;
etiene32 1:d6e2c8220523 173 float seconde, minutes_float;
etiene32 1:d6e2c8220523 174 //Latitude
etiene32 1:d6e2c8220523 175 degres = latitude;
etiene32 1:d6e2c8220523 176 minutes_float = (latitude - degres)*60;
etiene32 1:d6e2c8220523 177 minutes = minutes_float;
etiene32 1:d6e2c8220523 178 seconde = (minutes_float - minutes) * 60;
etiene32 1:d6e2c8220523 179 myPosition_DMS.degres_lat = degres;
etiene32 1:d6e2c8220523 180 myPosition_DMS.minute_lat = minutes;
etiene32 1:d6e2c8220523 181 myPosition_DMS.seconde_lat = seconde;
etiene32 1:d6e2c8220523 182 myPosition_DMS.lat_card = myPosition.lat_cardinal;
etiene32 1:d6e2c8220523 183 //Longitude
etiene32 1:d6e2c8220523 184 degres = longitude;
etiene32 1:d6e2c8220523 185 minutes_float = (longitude - degres)*60;
etiene32 1:d6e2c8220523 186 minutes = minutes_float;
etiene32 1:d6e2c8220523 187 seconde = (minutes_float - minutes) * 60;
etiene32 1:d6e2c8220523 188 myPosition_DMS.degres_lon = degres;
etiene32 1:d6e2c8220523 189 myPosition_DMS.minute_lon = minutes;
etiene32 1:d6e2c8220523 190 myPosition_DMS.seconde_lon = seconde;
etiene32 1:d6e2c8220523 191 myPosition_DMS.lon_card = myPosition.long_cardinal;
nirnakern 0:4e088cbb2dbf 192 }
nirnakern 0:4e088cbb2dbf 193
etiene32 1:d6e2c8220523 194 void conversion_decimal(float coord, int indice){ //indice -> 1=latitude 2=longitude
etiene32 1:d6e2c8220523 195 int degres, minute_int;
etiene32 1:d6e2c8220523 196 double minute, coord_d, seconde;
etiene32 1:d6e2c8220523 197 //Séparation degres, minute, seconde mathématiquement
etiene32 1:d6e2c8220523 198 coord_d = coord / 100.0;
etiene32 1:d6e2c8220523 199 degres = coord_d; //float vers int, on garde donc que la partie décimale donc degres ici
etiene32 1:d6e2c8220523 200 minute = coord_d - degres;
etiene32 1:d6e2c8220523 201 minute = minute * 100.0;
etiene32 1:d6e2c8220523 202 minute_int = minute;
etiene32 1:d6e2c8220523 203 seconde = (minute-minute_int)*60;
etiene32 1:d6e2c8220523 204 //Calcul position decimal (toujours calculer pour comparaison avec last_position)
etiene32 1:d6e2c8220523 205 if(indice==1){
etiene32 1:d6e2c8220523 206 if(degres>=0){
etiene32 1:d6e2c8220523 207 myPosition_DD.latitude = (seconde/3600.0)+(minute_int/60.0)+degres*1.0;
etiene32 1:d6e2c8220523 208 }
etiene32 1:d6e2c8220523 209 else{
etiene32 1:d6e2c8220523 210 myPosition_DD.latitude = -(seconde/3600.0)-(minute_int/60.0)+degres*1.0;
etiene32 1:d6e2c8220523 211 }
nirnakern 0:4e088cbb2dbf 212 }
etiene32 1:d6e2c8220523 213 if(indice==2){
etiene32 1:d6e2c8220523 214 if(degres>=0){
etiene32 1:d6e2c8220523 215 myPosition_DD.longitude = (seconde/3600.0)+(minute_int/60.0)+degres*1.0;
etiene32 1:d6e2c8220523 216 }
etiene32 1:d6e2c8220523 217 else{
etiene32 1:d6e2c8220523 218 myPosition_DD.longitude = -(seconde/3600.0)-(minute_int/60.0)+degres*1.0;
etiene32 1:d6e2c8220523 219 }
etiene32 1:d6e2c8220523 220 }
nirnakern 0:4e088cbb2dbf 221 }
nirnakern 0:4e088cbb2dbf 222
etiene32 1:d6e2c8220523 223 void f2 (){
etiene32 1:d6e2c8220523 224 int stop = 0;
etiene32 1:d6e2c8220523 225 int compteur=0;
etiene32 1:d6e2c8220523 226 Thread::wait (1000);
etiene32 1:d6e2c8220523 227 Thread::wait (2000);
etiene32 1:d6e2c8220523 228 while (stop==0){
etiene32 1:d6e2c8220523 229 Thread::wait (time_between);
etiene32 1:d6e2c8220523 230 data_access.lock();
etiene32 1:d6e2c8220523 231 conversion_decimal(myPosition.lat,1);
etiene32 1:d6e2c8220523 232 conversion_decimal(myPosition.longitud,2);
etiene32 1:d6e2c8220523 233 if(abs(last_position.latitude-myPosition_DD.latitude)<sensibilite || abs(last_position.longitude-myPosition_DD.longitude)<sensibilite){
etiene32 1:d6e2c8220523 234 //printf("Meme position \r\n");
etiene32 1:d6e2c8220523 235 compteur++;
etiene32 1:d6e2c8220523 236 }
etiene32 1:d6e2c8220523 237 else{
etiene32 1:d6e2c8220523 238 last_position.latitude=myPosition_DD.latitude;
etiene32 1:d6e2c8220523 239 last_position.longitude=myPosition_DD.longitude;
etiene32 1:d6e2c8220523 240 compteur=0;
etiene32 1:d6e2c8220523 241 }
etiene32 1:d6e2c8220523 242 data_access.unlock();
etiene32 1:d6e2c8220523 243 if(compteur==nbre_iteration){
etiene32 1:d6e2c8220523 244 stop=1;
etiene32 1:d6e2c8220523 245 }
etiene32 1:d6e2c8220523 246 }
etiene32 1:d6e2c8220523 247 if(abs(last_position.latitude-last_iteration_position.latitude)>sensibilite || abs(last_position.longitude-last_iteration_position.longitude)>sensibilite){
etiene32 1:d6e2c8220523 248 last_iteration_position.latitude = last_position.latitude;
etiene32 1:d6e2c8220523 249 last_iteration_position.longitude = last_position.longitude;
etiene32 1:d6e2c8220523 250 }
etiene32 1:d6e2c8220523 251 if(unite==1){ //DM
etiene32 1:d6e2c8220523 252 decimal_to_DM(last_iteration_position.latitude, last_iteration_position.longitude);
etiene32 1:d6e2c8220523 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:d6e2c8220523 254 }
etiene32 1:d6e2c8220523 255 if(unite==2){ //DMS
etiene32 1:d6e2c8220523 256 decimal_to_DMS(last_iteration_position.latitude, last_iteration_position.longitude);
etiene32 1:d6e2c8220523 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:d6e2c8220523 258 }
etiene32 1:d6e2c8220523 259 if(unite==3){ //Décimal
etiene32 1:d6e2c8220523 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:d6e2c8220523 261 }
etiene32 1:d6e2c8220523 262 }
nirnakern 0:4e088cbb2dbf 263
etiene32 1:d6e2c8220523 264 void Start_GPS(){
etiene32 1:d6e2c8220523 265 printf(" Start !\r\n");
etiene32 1:d6e2c8220523 266 t1.start(f1);
etiene32 1:d6e2c8220523 267 t2.start(f2);
etiene32 1:d6e2c8220523 268 }
nirnakern 0:4e088cbb2dbf 269
nirnakern 0:4e088cbb2dbf 270 int main() {
etiene32 1:d6e2c8220523 271 Start_GPS();
etiene32 1:d6e2c8220523 272 }