STM Imu acquisition setup using ethernet
Dependencies: F7_Ethernet mbed HTS221 LPS22HB LSM303AGR LSM6DSL
Fork of Nucleo_F746ZG_Ethernet by
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?
User | Revision | Line number | New 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 | } |