Diff: main.cpp

Revision:

1:ef1bbf9b6205

Parent:

0:4e088cbb2dbf

Child:

2:1eb73fe8ab8f

--- a/main.cpp	Mon Oct 23 07:11:35 2017 +0000
+++ b/main.cpp	Fri Jan 26 09:52:12 2018 +0000
@@ -1,20 +1,50 @@
+/*QUESTA è LA VERSIONE PER IL GPS SU UART*/
+
 #include "mbed.h"
 #include "rtos/rtos.h"
 #include "Teseo-LIV3F.h"
 #include <cstdlib>
 #include <vector>
 #include "XNucleoIKS01A2.h"
+#include <algorithm>
+#include "EthernetInterface.h"
+#include "NTPClient.h"
+
+
+/*IP E AFFINI*/
+
+const char * IP = "169.254.112.125";
+const char * MASK = "255.255.255.0";
+const char * GATEWAY = "169.254.112.1";
+
+
+
+/*cose imu*/
+
+#define ECHO_SERVER_PORT   7  
+
+static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
+Timer t;
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+
+
 
 /*cose per gps*/
 static Thread t1;
 static Thread t2;
 
-static Mutex data_access;
+static Mutex message_access;
 
 static Timer system_timer;
 static Serial usb_serial(USBTX, USBRX, 9600);
 static Serial *serial_debug= &usb_serial;
 
+static std::vector<int> ids;
+
+
 
 static Teseo_LIV3F Teseo(D8,       D13,        D6,      PE_7,          PE_8,          &usb_serial);
 
@@ -38,127 +68,70 @@
 
 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;
+
+
+/*THIS FUNCTION IS SIMPLY USED TO PRINT ON SERIAL THE NMEA MSG*/
+/*
+void print_function (enum nmea_msg_id id, char *buf, int len){
+    
+    if (id == NMEA_END__) {
+        puts("Not found\n\r");
+        return;
+    }
 
-void f1 (){
+    if (!len) {
+        puts("Not valid\n\r");
+        return;
+    }
+    printf("notify: %d " , id);
+    for (int i = 0; i < len; ++i)
+        printf("%c",buf[i]);
+    puts("\n\r");
+    
+    
+    
+}
+*/
+/*THIS FUNCTION PRINT ONLY THE MESSAGE WITH ID SAVED IN STATIC VARIALBE ids*/
+/*
+void getSpectificMsg (){
     
     enum nmea_msg_id id;
     
-    
-  
-
-
     Teseo.Reset(serial_debug);
     Teseo.startListener(serial_debug);
 
     while(1) {
+        
         osEvent evt = Teseo.queue.get();
-        if (evt.status == osEventMessage) {
+        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 ( find(ids.begin(), ids.end(), id) != ids.end() ){
+                print_function (id, message->buf, message->len);
+            }
            
-           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=");
+            Teseo.mpool.free(message);
+           
+        }           
                
                
-               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);
-        }
+              
 
     }
     
@@ -166,72 +139,109 @@
 
 
 
-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);
+  
+    
+    //cpose per comunicazioni
+
+     
+   
+    
+    EthernetInterface eth;
+    eth.init(IP,MASK,GATEWAY); //Use DHCP
+    eth.connect();
+    printf("IP Address is %s\n", eth.getIPAddress());
+    
    
-    t2.start (f2);
+  
+      
+    UDPSocket server;
+    server.bind(ECHO_SERVER_PORT);
+
+    
+    Endpoint client;
+    char buffer[256];
+    char outBuffer[256];
+    int32_t  packetnumber =0;
+    
+     
+    NTPClient ntp;
+    ntp.setTime("169.254.92.70", 123);
+
+       
+     time_t ctTime;
+     ctTime = time(NULL);
+     printf("Time is set to (UTC): %s\r",  ctime(&ctTime));
+    
+    while(1) {
+
+        printf("\nWaiting for UDP packet...\n");
+        
+        myled1=1;
+        int n = server.receiveFrom(client, buffer, sizeof(buffer));
+        buffer[n] = '\0';
+        
+        printf("Received packet from: %s\n", client.get_address());
+        printf("Packet contents : '%s'\n",buffer);
+        
+        
+        //set time
+        NTPClient ntp;
+        ntp.setTime(client.get_address(), 123,10000);
     
-    t3.start (accgyro_thread);
-    
+       
+        time_t ctTime;
+        ctTime = time(NULL);
+        printf("Time is set to (UTC): %s\r",  ctime(&ctTime));
+         time_t offset = time(NULL);
+        t.start();
+        
+        
+        printf("Start Sending GPS messages\n");
+        
+        myled2=1;
+        
+        myled3=1;
+        server.sendTo(client, buffer, n);
+        
+        
+        memset (&buffer[0], '\0', sizeof(buffer));
+            while (1){
+                packetnumber =packetnumber+1;
+                //send imu
+                
+                if (Teseo._uart.readable()) {
+                    int c = Teseo._uart.getc();
+                    sprintf (buffer + strlen(buffer), "%c", c);
+                    serial_debug->putc(c);
+                    if (c =='\n'){
+                       // sprintf (outBuffer , "%d,%s",(t.read_ms()+(offset*1000)),buffer);
+                         sprintf (outBuffer , "%s",buffer);
+                        
+                        n =sizeof (outBuffer);
+                        server.sendTo(client, outBuffer, n);
+                        memset (&buffer[0], '\0', sizeof(buffer)); 
+                        memset (&outBuffer[0], '\0', sizeof(outBuffer));
+                        
+                        
+                        }
+                }
+                
+                
+                
+               
+                //printf (buffer);
+                
+                //wait (1);
+                }
+   
+     
     
-    
-    
+        
+        }
 }