Simone Mentasti / Mbed 2 deprecated AcquisizioneIMU

STM Imu acquisition setup using ethernet

Dependencies:   F7_Ethernet mbed HTS221 LPS22HB LSM303AGR LSM6DSL

Fork of Nucleo_F746ZG_Ethernet by Dieter Graef

main.cpp@9:45a96c88754d, 2018-10-15 (annotated)

Committer:
nirnakern
Date:
Mon Oct 15 14:41:19 2018 +0000
Revision:
9:45a96c88754d
Parent:
8:ee9a824852bf 
First Commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nirnakern 4:04c17d376d57 1 /*QUESTA è LA VERSIONE PER IMU SU Ethernet*/
DieterGraef 0:f9b6112278fe 2 #include "mbed.h"
DieterGraef 0:f9b6112278fe 3 #include "rtos.h"
DieterGraef 0:f9b6112278fe 4 #include "EthernetInterface.h"
DieterGraef 0:f9b6112278fe 5 #include "NTPClient.h"
DieterGraef 0:f9b6112278fe 6 #include <stdio.h>
nirnakern 3:758e8ed30819 7 #include "XNucleoIKS01A2.h"
DieterGraef 0:f9b6112278fe 8
nirnakern 3:758e8ed30819 9 #define ECHO_SERVER_PORT 7
nirnakern 6:21fb4863e28d 10 #define DEBUG 0
nirnakern 8:ee9a824852bf 11 #define RAW 0
DieterGraef 0:f9b6112278fe 12
nirnakern 3:758e8ed30819 13 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
nirnakern 9:45a96c88754d 14
nirnakern 3:758e8ed30819 15 DigitalOut myled1(LED1);
nirnakern 3:758e8ed30819 16 DigitalOut myled2(LED2);
nirnakern 3:758e8ed30819 17 DigitalOut myled3(LED3);
nirnakern 3:758e8ed30819 18 int main() {
nirnakern 3:758e8ed30819 19
nirnakern 3:758e8ed30819 20 //cose per accelerometro
nirnakern 8:ee9a824852bf 21
nirnakern 8:ee9a824852bf 22 #if !RAW
nirnakern 3:758e8ed30819 23 int32_t axes[3];
nirnakern 3:758e8ed30819 24 int32_t axes2[3];
nirnakern 8:ee9a824852bf 25 #else
nirnakern 8:ee9a824852bf 26 int16_t axes[3];
nirnakern 8:ee9a824852bf 27 int16_t axes2[3];
nirnakern 8:ee9a824852bf 28 #endif
nirnakern 3:758e8ed30819 29 uint8_t id;
nirnakern 3:758e8ed30819 30
nirnakern 3:758e8ed30819 31 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
nirnakern 3:758e8ed30819 32 acc_gyro->enable_x();
nirnakern 3:758e8ed30819 33 acc_gyro->enable_g();
nirnakern 3:758e8ed30819 34 acc_gyro->read_id(&id);
nirnakern 3:758e8ed30819 35 printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
nirnakern 3:758e8ed30819 36
nirnakern 3:758e8ed30819 37 acc_gyro->set_x_odr(208.0f);
nirnakern 3:758e8ed30819 38 acc_gyro->set_g_odr(208.0f);
nirnakern 3:758e8ed30819 39 acc_gyro->set_x_fs(2.0f);
nirnakern 5:32078e9590f0 40 acc_gyro->set_g_fs(125.0f);
nirnakern 3:758e8ed30819 41
nirnakern 3:758e8ed30819 42 //cpose per comunicazioni
nirnakern 9:45a96c88754d 43 const char * IP = "192.168.1.205";
nirnakern 3:758e8ed30819 44 const char * MASK = "255.255.255.0";
nirnakern 9:45a96c88754d 45 const char * GATEWAY = "192.168.1.1";
nirnakern 3:758e8ed30819 46
nirnakern 6:21fb4863e28d 47 #if !DEBUG
nirnakern 3:758e8ed30819 48
nirnakern 3:758e8ed30819 49 EthernetInterface eth;
nirnakern 3:758e8ed30819 50 eth.init(IP,MASK,GATEWAY); //Use DHCP
nirnakern 3:758e8ed30819 51 eth.connect();
nirnakern 5:32078e9590f0 52 printf("IP Address is %s\n\r", eth.getIPAddress());
nirnakern 3:758e8ed30819 53
nirnakern 3:758e8ed30819 54
nirnakern 3:758e8ed30819 55
nirnakern 3:758e8ed30819 56
nirnakern 3:758e8ed30819 57 UDPSocket server;
nirnakern 3:758e8ed30819 58 server.bind(ECHO_SERVER_PORT);
DieterGraef 0:f9b6112278fe 59
nirnakern 3:758e8ed30819 60
nirnakern 3:758e8ed30819 61 Endpoint client;
nirnakern 6:21fb4863e28d 62
nirnakern 6:21fb4863e28d 63 #endif
nirnakern 3:758e8ed30819 64 char buffer[256];
nirnakern 3:758e8ed30819 65 int32_t packetnumber =0;
nirnakern 3:758e8ed30819 66
nirnakern 6:21fb4863e28d 67
nirnakern 3:758e8ed30819 68
nirnakern 3:758e8ed30819 69
nirnakern 3:758e8ed30819 70
nirnakern 3:758e8ed30819 71
nirnakern 3:758e8ed30819 72
nirnakern 3:758e8ed30819 73
nirnakern 3:758e8ed30819 74
nirnakern 3:758e8ed30819 75
nirnakern 3:758e8ed30819 76
nirnakern 3:758e8ed30819 77
nirnakern 3:758e8ed30819 78
nirnakern 3:758e8ed30819 79 while(1) {
DieterGraef 0:f9b6112278fe 80
nirnakern 6:21fb4863e28d 81 #if !DEBUG
nirnakern 3:758e8ed30819 82 printf("\nWaiting for UDP packet...\n");
nirnakern 3:758e8ed30819 83
nirnakern 3:758e8ed30819 84 myled1=1;
nirnakern 3:758e8ed30819 85 int n = server.receiveFrom(client, buffer, sizeof(buffer));
nirnakern 3:758e8ed30819 86 buffer[n] = '\0';
nirnakern 3:758e8ed30819 87
nirnakern 5:32078e9590f0 88 printf("Received packet from: %s\n\r", client.get_address());
nirnakern 5:32078e9590f0 89 printf("Packet contents : '%s'\n\r",buffer);
nirnakern 3:758e8ed30819 90
nirnakern 9:45a96c88754d 91
DieterGraef 0:f9b6112278fe 92
nirnakern 6:21fb4863e28d 93 #endif
nirnakern 3:758e8ed30819 94
nirnakern 9:45a96c88754d 95
nirnakern 3:758e8ed30819 96 printf("Calibrating wait 30s\n");
nirnakern 3:758e8ed30819 97
nirnakern 3:758e8ed30819 98 myled2=1;
nirnakern 3:758e8ed30819 99
nirnakern 3:758e8ed30819 100 long offsetGX=0;
nirnakern 3:758e8ed30819 101 long offsetGY=0;
nirnakern 3:758e8ed30819 102 long offsetGZ=0;
nirnakern 3:758e8ed30819 103 long offsetAX=0;
nirnakern 3:758e8ed30819 104 long offsetAY=0;
nirnakern 3:758e8ed30819 105 long offsetAZ=0;
nirnakern 3:758e8ed30819 106 long counter=0;
nirnakern 3:758e8ed30819 107
nirnakern 3:758e8ed30819 108 for (int i=0;i<4500;i++){
nirnakern 8:ee9a824852bf 109 #if !RAW
nirnakern 3:758e8ed30819 110 acc_gyro->get_x_axes(axes);
nirnakern 3:758e8ed30819 111 acc_gyro->get_g_axes(axes2);
nirnakern 8:ee9a824852bf 112 #else
nirnakern 8:ee9a824852bf 113 acc_gyro->get_x_axes_raw(axes);
nirnakern 8:ee9a824852bf 114 acc_gyro->get_g_axes_raw(axes2);
nirnakern 3:758e8ed30819 115
nirnakern 8:ee9a824852bf 116 #endif
nirnakern 5:32078e9590f0 117 //offsetGX=offsetGX+ axes[0];
nirnakern 5:32078e9590f0 118 //offsetGY=offsetGY+ axes[1];
nirnakern 5:32078e9590f0 119 //offsetGZ=offsetGZ+ axes[2];
nirnakern 3:758e8ed30819 120
nirnakern 3:758e8ed30819 121 offsetAX=offsetAX+ axes2[0];
nirnakern 3:758e8ed30819 122 offsetAY=offsetAY+ axes2[1];
nirnakern 3:758e8ed30819 123 offsetAZ=offsetAZ+ axes2[2];
nirnakern 3:758e8ed30819 124 counter++;
nirnakern 3:758e8ed30819 125
nirnakern 3:758e8ed30819 126
nirnakern 3:758e8ed30819 127
DieterGraef 0:f9b6112278fe 128 }
nirnakern 3:758e8ed30819 129
nirnakern 3:758e8ed30819 130
nirnakern 5:32078e9590f0 131 //offsetGX=offsetGX/counter;
nirnakern 5:32078e9590f0 132 //offsetGY=offsetGY/counter;
nirnakern 5:32078e9590f0 133 //offsetGZ=offsetGZ/counter;
nirnakern 3:758e8ed30819 134 offsetAX=offsetAX/counter;
nirnakern 3:758e8ed30819 135 offsetAY=offsetAY/counter;
nirnakern 3:758e8ed30819 136 offsetAZ=offsetAZ/counter;
nirnakern 3:758e8ed30819 137
nirnakern 3:758e8ed30819 138
nirnakern 3:758e8ed30819 139
nirnakern 3:758e8ed30819 140
nirnakern 3:758e8ed30819 141
nirnakern 5:32078e9590f0 142 printf("Start Sending Imu messages\n\r");
nirnakern 3:758e8ed30819 143
nirnakern 3:758e8ed30819 144 myled3=1;
nirnakern 6:21fb4863e28d 145 #if !DEBUG
nirnakern 3:758e8ed30819 146 server.sendTo(client, buffer, n);
nirnakern 6:21fb4863e28d 147 #endif
nirnakern 9:45a96c88754d 148
nirnakern 3:758e8ed30819 149
nirnakern 3:758e8ed30819 150 while (1){
nirnakern 6:21fb4863e28d 151 #if !DEBUG
nirnakern 3:758e8ed30819 152 packetnumber =packetnumber+1;
nirnakern 6:21fb4863e28d 153 #endif
nirnakern 3:758e8ed30819 154 //send imu
nirnakern 3:758e8ed30819 155 memset (&buffer[0], '\0', sizeof(buffer));
nirnakern 8:ee9a824852bf 156 #if !RAW
nirnakern 3:758e8ed30819 157 acc_gyro->get_x_axes(axes);
nirnakern 3:758e8ed30819 158 acc_gyro->get_g_axes(axes2);
nirnakern 8:ee9a824852bf 159 #else
nirnakern 8:ee9a824852bf 160 acc_gyro->get_x_axes_raw(axes);
nirnakern 8:ee9a824852bf 161 acc_gyro->get_g_axes_raw(axes2);
nirnakern 8:ee9a824852bf 162 #endif
nirnakern 6:21fb4863e28d 163 double accConversion = 0.009806;
nirnakern 6:21fb4863e28d 164 double gyrConversion = 0.0174533/1000;
nirnakern 6:21fb4863e28d 165
nirnakern 8:ee9a824852bf 166 #if !RAW
nirnakern 7:6b56beca495e 167 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));
nirnakern 8:ee9a824852bf 168 #else
nirnakern 8:ee9a824852bf 169 sprintf(buffer, "%d,%d,%d,%d,%d,%d\n\r", axes[0], axes[1], axes[2], axes2[0], axes2[1], axes2[2]);
nirnakern 8:ee9a824852bf 170 #endif
nirnakern 6:21fb4863e28d 171
nirnakern 6:21fb4863e28d 172 #if DEBUG
nirnakern 6:21fb4863e28d 173
nirnakern 6:21fb4863e28d 174 printf (buffer);
nirnakern 6:21fb4863e28d 175
nirnakern 6:21fb4863e28d 176 wait (1);
nirnakern 6:21fb4863e28d 177 #else
nirnakern 3:758e8ed30819 178 n =sizeof (buffer);
nirnakern 3:758e8ed30819 179 server.sendTo(client, buffer, n);
nirnakern 6:21fb4863e28d 180 #endif
nirnakern 6:21fb4863e28d 181
nirnakern 3:758e8ed30819 182 }
nirnakern 3:758e8ed30819 183
nirnakern 3:758e8ed30819 184
nirnakern 3:758e8ed30819 185
nirnakern 3:758e8ed30819 186
DieterGraef 0:f9b6112278fe 187 }
nirnakern 3:758e8ed30819 188 }