As you probably already know from my project "Fuelino", I have been using a cheap and easy-to-use IMU (Inertial Measurement Unit) called MPU-6050. This board mounts a cheap integrated circuit capable of acquiring acceleration and gyroscope (and temperature) signals with 16 bits resolution. Such raw data can be read from the main microcontroller unit (for example Arduino, Raspberry Pi, ESP8266) using I2C communication protocol.
At the beginning, it is not so simple to understand how to communicate with MPU-6050, this is why I decided to create a simple library which "abstracts" the communication with MPU-6050, and lets you simply read the values of acceleration, gyroscope, temperature. The library is available on GitHub, inside the sub-folder "MPU6050mgr". Inside the GitHub repository, I also provided a simple example which shows how to send automatically the measurements to your PC, using Serial communication, once a second (MIN_TIME_BETWEEN_PRINTS set to 1000ms). This is done by setting:
#define MIN_TIME_BETWEEN_PRINTS 1000
#define AUTOMATIC_PRINT 1
In order to test the sketch, you can use both the Serial Monitor embedded in Arduino IDE (as shown above), or RealTerm (as shown below).
Alternatively, by setting "AUTOMATIC_PRINT" to 0, the example sketch below works in "polling mode". This means that, whenever you send, from your PC, an ASCII character "m", Arduino replies with a packet (binary data) which contains the raw signals read from MPU-6050.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 |
// Created by Davide Cavaliere // E-mail: dadez87@gmail.com // Website: www.monocilindro.com // 25 February 2016 // This is an example on how to use the "MPU6050mgr" library // You should connect MPU-6050 to your Arduino using I2C // In case "AUTOMATIC_PRINT = 1", ASCII data (acceleration, gyroscope, temperature) are sent via Serial once every 1 second // In case "AUTOMATIC_PRINT = 0", binary raw data (acceleration, gyroscope, temperature) are sent via Serial when you poll the message using "m" character // For more info, please read the source files "MPU6050mgr.cpp" and "MPU6050mgr.h" #include "src/MPU6050mgr/MPU6050mgr.h" // IMU module (if present) #define MIN_TIME_BETWEEN_PRINTS 1000 // time between prints, in ms #define AUTOMATIC_PRINT 1 // Enables automatic printing (=1). In case =0, polling printing is active unsigned long time_last_print = 0; // last time that the MPU6050 was sent to Serial void setup() { Serial.begin(57600); MPU6050mgr.begin(); // Initializes IMU module } void loop() { unsigned long time_now_tmp = millis(); // Time entering the loop MPU6050mgr.manager(time_now_tmp); // IMU communication manager #if AUTOMATIC_PRINT == 1 if ((time_now_tmp - time_last_print) >= MIN_TIME_BETWEEN_PRINTS){ MPU6050mgr.send_ASCII_data(); time_last_print=time_now_tmp; } #else if if (Serial.available()){ // if any data available on Serial char tmp_char = Serial.read(); // read character received on Serial if (tmp_char == 'm'){ // if the character read is "m", as "mpu6050" uint8_t buffer_temporary[MPU6050_BUFFER_SD_WRITE_SIZE]; // create temporary writing buffer if (MPU6050mgr.prepare_SD_packet(buffer_temporary)){ // if there is any packet available on MPU6050 buffer Serial.write(buffer_temporary, MPU6050_BUFFER_SD_WRITE_SIZE); // Send IMU data to serial } } } #endif } |
When the sketch is compiled in "polling mode", you should use RealTerm or similar tools to visualize the binary data (HEX format below) received from Arduino. The polling is done by simply sending an "m" ASCII character to Arduino (serial communication should be initialized at 57600 baud). When you send this command to Arduino, it replies with a packet composed by 21 bytes, which have the following meaning:
- 1 byte is the "packet type". The value is always 0x49 (ASCII character "I").
- 4 bytes are the "time stamp", in milliseconds, from the time at which Arduino did the Power On.
- 12 bytes are the "acceleration and gyroscope raw data". There are 3 axis for acceleration signals (X, Y, Z axis), and then also 3 axis for gyroscope signals. In total, 6 info, each one is a signed integer with 16 bits (2 bytes). The raw data received from MPU-6050 is encoded into "big endian format" (higher byte comes first, lower byte comes after). However, since the PC (and Arduino) works with "little endian format", the MPU6050mgr library takes care of this conversion by switching the order of the bytes. In addition to that, it is possible to apply a filter to the raw data by setting the filtering coefficient "MPU6050_SIG_FILT_CNST" inside "MPU6050mgr.cpp". When this constant is set to "0", no filtering is done. The filter implemented is a simple IIR filter, which composes the last sample and the previous one.
- 2 bytes are the "temperature" signal. In order to convert to Celsius degrees, you should divide by 340.0 and then sum 36.53. You can manually perform this operation using a calculator, or any program (you should first cast the integer number to float or double, and then operate).
- 2 bytes are the "checksum", in case you want to implement error detection.
Now, let's analyze deeply the most important parts of the library. In order to understand better how it works, you should have a look at the files "MPU6050mgr.cpp" and "MPU6050mgr.h" which are available here.
There are just few "defines" that you need to check, and update if necessary.
- MPU6050_I2C_ADDRESS is the I2C address of MPU6050. The standard is 0x68
- MPU6050_TASK_TIME_TARGET is the time, in ms, at which MPU6050 is polled (example 1 time every 20ms)
- MPU6050_TASK_TIME_MULT is the number of polling after which the buffer is filled. For example, if you set 5 here, and the previous period is 20ms, one buffer will be filled once every 100ms
- MPU6050_SIG_FILT_CNST is the filtering constant of IIR filter. The max value is (2^15 -1).
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 |
#include <Arduino.h> #include <Wire.h> #include "MPU6050mgr.h" #define MPU6050_I2C_ADDRESS 0x68 // I2C address of the MPU-6050 #define MPU6050_TASK_TIME_TARGET (uint16_t)20 // Task time target, in ms #define MPU6050_TASK_TIME_MULT (uint8_t)5 // Multiplier for polling time [the sampling time is the multiplication of this value and polling time] #define MPU6050_SIG_FILT_CNST (int16_t)10000 // Filtering constant for newly received signals (0 = No filtering. Max value: 2^15 -1) // CALCULATED THE UBX CHECKSUM OF A CHAR ARRAY uint16_t COMM_calculate_checksum(uint8_t* array, uint8_t array_start, uint8_t array_length) { uint8_t CK_A = 0; uint8_t CK_B = 0; uint8_t i; for (i = 0; i < array_length; i++) { CK_A = CK_A + array[array_start + i]; CK_B = CK_B + CK_A; } return ((CK_A << 8) | (CK_B)); } // Copies data from one array to an other void copy_uint8_array(uint8_t* array_src_data, uint8_t array_src_start, uint8_t* array_dst_data, uint8_t array_dst_start, uint8_t array_length){ for (uint8_t i=0; i<array_length; i++){ array_dst_data[array_dst_start+i] = array_src_data[array_src_start+i]; // copy start to destination byte } } // Reads multiple bytes from MPU6050 uint8_t MPU6050_read(uint8_t start, uint8_t *buffer, uint8_t size){ Wire.beginTransmission(MPU6050_I2C_ADDRESS); Wire.write(start); Wire.endTransmission(false); // hold the I2C-bus Wire.requestFrom(MPU6050_I2C_ADDRESS, (int)size, true); uint8_t i = 0; while(Wire.available() && (i<size)){ buffer[i++]=Wire.read(); } if (i != size) return 1; // Did not read enough bytes return 0; // no error } // Writes multiple bytes to MPU6050 uint8_t MPU6050_write(uint8_t start, uint8_t *pData, uint8_t size){ Wire.beginTransmission(MPU6050_I2C_ADDRESS); if (Wire.write(start) != 1) return 1; // write the start address if (Wire.write(pData, size) != 1) return 1; // write data bytes if (Wire.endTransmission(true) != 0) return 1; // release the I2C-bus return 0; // no error } // Writes single byte to MPU6050 uint8_t MPU6050_write_reg(uint8_t reg, uint8_t data){ uint8_t tmp_ret = MPU6050_write(reg, &data, 1); return tmp_ret; } // Initializes MPU6050 void MPU6050mgr_class::begin(){ //#if (MPU6050_PRESENT == 1) Wire.begin(); // Initializes Wire communication (I2C) MPU6050_write_reg(0x6B,0); // Wake up MPU6050_write_reg(0x1A,0x03); // Filter: Acceleration 44Hz, Gyroscope 42Hz //#endif // Clean acceleration and gyroscope signals for (uint8_t i=0; i< 6; i++){ ag[i]=0; } } // Filters the signal using present value and old value int16_t MPU6050mgr_class::filter_signal(int16_t sig_old, int16_t sig_new){ int32_t acc = 1 << 14; // accumulator for MACs // load rounding constant acc += ((int32_t)sig_old * (int32_t)MPU6050_SIG_FILT_CNST); acc += ((int32_t)sig_new * ((int32_t)32768 - (int32_t)MPU6050_SIG_FILT_CNST)); // saturate the result if ( acc > 0x3fffffff ) { acc = 0x3fffffff; } else if ( acc < -0x40000000 ) { acc = -0x40000000; } return ((int16_t)(acc >> 15)); // convert from Q30 to Q15 } // Reads data from MPU-6050, filters the signals, and saves the signals in the temporary buffer uint8_t MPU6050mgr_class::read_data(){ // Receives data from MPU-6050 uint8_t IMU_buffer_recv[14]; // temporary buffer for data receive if (MPU6050_read(0x3B, IMU_buffer_recv, 14)) return 1; // Not OK (data could not be received correctly) // Transforms raw data into integers, and filters for (uint8_t i=0; i<6; i++){ // 6 signals (Acceleration and Gyroscope) uint8_t j = 2*i; // for Acceleration data if (j >= 6) j = j + 2; // for Gyroscope data, starting from index 8 on MPU packet int16_t int_old = ag[i]; // Old value int16_t int_new = (IMU_buffer_recv[j]<<8) | (IMU_buffer_recv[j+1]); // Acceleration or Gyroscope value ag[i] = filter_signal(int_old, int_new); // Filtered signal } temperature = (IMU_buffer_recv[6]<<8) | (IMU_buffer_recv[7]); // Temperature value return 0; // OK } // Manages the requests coming from outside (Main Loop and Yield) void MPU6050mgr_class::manager(unsigned long time_now_ms){ //#if (MPU6050_PRESENT == 1) uint32_t time_now_ms_32bit = time_now_ms; // 32 bits uint16_t time_now_ms_16bit = (uint16_t)(time_now_ms & 0x0000FFFF); // 16 bits // Checks if enough time has expired. In positive case, read data from IMU, and stores it into the buffer if ((time_now_ms_16bit - polling_time_last) >= MPU6050_TASK_TIME_TARGET){ polling_time_last += MPU6050_TASK_TIME_TARGET; read_data(); // Reads data from MPU and filters it task_multiplier_cnt++; if (task_multiplier_cnt >= MPU6050_TASK_TIME_MULT) { // I have polled many times, I can store now task_multiplier_cnt = 0; // rollover if (buffer_data_available() < (MPU6050_BUFFERS_NUMBER-1)){ // to avoid overwriting the buffer before it is used item_last_write++; // One more buffer item added if (item_last_write == MPU6050_BUFFERS_NUMBER) item_last_write=0; // rollover copy_uint8_array((uint8_t*)(&ag[0]), 0, data_buffer[item_last_write].IMU_data, 0, 12); // Copy acceleration and gyroscope copy_uint8_array((uint8_t*)(&time_now_ms_32bit), 0, data_buffer[item_last_write].polling_time_stamp, 0, 4); // Add time stamp } } } //#endif } // Flushes the buffer (so that there is no data to write) void MPU6050mgr_class::flush_buffer(){ item_last_read = item_last_write; } // Returns the number of buffer packets available to write on SD uint8_t MPU6050mgr_class::buffer_data_available(){ if (item_last_write >= item_last_read) return (item_last_write - item_last_read); // 0 or higher return (MPU6050_BUFFERS_NUMBER + item_last_write - item_last_read); // when there is a rollover } // Prepares the packet for SD writing, and returns 1 in case a buffer was filled uint8_t MPU6050mgr_class::prepare_SD_packet(uint8_t* temp_data_buffer_SD){ if (!MPU6050mgr.buffer_data_available()) return 0; // No new data available temp_data_buffer_SD[0]=0x49; // "I" for IMU | Byte 0 item_last_read++; // One more item will be soon written to SD if (item_last_read == MPU6050_BUFFERS_NUMBER) item_last_read = 0; // rollover copy_uint8_array(data_buffer[item_last_read].polling_time_stamp, 0, temp_data_buffer_SD, 1, 4); // Time Stamp, in ms (32 bits) | Bytes 1-4 copy_uint8_array(data_buffer[item_last_read].IMU_data, 0, temp_data_buffer_SD, 5, 12); // IMU data | Bytes 5-16 copy_uint8_array((uint8_t*)(&temperature), 0, temp_data_buffer_SD, 17, 2); // Temperature | Bytes 17-18 uint16_t CK_SUM = COMM_calculate_checksum(temp_data_buffer_SD, 0, 19); temp_data_buffer_SD[19] = (uint8_t)(CK_SUM >> 8); // Checksum | Byte 19 temp_data_buffer_SD[20] = (uint8_t)(CK_SUM & 0xFF); // Checksum | Byte 20 return 1; } // Prepares the packet for COMM send (Serial Service protocol) void MPU6050mgr_class::prepare_COMM_packet(uint8_t* temp_data_buffer_COMM){ copy_uint8_array((uint8_t*)(&polling_time_last), 0, temp_data_buffer_COMM, 0, 2); // Time Stamp, in ms (16 bits) copy_uint8_array((uint8_t*)(&ag[0]), 0, temp_data_buffer_COMM, 2, 12); // Copy acceleration and gyroscope (12 bytes) } // Sends ASCII data on Serial void MPU6050mgr_class::send_ASCII_data(){ for (uint8_t i=0; i<6; i++){ Serial.print(ag[i]); Serial.print(F(",")); } Serial.print(temperature); Serial.print(F(",")); Serial.write('\r'); Serial.write('\n'); } MPU6050mgr_class MPU6050mgr; |
Hi David,
I am working on human hand function. I need to get the hand trajectory using IMU sensors. I am currently working on Adafruit LSM9DS1 IMU sensor and another in-built LSM9DS1 of Arduino nano33 ble sense. I am very new to Madgwick/kalman/complementary filter. I am having knowledge and interest more towards its mdical application. Could you please help me or advice on the completion of my work?. We can collaborate and will give further details if you are ready to work on algorithm part.