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-10-15
- Revision:
- 9:45a96c88754d
- Parent:
- 8:ee9a824852bf
File content as of revision 9:45a96c88754d:
/*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 #define RAW 0 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5); DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); int main() { //cose per accelerometro #if !RAW int32_t axes[3]; int32_t axes2[3]; #else int16_t axes[3]; int16_t axes2[3]; #endif 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 = "192.168.1.205"; const char * MASK = "255.255.255.0"; const char * GATEWAY = "192.168.1.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); #endif 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++){ #if !RAW acc_gyro->get_x_axes(axes); acc_gyro->get_g_axes(axes2); #else acc_gyro->get_x_axes_raw(axes); acc_gyro->get_g_axes_raw(axes2); #endif //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 while (1){ #if !DEBUG packetnumber =packetnumber+1; #endif //send imu memset (&buffer[0], '\0', sizeof(buffer)); #if !RAW acc_gyro->get_x_axes(axes); acc_gyro->get_g_axes(axes2); #else acc_gyro->get_x_axes_raw(axes); acc_gyro->get_g_axes_raw(axes2); #endif double accConversion = 0.009806; double gyrConversion = 0.0174533/1000; #if !RAW 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)); #else sprintf(buffer, "%d,%d,%d,%d,%d,%d\n\r", axes[0], axes[1], axes[2], axes2[0], axes2[1], axes2[2]); #endif #if DEBUG printf (buffer); wait (1); #else n =sizeof (buffer); server.sendTo(client, buffer, n); #endif } } }