STM Imu acquisition setup using ethernet
Dependencies: F7_Ethernet mbed HTS221 LPS22HB LSM303AGR LSM6DSL
Fork of Nucleo_F746ZG_Ethernet by
main.cpp
- Committer:
- nirnakern
- Date:
- 2018-01-31
- Revision:
- 7:6b56beca495e
- Parent:
- 6:21fb4863e28d
- Child:
- 8:ee9a824852bf
File content as of revision 7:6b56beca495e:
/*QUESTA è LA VERSIONE PER IMU SU Ethernet*/ #include "mbed.h" #include "rtos.h" #include "EthernetInterface.h" #include "NTPClient.h" #include <stdio.h> #include "XNucleoIKS01A2.h" #define ECHO_SERVER_PORT 7 #define DEBUG 0 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5); Timer t; DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); int main() { //cose per accelerometro int32_t axes[3]; int32_t axes2[3]; uint8_t id; static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro; acc_gyro->enable_x(); acc_gyro->enable_g(); acc_gyro->read_id(&id); printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id); acc_gyro->set_x_odr(208.0f); acc_gyro->set_g_odr(208.0f); acc_gyro->set_x_fs(2.0f); acc_gyro->set_g_fs(125.0f); //cpose per comunicazioni const char * IP = "169.254.112.124"; const char * MASK = "255.255.255.0"; const char * GATEWAY = "169.254.112.1"; #if !DEBUG EthernetInterface eth; eth.init(IP,MASK,GATEWAY); //Use DHCP eth.connect(); printf("IP Address is %s\n\r", eth.getIPAddress()); UDPSocket server; server.bind(ECHO_SERVER_PORT); Endpoint client; #endif char buffer[256]; int32_t packetnumber =0; while(1) { #if !DEBUG 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\r", client.get_address()); printf("Packet contents : '%s'\n\r",buffer); NTPClient ntp; ntp.setTime(client.get_address(), 123); time_t ctTime; ctTime = time(NULL); printf("Time is set to (UTC): %s,\r", ctime(&ctTime)); #endif time_t offset = time(NULL); t.start(); printf("Calibrating wait 30s\n"); myled2=1; long offsetGX=0; long offsetGY=0; long offsetGZ=0; long offsetAX=0; long offsetAY=0; long offsetAZ=0; long counter=0; for (int i=0;i<4500;i++){ acc_gyro->get_x_axes(axes); acc_gyro->get_g_axes(axes2); //offsetGX=offsetGX+ axes[0]; //offsetGY=offsetGY+ axes[1]; //offsetGZ=offsetGZ+ axes[2]; offsetAX=offsetAX+ axes2[0]; offsetAY=offsetAY+ axes2[1]; offsetAZ=offsetAZ+ axes2[2]; counter++; } //offsetGX=offsetGX/counter; //offsetGY=offsetGY/counter; //offsetGZ=offsetGZ/counter; offsetAX=offsetAX/counter; offsetAY=offsetAY/counter; offsetAZ=offsetAZ/counter; printf("Start Sending Imu messages\n\r"); myled3=1; #if !DEBUG server.sendTo(client, buffer, n); #endif Timer t; t.start(); while (1){ #if !DEBUG packetnumber =packetnumber+1; #endif //send imu memset (&buffer[0], '\0', sizeof(buffer)); acc_gyro->get_x_axes(axes); acc_gyro->get_g_axes(axes2); double accConversion = 0.009806; double gyrConversion = 0.0174533/1000; 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)); #if DEBUG printf (buffer); wait (1); #else n =sizeof (buffer); server.sendTo(client, buffer, n); #endif } } }