Diff: main.cpp

Revision:

0:4e088cbb2dbf

Child:

1:2f8deb2a7fe1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 23 07:11:35 2017 +0000
@@ -0,0 +1,237 @@
+#include "mbed.h"
+#include "rtos/rtos.h"
+#include "Teseo-LIV3F.h"
+#include <cstdlib>
+#include <vector>
+#include "XNucleoIKS01A2.h"
+
+/*cose per gps*/
+static Thread t1;
+static Thread t2;
+
+static Mutex data_access;
+
+static Timer system_timer;
+static Serial usb_serial(USBTX, USBRX, 9600);
+static Serial *serial_debug= &usb_serial;
+
+
+static Teseo_LIV3F Teseo(D8,       D13,        D6,      PE_7,          PE_8,          &usb_serial);
+
+
+struct gps_coordinate{
+    float lat;
+    char lat_cardinal;
+    float longitud;
+    char long_cardinal;
+    bool valid;
+    
+    
+    
+    };
+    
+    gps_coordinate myPosition;
+
+
+
+/*cose per sensori*/
+
+static Thread t3;
+
+static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
+
+
+/* Retrieve the composing elements of the expansion board */
+static LSM303AGRMagSensor *magnetometer = mems_expansion_board->magnetometer;
+static HTS221Sensor *hum_temp = mems_expansion_board->ht_sensor;
+static LPS22HBSensor *press_temp = mems_expansion_board->pt_sensor;
+static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
+static LSM303AGRAccSensor *accelerometer = mems_expansion_board->accelerometer;
+
+void f1 (){
+    
+    enum nmea_msg_id id;
+    
+    
+  
+
+
+    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 f2 (){
+   Thread::wait (1000);
+ 
+   Thread::wait (2000);
+   while (true){ 
+   Thread::wait (3000);
+        data_access.lock();
+    
+        printf ("lat:%f %c, lon:%f %c, %d\n\r",myPosition.lat,myPosition.lat_cardinal, myPosition.longitud, myPosition.long_cardinal, myPosition.valid );
+    
+    
+    
+        data_access.unlock();
+    }
+    
+}
+
+
+void accgyro_thread(){
+    
+    uint8_t id;
+  float value1, value2;
+  char buffer1[32], buffer2[32];
+  int32_t axes[3];
+  int32_t axesOld[3];
+  float odr =208.0f;
+  
+  
+  /* Enable all sensors */
+  
+  accelerometer->enable();
+  acc_gyro->enable_x();
+  acc_gyro->enable_g();
+  acc_gyro->set_g_odr (odr);
+  
+  while (true){
+    
+    Thread::wait (5000);
+    
+    
+    acc_gyro->get_g_axes(axes);
+    printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
+    
+    
+    acc_gyro->get_x_axes(axes);
+   printf("LSM6DSL [acc/mg]:      %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
+
+    
+    
+    }
+    
+    
+}
+
+
+
+
+int main() {
+    
+    t1.start(f1);
+   
+    t2.start (f2);
+    
+    t3.start (accgyro_thread);
+    
+    
+    
+    
+}