main.cpp

00001 /*QUESTA è LA VERSIONE PER IMU SU Ethernet*/
00002 #include "mbed.h"
00003 #include "rtos.h"
00004 #include "EthernetInterface.h"
00005 #include "NTPClient.h"
00006 #include <stdio.h>
00007 #include "XNucleoIKS01A2.h"
00008 
00009 #define ECHO_SERVER_PORT   7  
00010 #define DEBUG 0
00011 #define RAW 0
00012 
00013 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
00014 
00015 DigitalOut myled1(LED1);
00016 DigitalOut myled2(LED2);
00017 DigitalOut myled3(LED3);
00018 int main() {
00019      
00020     //cose per accelerometro
00021     
00022     #if !RAW
00023     int32_t axes[3];
00024     int32_t axes2[3];
00025     #else 
00026     int16_t axes[3];
00027     int16_t axes2[3];
00028     #endif
00029     uint8_t id;
00030     
00031     static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
00032     acc_gyro->enable_x();
00033     acc_gyro->enable_g();
00034     acc_gyro->read_id(&id);
00035     printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
00036     
00037     acc_gyro->set_x_odr(208.0f);
00038     acc_gyro->set_g_odr(208.0f);
00039     acc_gyro->set_x_fs(2.0f);
00040     acc_gyro->set_g_fs(125.0f);
00041     
00042     //cpose per comunicazioni
00043     const char * IP = "192.168.1.205";
00044     const char * MASK = "255.255.255.0";
00045     const char * GATEWAY = "192.168.1.1";
00046      
00047    #if !DEBUG
00048     
00049     EthernetInterface eth;
00050     eth.init(IP,MASK,GATEWAY); //Use DHCP
00051     eth.connect();
00052     printf("IP Address is %s\n\r", eth.getIPAddress());
00053     
00054    
00055   
00056       
00057     UDPSocket server;
00058     server.bind(ECHO_SERVER_PORT);
00059 
00060     
00061     Endpoint client;
00062        
00063  #endif
00064     char buffer[256];
00065     int32_t  packetnumber =0;
00066     
00067  
00068     
00069     
00070     
00071   
00072      
00073             
00074      
00075           
00076     
00077     
00078     
00079     while(1) {
00080 
00081 #if !DEBUG
00082         printf("\nWaiting for UDP packet...\n");
00083         
00084         myled1=1;
00085         int n = server.receiveFrom(client, buffer, sizeof(buffer));
00086         buffer[n] = '\0';
00087         
00088         printf("Received packet from: %s\n\r", client.get_address());
00089         printf("Packet contents : '%s'\n\r",buffer);
00090         
00091                 
00092 
00093 #endif
00094     
00095         
00096         printf("Calibrating wait 30s\n");
00097         
00098         myled2=1;
00099         
00100         long offsetGX=0;
00101         long offsetGY=0;
00102         long offsetGZ=0;
00103         long offsetAX=0;
00104         long offsetAY=0;
00105         long offsetAZ=0;
00106         long counter=0;
00107         
00108         for (int i=0;i<4500;i++){
00109             #if !RAW
00110             acc_gyro->get_x_axes(axes);
00111             acc_gyro->get_g_axes(axes2);
00112             #else
00113             acc_gyro->get_x_axes_raw(axes);
00114             acc_gyro->get_g_axes_raw(axes2);
00115             
00116             #endif
00117              //offsetGX=offsetGX+ axes[0];
00118              //offsetGY=offsetGY+ axes[1];
00119              //offsetGZ=offsetGZ+ axes[2];
00120         
00121              offsetAX=offsetAX+ axes2[0];
00122              offsetAY=offsetAY+ axes2[1];
00123              offsetAZ=offsetAZ+ axes2[2];    
00124             counter++;
00125             
00126             
00127             
00128         }
00129         
00130         
00131          //offsetGX=offsetGX/counter;
00132          //offsetGY=offsetGY/counter;
00133          //offsetGZ=offsetGZ/counter;
00134          offsetAX=offsetAX/counter;
00135          offsetAY=offsetAY/counter;
00136          offsetAZ=offsetAZ/counter;
00137         
00138         
00139         
00140         
00141         
00142         printf("Start Sending Imu messages\n\r");
00143         
00144         myled3=1;
00145         #if !DEBUG
00146         server.sendTo(client, buffer, n);
00147         #endif
00148        
00149         
00150             while (1){
00151                 #if !DEBUG
00152                 packetnumber =packetnumber+1;
00153                 #endif
00154                 //send imu
00155                 memset (&buffer[0], '\0', sizeof(buffer));
00156                 #if !RAW
00157                 acc_gyro->get_x_axes(axes);
00158                 acc_gyro->get_g_axes(axes2);
00159                 #else
00160                 acc_gyro->get_x_axes_raw(axes);
00161                 acc_gyro->get_g_axes_raw(axes2);
00162                 #endif
00163                 double accConversion = 0.009806;
00164                 double gyrConversion = 0.0174533/1000;
00165                 
00166                 #if !RAW
00167                 sprintf(buffer, "%02.6f,%02.6f,%02.6f,%03.6f,%03.6f,%03.6f\n\r",  (axes[0]*accConversion), (axes[1]*accConversion), (axes[2]*accConversion), (axes2[0]*gyrConversion), (axes2[1]*gyrConversion), (axes2[2]*gyrConversion));
00168                 #else
00169                 sprintf(buffer, "%d,%d,%d,%d,%d,%d\n\r",  axes[0], axes[1], axes[2], axes2[0], axes2[1], axes2[2]);
00170                 #endif
00171                
00172                #if DEBUG
00173                
00174                 printf (buffer);
00175                 
00176                 wait (1);
00177                 #else
00178                 n =sizeof (buffer);
00179                 server.sendTo(client, buffer, n);
00180                 #endif
00181                 
00182                 }
00183    
00184      
00185     
00186         
00187         }
00188 }