hue

Dependencies:   mbed mbed-rtos

main.cpp

Committer:
etiene32
Date:
2019-02-19
Revision:
1:d6e2c8220523
Parent:
0:4e088cbb2dbf

File content as of revision 1:d6e2c8220523:

#include "mbed.h"
#include "rtos/rtos.h"
#include "Teseo-LIV3F.h"
#include <cstdlib>
#include <vector>
#include <sstream>
#include <stdio.h> 

int time_between=3000; //Temps entre 2 mesures
int unite=3; // 1 = DM ; 2 = DMS ; 3 = Décimal
float sensibilite=0.00001; //Valeur en dessous de laquelle on considere aucun changement de position 
int nbre_iteration = 2; //nombre de meme position avant stoppage du gps car certain de la position

//Def fonction
void Start_GPS();
void f1 ();
void f2 ();
void conversion_decimal(float coord, int indice);
void decimal_to_DM(float latitude, float longitude);
void decimal_to_DMS(float latitude, float longitude);

char buffer[20];
char buffer1[20];

/*cose per gps*/
static Thread t1;
static Thread t2;

static Mutex data_access;

static Timer system_timer;
static Serial usb_serial(USBTX, USBRX, 115200);
static Serial *serial_debug= &usb_serial;


static Teseo_LIV3F Teseo(D7,D13,D6,D2,D8,&usb_serial);


struct gps_coordinate{
    float lat;
    char lat_cardinal;
    float longitud;
    char long_cardinal;
    bool valid;   
    };

struct gps_DM_coordinate{
   int degres_lat;
   float minute_lat;
   char lat_card;
   int degres_lon;
   float minute_lon;
   char lon_card;
};

struct gps_DMS_coordinate{
   int degres_lat;
   int minute_lat;
   float seconde_lat;
   char lat_card;
   int degres_lon;
   int minute_lon;
   float seconde_lon;
   char lon_card;
};

struct gps_DD_coordinate{
   float latitude;
   float longitude;
};

struct last_coordinate{
   float latitude;
   float longitude;
};
   
gps_coordinate myPosition;

gps_DM_coordinate myPosition_DM;
gps_DMS_coordinate myPosition_DMS;
gps_DD_coordinate myPosition_DD;
last_coordinate last_position;
last_coordinate last_iteration_position;

void f1 (){
    enum nmea_msg_id id;
    last_position.latitude=0;
    last_position.longitude=0;
    Teseo.Reset(serial_debug);
    Teseo.startListener(serial_debug);
    while(1) {
        osEvent evt = Teseo.queue.get();
        if (evt.status == osEventMessage) {
            struct teseo_msg *message = (struct teseo_msg *)evt.value.p;
            if (message->len){
                id = Teseo.MsgDetect(message->buf, message->len, serial_debug); 
           }
           if (id==1){
               //da qui converto il messaggio in coordinate//
               data_access.lock();
               vector<char> pos;
               //latitudine
               //printf ("latitudine=");
               int cursor;
               for (int i=7;;i++){
                   cursor=i;
                   if (message->buf[i] == ',')
                        break;
                   pos.push_back(message->buf[i]);  
               }
               //ma quanto è cool? conversione da vector ad array e poi a float
               myPosition.lat=  atof (&pos[0]);
               //printf ("%f",myPosition.lat);
               //printf ("  ");
               myPosition.lat_cardinal= message->buf[cursor+1];
               //printf ("%c   ",myPosition.lat_cardinal);
               pos.clear();
               //printf ("longitudine=");
               for (int i=cursor+3;;i++){
                   cursor=i;
                   if (message->buf[i] == ',')
                        break;
                   pos.push_back(message->buf[i]); 
                }
                myPosition.longitud=  atof (&pos[0]);
                //printf ("%f",myPosition.longitud);
                //printf ("  ");              
                myPosition.long_cardinal= message->buf[cursor+1];
                // printf ("%c   ",myPosition.long_cardinal);            
                for (int i=cursor+3;;i++){
                   cursor=i;
                   if (message->buf[i] == 'A' ){
                        myPosition.valid=true;
                        break;
                    }else if (message->buf[i] == 'V' ) {
                        myPosition.valid=false;
                        break;                                      
                    }          
                }
                if (myPosition.valid==true){
                   //printf ("Valid");
                   }   
                if (myPosition.valid==false){
                   //printf ("Not Valid");
                   }
               data_access.unlock();
              // printf ("\n\r");
               }
            Teseo.mpool.free(message);
        }
    }
}

void decimal_to_DM(float latitude, float longitude){
    int degres;
    float minutes;
    //Latitude
    degres = latitude;
    minutes = (latitude - degres)*60;
    myPosition_DM.degres_lat = degres;
    myPosition_DM.minute_lat = minutes;
    myPosition_DM.lat_card = myPosition.lat_cardinal;
    //Longitude
    degres = longitude;
    minutes = (longitude - degres)*60;
    myPosition_DM.degres_lon = degres;
    myPosition_DM.minute_lon = minutes;
    myPosition_DM.lon_card = myPosition.long_cardinal;
}

void decimal_to_DMS(float latitude, float longitude){
    int degres, minutes;
    float seconde, minutes_float;
    //Latitude
    degres = latitude;
    minutes_float = (latitude - degres)*60;
    minutes = minutes_float;
    seconde = (minutes_float - minutes) * 60;
    myPosition_DMS.degres_lat = degres;
    myPosition_DMS.minute_lat = minutes;
    myPosition_DMS.seconde_lat = seconde;
    myPosition_DMS.lat_card = myPosition.lat_cardinal;
    //Longitude
    degres = longitude;
    minutes_float = (longitude - degres)*60;
    minutes = minutes_float;
    seconde = (minutes_float - minutes) * 60;
    myPosition_DMS.degres_lon = degres;
    myPosition_DMS.minute_lon = minutes;
    myPosition_DMS.seconde_lon = seconde;
    myPosition_DMS.lon_card = myPosition.long_cardinal;
}

void conversion_decimal(float coord, int indice){ //indice -> 1=latitude 2=longitude
    int degres, minute_int;
    double minute, coord_d, seconde;
    //Séparation degres, minute, seconde mathématiquement
    coord_d = coord / 100.0;
    degres = coord_d; //float vers int, on garde donc que la partie décimale donc degres ici
    minute = coord_d - degres;
    minute = minute * 100.0; 
    minute_int = minute;
    seconde = (minute-minute_int)*60;
    //Calcul position decimal (toujours calculer pour comparaison avec last_position)
    if(indice==1){
        if(degres>=0){
            myPosition_DD.latitude = (seconde/3600.0)+(minute_int/60.0)+degres*1.0;
        }
        else{
            myPosition_DD.latitude = -(seconde/3600.0)-(minute_int/60.0)+degres*1.0;
        }
    }
    if(indice==2){
        if(degres>=0){
            myPosition_DD.longitude = (seconde/3600.0)+(minute_int/60.0)+degres*1.0;
        }
        else{
            myPosition_DD.longitude = -(seconde/3600.0)-(minute_int/60.0)+degres*1.0;
        }
    }  
}

void f2 (){
   int stop = 0;
   int compteur=0;
   Thread::wait (1000); 
   Thread::wait (2000);
   while (stop==0){ 
        Thread::wait (time_between);
        data_access.lock();
        conversion_decimal(myPosition.lat,1);
        conversion_decimal(myPosition.longitud,2);
        if(abs(last_position.latitude-myPosition_DD.latitude)<sensibilite || abs(last_position.longitude-myPosition_DD.longitude)<sensibilite){
            //printf("Meme position \r\n");
            compteur++;
        }
        else{
            last_position.latitude=myPosition_DD.latitude;
            last_position.longitude=myPosition_DD.longitude;
            compteur=0;
        }
        data_access.unlock();
        if(compteur==nbre_iteration){
            stop=1;    
        }
    }
    if(abs(last_position.latitude-last_iteration_position.latitude)>sensibilite || abs(last_position.longitude-last_iteration_position.longitude)>sensibilite){
        last_iteration_position.latitude = last_position.latitude;
        last_iteration_position.longitude = last_position.longitude;
    }
    if(unite==1){ //DM
        decimal_to_DM(last_iteration_position.latitude, last_iteration_position.longitude);
        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);
    }
    if(unite==2){ //DMS
        decimal_to_DMS(last_iteration_position.latitude, last_iteration_position.longitude);    
        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);
    }
    if(unite==3){ //Décimal
        printf("Votre position est : \r\n\t latitude : %f \r\n\t longitude : %f \r\n",last_iteration_position.latitude, last_iteration_position.longitude); 
    }
}

void Start_GPS(){
    printf(" Start !\r\n");
    t1.start(f1);
    t2.start(f2);
}  

int main() {
    Start_GPS();       
}