main.cpp

00001 #include "mbed.h"
00002 #include "rtos/rtos.h"
00003 #include "Teseo-LIV3F.h"
00004 #include <cstdlib>
00005 #include <vector>
00006 #include "XNucleoIKS01A2.h"
00007 
00008 /*cose per gps*/
00009 static Thread t1;
00010 static Thread t2;
00011 
00012 static Mutex data_access;
00013 
00014 static Timer system_timer;
00015 static Serial usb_serial(USBTX, USBRX, 9600);
00016 static Serial *serial_debug= &usb_serial;
00017 
00018 
00019 static Teseo_LIV3F Teseo(D8,       D13,        D6,      PE_7,          PE_8,          &usb_serial);
00020 
00021 
00022 struct gps_coordinate{
00023     float lat;
00024     char lat_cardinal;
00025     float longitud;
00026     char long_cardinal;
00027     bool valid;
00028     
00029     
00030     
00031     };
00032     
00033     gps_coordinate myPosition;
00034 
00035 
00036 
00037 /*cose per sensori*/
00038 
00039 static Thread t3;
00040 
00041 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
00042 
00043 
00044 /* Retrieve the composing elements of the expansion board */
00045 static LSM303AGRMagSensor *magnetometer = mems_expansion_board->magnetometer;
00046 static HTS221Sensor *hum_temp = mems_expansion_board->ht_sensor;
00047 static LPS22HBSensor *press_temp = mems_expansion_board->pt_sensor;
00048 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
00049 static LSM303AGRAccSensor *accelerometer = mems_expansion_board->accelerometer;
00050 
00051 void f1 (){
00052     
00053     enum nmea_msg_id id;
00054     
00055     
00056   
00057 
00058 
00059     Teseo.Reset(serial_debug);
00060     Teseo.startListener(serial_debug);
00061 
00062     while(1) {
00063         osEvent evt = Teseo.queue.get();
00064         if (evt.status == osEventMessage) {
00065             struct teseo_msg *message = (struct teseo_msg *)evt.value.p;
00066             if (message->len){
00067                 id = Teseo.MsgDetect(message->buf, message->len, serial_debug);
00068                 
00069            }
00070            
00071            if (id==1){
00072                
00073                
00074                
00075                
00076                //da qui converto il messaggio in coordinate//
00077                data_access.lock();
00078                
00079                vector<char> pos;
00080               
00081                
00082                //latitudine
00083              // printf ("latitudine=");
00084                
00085                int cursor;
00086                for (int i=7;;i++){
00087                    cursor=i;
00088                    if (message->buf[i] == ',')
00089                         break;
00090                    pos.push_back(message->buf[i]);
00091                   
00092                   
00093                 }
00094             
00095        
00096                 //ma quanto è cool? conversione da vector ad array e poi a float
00097                 myPosition.lat=  atof (&pos[0]);
00098             //    printf ("%f",myPosition.lat);
00099              //   printf ("  ");
00100                 
00101                 myPosition.lat_cardinal= message->buf[cursor+1];
00102               //  printf ("%c   ",myPosition.lat_cardinal);
00103                 
00104                 pos.clear();
00105                 
00106                 
00107               //  printf ("longitudine=");
00108                
00109                
00110                for (int i=cursor+3;;i++){
00111                    cursor=i;
00112                    if (message->buf[i] == ',')
00113                         break;
00114                    pos.push_back(message->buf[i]);
00115                   
00116                   
00117                 }
00118             
00119        
00120                 
00121                 myPosition.longitud=  atof (&pos[0]);
00122               //  printf ("%f",myPosition.longitud);
00123               //  printf ("  ");
00124                 
00125                 myPosition.long_cardinal= message->buf[cursor+1];
00126                // printf ("%c   ",myPosition.long_cardinal);
00127                 
00128                 for (int i=cursor+3;;i++){
00129                    cursor=i;
00130                    if (message->buf[i] == 'A' ){
00131                         myPosition.valid=true;
00132                         break;
00133                     }else if (message->buf[i] == 'V' ) {
00134                         myPosition.valid=false;
00135                         break;
00136                                                 
00137                     }
00138                    
00139                   
00140                   
00141                 }
00142                 
00143                 if (myPosition.valid==true){
00144                   // printf ("Valid");
00145                    }
00146                    
00147                    
00148                 if (myPosition.valid==false){
00149                    //printf ("Not Valid");
00150                    }
00151                
00152                
00153                data_access.unlock();
00154                
00155               // printf ("\n\r");
00156                
00157                }
00158            
00159 
00160             Teseo.mpool.free(message);
00161         }
00162 
00163     }
00164     
00165 }
00166 
00167 
00168 
00169 void f2 (){
00170    Thread::wait (1000);
00171  
00172    Thread::wait (2000);
00173    while (true){ 
00174    Thread::wait (3000);
00175         data_access.lock();
00176     
00177         printf ("lat:%f %c, lon:%f %c, %d\n\r",myPosition.lat,myPosition.lat_cardinal, myPosition.longitud, myPosition.long_cardinal, myPosition.valid );
00178     
00179     
00180     
00181         data_access.unlock();
00182     }
00183     
00184 }
00185 
00186 
00187 void accgyro_thread(){
00188     
00189     uint8_t id;
00190   float value1, value2;
00191   char buffer1[32], buffer2[32];
00192   int32_t axes[3];
00193   int32_t axesOld[3];
00194   float odr =208.0f;
00195   
00196   
00197   /* Enable all sensors */
00198   
00199   accelerometer->enable();
00200   acc_gyro->enable_x();
00201   acc_gyro->enable_g();
00202   acc_gyro->set_g_odr (odr);
00203   
00204   while (true){
00205     
00206     Thread::wait (5000);
00207     
00208     
00209     acc_gyro->get_g_axes(axes);
00210     printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
00211     
00212     
00213     acc_gyro->get_x_axes(axes);
00214    printf("LSM6DSL [acc/mg]:      %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
00215 
00216     
00217     
00218     }
00219     
00220     
00221 }
00222 
00223 
00224 
00225 
00226 int main() {
00227     
00228     t1.start(f1);
00229    
00230     t2.start (f2);
00231     
00232     t3.start (accgyro_thread);
00233     
00234     
00235     
00236     
00237 }