oda itsuha / MPU9250

MPU9250 library

MPU9250.cpp@0:9b60815f8984, 2021-10-16 (annotated)

Committer:
odaa
Date:
Sat Oct 16 02:28:24 2021 +0000
Revision:
0:9b60815f8984
copy&paste

Who changed what in which revision?

UserRevisionLine numberNew contents of line
odaa 0:9b60815f8984 1 #include "MPU9250.h"
odaa 0:9b60815f8984 2
odaa 0:9b60815f8984 3 #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
odaa 0:9b60815f8984 4 #define Ki 0.0f
odaa 0:9b60815f8984 5
odaa 0:9b60815f8984 6 //******************************************************************************
odaa 0:9b60815f8984 7 MPU9250::MPU9250(PinName sda, PinName scl)
odaa 0:9b60815f8984 8 {
odaa 0:9b60815f8984 9 i2c_ = new I2C(sda, scl);
odaa 0:9b60815f8984 10 i2c_->frequency(400000);
odaa 0:9b60815f8984 11 }
odaa 0:9b60815f8984 12
odaa 0:9b60815f8984 13 //******************************************************************************
odaa 0:9b60815f8984 14 MPU9250::MPU9250(I2C *i2c):i2c_(i2c){}
odaa 0:9b60815f8984 15
odaa 0:9b60815f8984 16 //******************************************************************************
odaa 0:9b60815f8984 17 MPU9250::~MPU9250()
odaa 0:9b60815f8984 18 {
odaa 0:9b60815f8984 19 delete i2c_;
odaa 0:9b60815f8984 20 }
odaa 0:9b60815f8984 21
odaa 0:9b60815f8984 22 void MPU9250::writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
odaa 0:9b60815f8984 23 {
odaa 0:9b60815f8984 24 char data_write[2];
odaa 0:9b60815f8984 25 data_write[0] = subAddress;
odaa 0:9b60815f8984 26 data_write[1] = data;
odaa 0:9b60815f8984 27 i2c_->write(address, data_write, 2, 0);
odaa 0:9b60815f8984 28 }
odaa 0:9b60815f8984 29
odaa 0:9b60815f8984 30 char MPU9250::readByte(uint8_t address, uint8_t subAddress)
odaa 0:9b60815f8984 31 {
odaa 0:9b60815f8984 32 char data[1]; // `data` will store the register data
odaa 0:9b60815f8984 33 char data_write[1];
odaa 0:9b60815f8984 34 data_write[0] = subAddress;
odaa 0:9b60815f8984 35 i2c_->write(address, data_write, 1, 1); // no stop
odaa 0:9b60815f8984 36 i2c_->read(address, data, 1, 0);
odaa 0:9b60815f8984 37 return data[0];
odaa 0:9b60815f8984 38 }
odaa 0:9b60815f8984 39
odaa 0:9b60815f8984 40 void MPU9250::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
odaa 0:9b60815f8984 41 {
odaa 0:9b60815f8984 42 char data[14];
odaa 0:9b60815f8984 43 char data_write[1];
odaa 0:9b60815f8984 44 data_write[0] = subAddress;
odaa 0:9b60815f8984 45 i2c_->write(address, data_write, 1, 1); // no stop
odaa 0:9b60815f8984 46 i2c_->read(address, data, count, 0);
odaa 0:9b60815f8984 47 for(int ii = 0; ii < count; ii++) {
odaa 0:9b60815f8984 48 dest[ii] = data[ii];
odaa 0:9b60815f8984 49 }
odaa 0:9b60815f8984 50 }
odaa 0:9b60815f8984 51
odaa 0:9b60815f8984 52 float MPU9250::getMres() {
odaa 0:9b60815f8984 53 switch (Mscale)
odaa 0:9b60815f8984 54 {
odaa 0:9b60815f8984 55 // Possible magnetometer scales (and their register bit settings) are:
odaa 0:9b60815f8984 56 // 14 bit resolution (0) and 16 bit resolution (1)
odaa 0:9b60815f8984 57 case MFS_14BITS:
odaa 0:9b60815f8984 58 mRes = 10.0*4219.0/8190.0; // Proper scale to return milliGauss
odaa 0:9b60815f8984 59 break;
odaa 0:9b60815f8984 60 case MFS_16BITS:
odaa 0:9b60815f8984 61 mRes = 10.0*4219.0/32760.0; // Proper scale to return milliGauss
odaa 0:9b60815f8984 62 break;
odaa 0:9b60815f8984 63
odaa 0:9b60815f8984 64
odaa 0:9b60815f8984 65 }
odaa 0:9b60815f8984 66 return mRes;
odaa 0:9b60815f8984 67
odaa 0:9b60815f8984 68 }
odaa 0:9b60815f8984 69
odaa 0:9b60815f8984 70
odaa 0:9b60815f8984 71 float MPU9250::getGres() {
odaa 0:9b60815f8984 72 switch (Gscale)
odaa 0:9b60815f8984 73 {
odaa 0:9b60815f8984 74 // Possible gyro scales (and their register bit settings) are:
odaa 0:9b60815f8984 75 // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
odaa 0:9b60815f8984 76 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
odaa 0:9b60815f8984 77 case GFS_250DPS:
odaa 0:9b60815f8984 78 gRes = 250.0/32768.0;
odaa 0:9b60815f8984 79 break;
odaa 0:9b60815f8984 80 case GFS_500DPS:
odaa 0:9b60815f8984 81 gRes = 500.0/32768.0;
odaa 0:9b60815f8984 82 break;
odaa 0:9b60815f8984 83 case GFS_1000DPS:
odaa 0:9b60815f8984 84 gRes = 1000.0/32768.0;
odaa 0:9b60815f8984 85 break;
odaa 0:9b60815f8984 86 case GFS_2000DPS:
odaa 0:9b60815f8984 87 gRes = 2000.0/32768.0;
odaa 0:9b60815f8984 88 break;
odaa 0:9b60815f8984 89 }
odaa 0:9b60815f8984 90 return gRes;
odaa 0:9b60815f8984 91 }
odaa 0:9b60815f8984 92
odaa 0:9b60815f8984 93 float MPU9250::getAres() {
odaa 0:9b60815f8984 94 switch (Ascale)
odaa 0:9b60815f8984 95 {
odaa 0:9b60815f8984 96 // Possible accelerometer scales (and their register bit settings) are:
odaa 0:9b60815f8984 97 // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
odaa 0:9b60815f8984 98 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
odaa 0:9b60815f8984 99 case AFS_2G:
odaa 0:9b60815f8984 100 aRes = 2.0/32768.0;
odaa 0:9b60815f8984 101 break;
odaa 0:9b60815f8984 102 case AFS_4G:
odaa 0:9b60815f8984 103 aRes = 4.0/32768.0;
odaa 0:9b60815f8984 104 break;
odaa 0:9b60815f8984 105 case AFS_8G:
odaa 0:9b60815f8984 106 aRes = 8.0/32768.0;
odaa 0:9b60815f8984 107 break;
odaa 0:9b60815f8984 108 case AFS_16G:
odaa 0:9b60815f8984 109 aRes = 16.0/32768.0;
odaa 0:9b60815f8984 110 break;
odaa 0:9b60815f8984 111 }
odaa 0:9b60815f8984 112 return aRes;
odaa 0:9b60815f8984 113 }
odaa 0:9b60815f8984 114
odaa 0:9b60815f8984 115
odaa 0:9b60815f8984 116 void MPU9250::readAccelData(int16_t * destination)
odaa 0:9b60815f8984 117 {
odaa 0:9b60815f8984 118 uint8_t rawData[6]; // x/y/z accel register data stored here
odaa 0:9b60815f8984 119 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
odaa 0:9b60815f8984 120 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
odaa 0:9b60815f8984 121 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
odaa 0:9b60815f8984 122 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
odaa 0:9b60815f8984 123 }
odaa 0:9b60815f8984 124
odaa 0:9b60815f8984 125 void MPU9250::readGyroData(int16_t * destination)
odaa 0:9b60815f8984 126 {
odaa 0:9b60815f8984 127 uint8_t rawData[6]; // x/y/z gyro register data stored here
odaa 0:9b60815f8984 128 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
odaa 0:9b60815f8984 129 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
odaa 0:9b60815f8984 130 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
odaa 0:9b60815f8984 131 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
odaa 0:9b60815f8984 132 }
odaa 0:9b60815f8984 133
odaa 0:9b60815f8984 134 void MPU9250::readMagData(int16_t * destination)
odaa 0:9b60815f8984 135 {
odaa 0:9b60815f8984 136 uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
odaa 0:9b60815f8984 137 if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
odaa 0:9b60815f8984 138 readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array
odaa 0:9b60815f8984 139 uint8_t c = rawData[6]; // End data read by reading ST2 register
odaa 0:9b60815f8984 140 if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
odaa 0:9b60815f8984 141 destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
odaa 0:9b60815f8984 142 destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian
odaa 0:9b60815f8984 143 destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ;
odaa 0:9b60815f8984 144 }
odaa 0:9b60815f8984 145 }
odaa 0:9b60815f8984 146 }
odaa 0:9b60815f8984 147
odaa 0:9b60815f8984 148 int16_t MPU9250::readTempData()
odaa 0:9b60815f8984 149 {
odaa 0:9b60815f8984 150 uint8_t rawData[2]; // x/y/z gyro register data stored here
odaa 0:9b60815f8984 151 readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
odaa 0:9b60815f8984 152 return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]) ; // Turn the MSB and LSB into a 16-bit value
odaa 0:9b60815f8984 153 }
odaa 0:9b60815f8984 154
odaa 0:9b60815f8984 155
odaa 0:9b60815f8984 156 void MPU9250::resetMPU9250() {
odaa 0:9b60815f8984 157 // reset device
odaa 0:9b60815f8984 158 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
odaa 0:9b60815f8984 159 wait(0.1);
odaa 0:9b60815f8984 160 }
odaa 0:9b60815f8984 161
odaa 0:9b60815f8984 162 void MPU9250::initAK8963(float * destination)
odaa 0:9b60815f8984 163 {
odaa 0:9b60815f8984 164 // First extract the factory calibration for each magnetometer axis
odaa 0:9b60815f8984 165 uint8_t rawData[3]; // x/y/z gyro calibration data stored here
odaa 0:9b60815f8984 166 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
odaa 0:9b60815f8984 167 wait(0.01);
odaa 0:9b60815f8984 168 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
odaa 0:9b60815f8984 169 wait(0.01);
odaa 0:9b60815f8984 170 readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values
odaa 0:9b60815f8984 171 destination[0] = (float)(rawData[0] - 128)/256.0f + 1.0f; // Return x-axis sensitivity adjustment values, etc.
odaa 0:9b60815f8984 172 destination[1] = (float)(rawData[1] - 128)/256.0f + 1.0f;
odaa 0:9b60815f8984 173 destination[2] = (float)(rawData[2] - 128)/256.0f + 1.0f;
odaa 0:9b60815f8984 174 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
odaa 0:9b60815f8984 175 wait(0.01);
odaa 0:9b60815f8984 176 // Configure the magnetometer for continuous read and highest resolution
odaa 0:9b60815f8984 177 // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
odaa 0:9b60815f8984 178 // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
odaa 0:9b60815f8984 179 writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR
odaa 0:9b60815f8984 180 wait(0.01);
odaa 0:9b60815f8984 181 }
odaa 0:9b60815f8984 182
odaa 0:9b60815f8984 183
odaa 0:9b60815f8984 184 void MPU9250::initMPU9250()
odaa 0:9b60815f8984 185 {
odaa 0:9b60815f8984 186 // Initialize MPU9250 device
odaa 0:9b60815f8984 187 // wake up device
odaa 0:9b60815f8984 188 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors
odaa 0:9b60815f8984 189 wait(0.1); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt
odaa 0:9b60815f8984 190
odaa 0:9b60815f8984 191 // get stable time source
odaa 0:9b60815f8984 192 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
odaa 0:9b60815f8984 193
odaa 0:9b60815f8984 194 // Configure Gyro and Accelerometer
odaa 0:9b60815f8984 195 // Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively;
odaa 0:9b60815f8984 196 // DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both
odaa 0:9b60815f8984 197 // Maximum delay is 4.9 ms which is just over a 200 Hz maximum rate
odaa 0:9b60815f8984 198 writeByte(MPU9250_ADDRESS, CONFIG, 0x03);
odaa 0:9b60815f8984 199
odaa 0:9b60815f8984 200 // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
odaa 0:9b60815f8984 201 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; the same rate set in CONFIG above
odaa 0:9b60815f8984 202
odaa 0:9b60815f8984 203 // Set gyroscope full scale range
odaa 0:9b60815f8984 204 // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
odaa 0:9b60815f8984 205 uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG);
odaa 0:9b60815f8984 206 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0xE0); // Clear self-test bits [7:5]
odaa 0:9b60815f8984 207 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
odaa 0:9b60815f8984 208 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c | Gscale << 3); // Set full scale range for the gyro
odaa 0:9b60815f8984 209
odaa 0:9b60815f8984 210 // Set accelerometer configuration
odaa 0:9b60815f8984 211 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG);
odaa 0:9b60815f8984 212 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0xE0); // Clear self-test bits [7:5]
odaa 0:9b60815f8984 213 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
odaa 0:9b60815f8984 214 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c | Ascale << 3); // Set full scale range for the accelerometer
odaa 0:9b60815f8984 215
odaa 0:9b60815f8984 216 // Set accelerometer sample rate configuration
odaa 0:9b60815f8984 217 // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
odaa 0:9b60815f8984 218 // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
odaa 0:9b60815f8984 219 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2);
odaa 0:9b60815f8984 220 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c & ~0x0F); // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
odaa 0:9b60815f8984 221 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c | 0x03); // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
odaa 0:9b60815f8984 222
odaa 0:9b60815f8984 223 // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
odaa 0:9b60815f8984 224 // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting
odaa 0:9b60815f8984 225
odaa 0:9b60815f8984 226 // Configure Interrupts and Bypass Enable
odaa 0:9b60815f8984 227 // Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips
odaa 0:9b60815f8984 228 // can join the I2C bus and all can be controlled by the Arduino as master
odaa 0:9b60815f8984 229 writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);
odaa 0:9b60815f8984 230 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
odaa 0:9b60815f8984 231 }
odaa 0:9b60815f8984 232
odaa 0:9b60815f8984 233
odaa 0:9b60815f8984 234 void MPU9250::setWakeOnMotion()
odaa 0:9b60815f8984 235 {
odaa 0:9b60815f8984 236 // resetMPU9250();
odaa 0:9b60815f8984 237
odaa 0:9b60815f8984 238 /* Make Sure Accel is running:
odaa 0:9b60815f8984 239 • In PWR_MGMT_1 (0x6B) make CYCLE =0, SLEEP = 0 and STANDBY = 0
odaa 0:9b60815f8984 240 • In PWR_MGMT_2 (0x6C) set DIS_XA, DIS_YA, DIS_ZA = 0 and DIS_XG, DIS_YG, DIS_ZG = 1 */
odaa 0:9b60815f8984 241 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00);
odaa 0:9b60815f8984 242 writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x07);
odaa 0:9b60815f8984 243
odaa 0:9b60815f8984 244 /* Set Accel LPF setting to 184 Hz Bandwidth:
odaa 0:9b60815f8984 245 • In ACCEL_CONFIG 2 (0x1D) set ACCEL_FCHOICE_B = 1 and A_DLPFCFG[2:]=1(b001) */
odaa 0:9b60815f8984 246 // writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x05);
odaa 0:9b60815f8984 247 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x01);
odaa 0:9b60815f8984 248
odaa 0:9b60815f8984 249 /* Enable Motion Interrupt:
odaa 0:9b60815f8984 250 • In INT_ENABLE (0x38), set the whole register to 0x40 to enable motion interrupt only */
odaa 0:9b60815f8984 251 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x40);
odaa 0:9b60815f8984 252
odaa 0:9b60815f8984 253 /* Enable Accel Hardware Intelligence:
odaa 0:9b60815f8984 254 • In MOT_DETECT_CTRL (0x69), set ACCEL_INTEL_EN = 1 and ACCEL_INTEL_MODE = 1 */
odaa 0:9b60815f8984 255 writeByte(MPU9250_ADDRESS, MOT_DETECT_CTRL, 0xC0);
odaa 0:9b60815f8984 256
odaa 0:9b60815f8984 257 /* Set Motion Threshold:
odaa 0:9b60815f8984 258 • In WOM_THR (0x1F), set the WOM_Threshold [7:0] to 1~255 LSBs (0~1020mg) */
odaa 0:9b60815f8984 259 writeByte(MPU9250_ADDRESS, WOM_THR, 0x64);
odaa 0:9b60815f8984 260 // writeByte(MPU9250_ADDRESS, WOM_THR, 0xFF);
odaa 0:9b60815f8984 261
odaa 0:9b60815f8984 262 /* Set Frequency of Wake-up:
odaa 0:9b60815f8984 263 • In LP_ACCEL_ODR (0x1E), set Lposc_clksel [3:0] = 0.24Hz ~ 500Hz */
odaa 0:9b60815f8984 264 writeByte(MPU9250_ADDRESS, LP_ACCEL_ODR, 0x07);
odaa 0:9b60815f8984 265
odaa 0:9b60815f8984 266 /* Enable Cycle Mode (Accel Low Power Mode):
odaa 0:9b60815f8984 267 • In PWR_MGMT_1 (0x6B) make CYCLE =1 */
odaa 0:9b60815f8984 268 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x20);
odaa 0:9b60815f8984 269 }
odaa 0:9b60815f8984 270
odaa 0:9b60815f8984 271
odaa 0:9b60815f8984 272 // Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
odaa 0:9b60815f8984 273 // of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
odaa 0:9b60815f8984 274 void MPU9250::calibrateMPU9250(float * dest1, float * dest2)
odaa 0:9b60815f8984 275 {
odaa 0:9b60815f8984 276 uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
odaa 0:9b60815f8984 277 uint16_t ii, packet_count, fifo_count;
odaa 0:9b60815f8984 278 int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
odaa 0:9b60815f8984 279
odaa 0:9b60815f8984 280 // reset device, reset all registers, clear gyro and accelerometer bias registers
odaa 0:9b60815f8984 281 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
odaa 0:9b60815f8984 282 wait(0.1);
odaa 0:9b60815f8984 283
odaa 0:9b60815f8984 284 // get stable time source
odaa 0:9b60815f8984 285 // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
odaa 0:9b60815f8984 286 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
odaa 0:9b60815f8984 287 writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
odaa 0:9b60815f8984 288 wait(0.2);
odaa 0:9b60815f8984 289
odaa 0:9b60815f8984 290 // Configure device for bias calculation
odaa 0:9b60815f8984 291 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
odaa 0:9b60815f8984 292 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
odaa 0:9b60815f8984 293 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source
odaa 0:9b60815f8984 294 writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
odaa 0:9b60815f8984 295 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes
odaa 0:9b60815f8984 296 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP
odaa 0:9b60815f8984 297 wait(0.015);
odaa 0:9b60815f8984 298
odaa 0:9b60815f8984 299 // Configure MPU9250 gyro and accelerometer for bias calculation
odaa 0:9b60815f8984 300 writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz
odaa 0:9b60815f8984 301 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
odaa 0:9b60815f8984 302 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity
odaa 0:9b60815f8984 303 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
odaa 0:9b60815f8984 304
odaa 0:9b60815f8984 305 uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
odaa 0:9b60815f8984 306 uint16_t accelsensitivity = 16384; // = 16384 LSB/g
odaa 0:9b60815f8984 307
odaa 0:9b60815f8984 308 // Configure FIFO to capture accelerometer and gyro data for bias calculation
odaa 0:9b60815f8984 309 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
odaa 0:9b60815f8984 310 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250)
odaa 0:9b60815f8984 311 wait(0.04); // accumulate 40 samples in 80 milliseconds = 480 bytes
odaa 0:9b60815f8984 312
odaa 0:9b60815f8984 313 // At end of sample accumulation, turn off FIFO sensor read
odaa 0:9b60815f8984 314 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO
odaa 0:9b60815f8984 315 readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
odaa 0:9b60815f8984 316 fifo_count = ((uint16_t)data[0] << 8) | data[1];
odaa 0:9b60815f8984 317 packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging
odaa 0:9b60815f8984 318
odaa 0:9b60815f8984 319 for (ii = 0; ii < packet_count; ii++) {
odaa 0:9b60815f8984 320 int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
odaa 0:9b60815f8984 321 readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
odaa 0:9b60815f8984 322 accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO
odaa 0:9b60815f8984 323 accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ;
odaa 0:9b60815f8984 324 accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ;
odaa 0:9b60815f8984 325 gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ;
odaa 0:9b60815f8984 326 gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ;
odaa 0:9b60815f8984 327 gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
odaa 0:9b60815f8984 328
odaa 0:9b60815f8984 329 accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
odaa 0:9b60815f8984 330 accel_bias[1] += (int32_t) accel_temp[1];
odaa 0:9b60815f8984 331 accel_bias[2] += (int32_t) accel_temp[2];
odaa 0:9b60815f8984 332 gyro_bias[0] += (int32_t) gyro_temp[0];
odaa 0:9b60815f8984 333 gyro_bias[1] += (int32_t) gyro_temp[1];
odaa 0:9b60815f8984 334 gyro_bias[2] += (int32_t) gyro_temp[2];
odaa 0:9b60815f8984 335
odaa 0:9b60815f8984 336 }
odaa 0:9b60815f8984 337 accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases
odaa 0:9b60815f8984 338 accel_bias[1] /= (int32_t) packet_count;
odaa 0:9b60815f8984 339 accel_bias[2] /= (int32_t) packet_count;
odaa 0:9b60815f8984 340 gyro_bias[0] /= (int32_t) packet_count;
odaa 0:9b60815f8984 341 gyro_bias[1] /= (int32_t) packet_count;
odaa 0:9b60815f8984 342 gyro_bias[2] /= (int32_t) packet_count;
odaa 0:9b60815f8984 343
odaa 0:9b60815f8984 344 if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation
odaa 0:9b60815f8984 345 else {accel_bias[2] += (int32_t) accelsensitivity;}
odaa 0:9b60815f8984 346
odaa 0:9b60815f8984 347 // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
odaa 0:9b60815f8984 348 data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
odaa 0:9b60815f8984 349 data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
odaa 0:9b60815f8984 350 data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
odaa 0:9b60815f8984 351 data[3] = (-gyro_bias[1]/4) & 0xFF;
odaa 0:9b60815f8984 352 data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
odaa 0:9b60815f8984 353 data[5] = (-gyro_bias[2]/4) & 0xFF;
odaa 0:9b60815f8984 354
odaa 0:9b60815f8984 355 /// Push gyro biases to hardware registers
odaa 0:9b60815f8984 356 /* writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);
odaa 0:9b60815f8984 357 writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);
odaa 0:9b60815f8984 358 writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);
odaa 0:9b60815f8984 359 writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);
odaa 0:9b60815f8984 360 writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);
odaa 0:9b60815f8984 361 writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);
odaa 0:9b60815f8984 362 */
odaa 0:9b60815f8984 363 dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction
odaa 0:9b60815f8984 364 dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
odaa 0:9b60815f8984 365 dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
odaa 0:9b60815f8984 366
odaa 0:9b60815f8984 367 // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
odaa 0:9b60815f8984 368 // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
odaa 0:9b60815f8984 369 // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
odaa 0:9b60815f8984 370 // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
odaa 0:9b60815f8984 371 // the accelerometer biases calculated above must be divided by 8.
odaa 0:9b60815f8984 372
odaa 0:9b60815f8984 373 int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
odaa 0:9b60815f8984 374 readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
odaa 0:9b60815f8984 375 accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1];
odaa 0:9b60815f8984 376 readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
odaa 0:9b60815f8984 377 accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1];
odaa 0:9b60815f8984 378 readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
odaa 0:9b60815f8984 379 accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1];
odaa 0:9b60815f8984 380
odaa 0:9b60815f8984 381 uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
odaa 0:9b60815f8984 382 uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
odaa 0:9b60815f8984 383
odaa 0:9b60815f8984 384 for(ii = 0; ii < 3; ii++) {
odaa 0:9b60815f8984 385 if(accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
odaa 0:9b60815f8984 386 }
odaa 0:9b60815f8984 387
odaa 0:9b60815f8984 388 // Construct total accelerometer bias, including calculated average accelerometer bias from above
odaa 0:9b60815f8984 389 accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
odaa 0:9b60815f8984 390 accel_bias_reg[1] -= (accel_bias[1]/8);
odaa 0:9b60815f8984 391 accel_bias_reg[2] -= (accel_bias[2]/8);
odaa 0:9b60815f8984 392
odaa 0:9b60815f8984 393 data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
odaa 0:9b60815f8984 394 data[1] = (accel_bias_reg[0]) & 0xFF;
odaa 0:9b60815f8984 395 data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
odaa 0:9b60815f8984 396 data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
odaa 0:9b60815f8984 397 data[3] = (accel_bias_reg[1]) & 0xFF;
odaa 0:9b60815f8984 398 data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
odaa 0:9b60815f8984 399 data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
odaa 0:9b60815f8984 400 data[5] = (accel_bias_reg[2]) & 0xFF;
odaa 0:9b60815f8984 401 data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
odaa 0:9b60815f8984 402
odaa 0:9b60815f8984 403 // Apparently this is not working for the acceleration biases in the MPU-9250
odaa 0:9b60815f8984 404 // Are we handling the temperature correction bit properly?
odaa 0:9b60815f8984 405 // Push accelerometer biases to hardware registers
odaa 0:9b60815f8984 406 /* writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]);
odaa 0:9b60815f8984 407 writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]);
odaa 0:9b60815f8984 408 writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]);
odaa 0:9b60815f8984 409 writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]);
odaa 0:9b60815f8984 410 writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);
odaa 0:9b60815f8984 411 writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]);
odaa 0:9b60815f8984 412 */
odaa 0:9b60815f8984 413 // Output scaled accelerometer biases for manual subtraction in the main program
odaa 0:9b60815f8984 414 dest2[0] = (float)accel_bias[0]/(float)accelsensitivity;
odaa 0:9b60815f8984 415 dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
odaa 0:9b60815f8984 416 dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
odaa 0:9b60815f8984 417 }
odaa 0:9b60815f8984 418
odaa 0:9b60815f8984 419
odaa 0:9b60815f8984 420 // Accelerometer and gyroscope self test; check calibration wrt factory settings
odaa 0:9b60815f8984 421 void MPU9250::MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
odaa 0:9b60815f8984 422 {
odaa 0:9b60815f8984 423 uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
odaa 0:9b60815f8984 424 uint8_t selfTest[6];
odaa 0:9b60815f8984 425 int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3];
odaa 0:9b60815f8984 426 float factoryTrim[6];
odaa 0:9b60815f8984 427 uint8_t FS = 0;
odaa 0:9b60815f8984 428
odaa 0:9b60815f8984 429 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz
odaa 0:9b60815f8984 430 writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
odaa 0:9b60815f8984 431 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<<FS); // Set full scale range for the gyro to 250 dps
odaa 0:9b60815f8984 432 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
odaa 0:9b60815f8984 433 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g
odaa 0:9b60815f8984 434
odaa 0:9b60815f8984 435 for( int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer
odaa 0:9b60815f8984 436
odaa 0:9b60815f8984 437 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
odaa 0:9b60815f8984 438 aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
odaa 0:9b60815f8984 439 aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
odaa 0:9b60815f8984 440 aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
odaa 0:9b60815f8984 441
odaa 0:9b60815f8984 442 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
odaa 0:9b60815f8984 443 gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
odaa 0:9b60815f8984 444 gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
odaa 0:9b60815f8984 445 gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
odaa 0:9b60815f8984 446 }
odaa 0:9b60815f8984 447
odaa 0:9b60815f8984 448 for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average current readings
odaa 0:9b60815f8984 449 aAvg[ii] /= 200;
odaa 0:9b60815f8984 450 gAvg[ii] /= 200;
odaa 0:9b60815f8984 451 }
odaa 0:9b60815f8984 452
odaa 0:9b60815f8984 453 // Configure the accelerometer for self-test
odaa 0:9b60815f8984 454 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
odaa 0:9b60815f8984 455 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
odaa 0:9b60815f8984 456 wait_ms(25); // Delay a while to let the device stabilize
odaa 0:9b60815f8984 457
odaa 0:9b60815f8984 458 for( int ii = 0; ii < 200; ii++) { // get average self-test values of gyro and acclerometer
odaa 0:9b60815f8984 459
odaa 0:9b60815f8984 460 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
odaa 0:9b60815f8984 461 aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
odaa 0:9b60815f8984 462 aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
odaa 0:9b60815f8984 463 aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
odaa 0:9b60815f8984 464
odaa 0:9b60815f8984 465 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
odaa 0:9b60815f8984 466 gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
odaa 0:9b60815f8984 467 gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
odaa 0:9b60815f8984 468 gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
odaa 0:9b60815f8984 469 }
odaa 0:9b60815f8984 470
odaa 0:9b60815f8984 471 for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average self-test readings
odaa 0:9b60815f8984 472 aSTAvg[ii] /= 200;
odaa 0:9b60815f8984 473 gSTAvg[ii] /= 200;
odaa 0:9b60815f8984 474 }
odaa 0:9b60815f8984 475
odaa 0:9b60815f8984 476 // Configure the gyro and accelerometer for normal operation
odaa 0:9b60815f8984 477 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);
odaa 0:9b60815f8984 478 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);
odaa 0:9b60815f8984 479 wait_ms(25); // Delay a while to let the device stabilize
odaa 0:9b60815f8984 480
odaa 0:9b60815f8984 481 // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
odaa 0:9b60815f8984 482 selfTest[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results
odaa 0:9b60815f8984 483 selfTest[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results
odaa 0:9b60815f8984 484 selfTest[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results
odaa 0:9b60815f8984 485 selfTest[3] = readByte(MPU9250_ADDRESS, SELF_TEST_X_GYRO); // X-axis gyro self-test results
odaa 0:9b60815f8984 486 selfTest[4] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_GYRO); // Y-axis gyro self-test results
odaa 0:9b60815f8984 487 selfTest[5] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_GYRO); // Z-axis gyro self-test results
odaa 0:9b60815f8984 488
odaa 0:9b60815f8984 489 // Retrieve factory self-test value from self-test code reads
odaa 0:9b60815f8984 490 factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation
odaa 0:9b60815f8984 491 factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation
odaa 0:9b60815f8984 492 factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation
odaa 0:9b60815f8984 493 factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation
odaa 0:9b60815f8984 494 factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation
odaa 0:9b60815f8984 495 factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation
odaa 0:9b60815f8984 496
odaa 0:9b60815f8984 497 // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
odaa 0:9b60815f8984 498 // To get percent, must multiply by 100
odaa 0:9b60815f8984 499 for (int i = 0; i < 3; i++) {
odaa 0:9b60815f8984 500 destination[i] = 100.0*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i]; // Report percent differences
odaa 0:9b60815f8984 501 destination[i+3] = 100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3]; // Report percent differences
odaa 0:9b60815f8984 502 }
odaa 0:9b60815f8984 503
odaa 0:9b60815f8984 504 }
odaa 0:9b60815f8984 505
odaa 0:9b60815f8984 506
odaa 0:9b60815f8984 507
odaa 0:9b60815f8984 508 // Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
odaa 0:9b60815f8984 509 // (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
odaa 0:9b60815f8984 510 // which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
odaa 0:9b60815f8984 511 // device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
odaa 0:9b60815f8984 512 // The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
odaa 0:9b60815f8984 513 // but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
odaa 0:9b60815f8984 514 void MPU9250::MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
odaa 0:9b60815f8984 515 {
odaa 0:9b60815f8984 516 float GyroMeasError = PI * (60.0f / 180.0f); // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
odaa 0:9b60815f8984 517 float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta
odaa 0:9b60815f8984 518 float GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
odaa 0:9b60815f8984 519 float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
odaa 0:9b60815f8984 520 q[0] = 1.0f;
odaa 0:9b60815f8984 521 q[1] = 0.0f;
odaa 0:9b60815f8984 522 q[2] = 0.0f;
odaa 0:9b60815f8984 523 q[3] = 0.0f;
odaa 0:9b60815f8984 524 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
odaa 0:9b60815f8984 525 float norm;
odaa 0:9b60815f8984 526 float hx, hy, _2bx, _2bz;
odaa 0:9b60815f8984 527 float s1, s2, s3, s4;
odaa 0:9b60815f8984 528 float qDot1, qDot2, qDot3, qDot4;
odaa 0:9b60815f8984 529
odaa 0:9b60815f8984 530 // Auxiliary variables to avoid repeated arithmetic
odaa 0:9b60815f8984 531 float _2q1mx;
odaa 0:9b60815f8984 532 float _2q1my;
odaa 0:9b60815f8984 533 float _2q1mz;
odaa 0:9b60815f8984 534 float _2q2mx;
odaa 0:9b60815f8984 535 float _4bx;
odaa 0:9b60815f8984 536 float _4bz;
odaa 0:9b60815f8984 537 float _2q1 = 2.0f * q1;
odaa 0:9b60815f8984 538 float _2q2 = 2.0f * q2;
odaa 0:9b60815f8984 539 float _2q3 = 2.0f * q3;
odaa 0:9b60815f8984 540 float _2q4 = 2.0f * q4;
odaa 0:9b60815f8984 541 float _2q1q3 = 2.0f * q1 * q3;
odaa 0:9b60815f8984 542 float _2q3q4 = 2.0f * q3 * q4;
odaa 0:9b60815f8984 543 float q1q1 = q1 * q1;
odaa 0:9b60815f8984 544 float q1q2 = q1 * q2;
odaa 0:9b60815f8984 545 float q1q3 = q1 * q3;
odaa 0:9b60815f8984 546 float q1q4 = q1 * q4;
odaa 0:9b60815f8984 547 float q2q2 = q2 * q2;
odaa 0:9b60815f8984 548 float q2q3 = q2 * q3;
odaa 0:9b60815f8984 549 float q2q4 = q2 * q4;
odaa 0:9b60815f8984 550 float q3q3 = q3 * q3;
odaa 0:9b60815f8984 551 float q3q4 = q3 * q4;
odaa 0:9b60815f8984 552 float q4q4 = q4 * q4;
odaa 0:9b60815f8984 553
odaa 0:9b60815f8984 554 // Normalise accelerometer measurement
odaa 0:9b60815f8984 555 norm = sqrt(ax * ax + ay * ay + az * az);
odaa 0:9b60815f8984 556 if (norm == 0.0f) return; // handle NaN
odaa 0:9b60815f8984 557 norm = 1.0f/norm;
odaa 0:9b60815f8984 558 ax *= norm;
odaa 0:9b60815f8984 559 ay *= norm;
odaa 0:9b60815f8984 560 az *= norm;
odaa 0:9b60815f8984 561
odaa 0:9b60815f8984 562 // Normalise magnetometer measurement
odaa 0:9b60815f8984 563 norm = sqrt(mx * mx + my * my + mz * mz);
odaa 0:9b60815f8984 564 if (norm == 0.0f) return; // handle NaN
odaa 0:9b60815f8984 565 norm = 1.0f/norm;
odaa 0:9b60815f8984 566 mx *= norm;
odaa 0:9b60815f8984 567 my *= norm;
odaa 0:9b60815f8984 568 mz *= norm;
odaa 0:9b60815f8984 569
odaa 0:9b60815f8984 570 // Reference direction of Earth's magnetic field
odaa 0:9b60815f8984 571 _2q1mx = 2.0f * q1 * mx;
odaa 0:9b60815f8984 572 _2q1my = 2.0f * q1 * my;
odaa 0:9b60815f8984 573 _2q1mz = 2.0f * q1 * mz;
odaa 0:9b60815f8984 574 _2q2mx = 2.0f * q2 * mx;
odaa 0:9b60815f8984 575 hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
odaa 0:9b60815f8984 576 hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
odaa 0:9b60815f8984 577 _2bx = sqrt(hx * hx + hy * hy);
odaa 0:9b60815f8984 578 _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
odaa 0:9b60815f8984 579 _4bx = 2.0f * _2bx;
odaa 0:9b60815f8984 580 _4bz = 2.0f * _2bz;
odaa 0:9b60815f8984 581
odaa 0:9b60815f8984 582 // Gradient decent algorithm corrective step
odaa 0:9b60815f8984 583 s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
odaa 0:9b60815f8984 584 s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
odaa 0:9b60815f8984 585 s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
odaa 0:9b60815f8984 586 s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
odaa 0:9b60815f8984 587 norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
odaa 0:9b60815f8984 588 norm = 1.0f/norm;
odaa 0:9b60815f8984 589 s1 *= norm;
odaa 0:9b60815f8984 590 s2 *= norm;
odaa 0:9b60815f8984 591 s3 *= norm;
odaa 0:9b60815f8984 592 s4 *= norm;
odaa 0:9b60815f8984 593
odaa 0:9b60815f8984 594 // Compute rate of change of quaternion
odaa 0:9b60815f8984 595 qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
odaa 0:9b60815f8984 596 qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
odaa 0:9b60815f8984 597 qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
odaa 0:9b60815f8984 598 qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
odaa 0:9b60815f8984 599
odaa 0:9b60815f8984 600 // Integrate to yield quaternion
odaa 0:9b60815f8984 601 q1 += qDot1 * deltat;
odaa 0:9b60815f8984 602 q2 += qDot2 * deltat;
odaa 0:9b60815f8984 603 q3 += qDot3 * deltat;
odaa 0:9b60815f8984 604 q4 += qDot4 * deltat;
odaa 0:9b60815f8984 605 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
odaa 0:9b60815f8984 606 norm = 1.0f/norm;
odaa 0:9b60815f8984 607 q[0] = q1 * norm;
odaa 0:9b60815f8984 608 q[1] = q2 * norm;
odaa 0:9b60815f8984 609 q[2] = q3 * norm;
odaa 0:9b60815f8984 610 q[3] = q4 * norm;
odaa 0:9b60815f8984 611
odaa 0:9b60815f8984 612 }
odaa 0:9b60815f8984 613
odaa 0:9b60815f8984 614
odaa 0:9b60815f8984 615
odaa 0:9b60815f8984 616 // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
odaa 0:9b60815f8984 617 // measured ones.
odaa 0:9b60815f8984 618 void MPU9250::MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
odaa 0:9b60815f8984 619 {
odaa 0:9b60815f8984 620 float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method
odaa 0:9b60815f8984 621 q[0] = 1.0f;
odaa 0:9b60815f8984 622 q[1] = 0.0f;
odaa 0:9b60815f8984 623 q[2] = 0.0f;
odaa 0:9b60815f8984 624 q[3] = 0.0f;
odaa 0:9b60815f8984 625 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
odaa 0:9b60815f8984 626 float norm;
odaa 0:9b60815f8984 627 float hx, hy, bx, bz;
odaa 0:9b60815f8984 628 float vx, vy, vz, wx, wy, wz;
odaa 0:9b60815f8984 629 float ex, ey, ez;
odaa 0:9b60815f8984 630 float pa, pb, pc;
odaa 0:9b60815f8984 631
odaa 0:9b60815f8984 632 // Auxiliary variables to avoid repeated arithmetic
odaa 0:9b60815f8984 633 float q1q1 = q1 * q1;
odaa 0:9b60815f8984 634 float q1q2 = q1 * q2;
odaa 0:9b60815f8984 635 float q1q3 = q1 * q3;
odaa 0:9b60815f8984 636 float q1q4 = q1 * q4;
odaa 0:9b60815f8984 637 float q2q2 = q2 * q2;
odaa 0:9b60815f8984 638 float q2q3 = q2 * q3;
odaa 0:9b60815f8984 639 float q2q4 = q2 * q4;
odaa 0:9b60815f8984 640 float q3q3 = q3 * q3;
odaa 0:9b60815f8984 641 float q3q4 = q3 * q4;
odaa 0:9b60815f8984 642 float q4q4 = q4 * q4;
odaa 0:9b60815f8984 643
odaa 0:9b60815f8984 644 // Normalise accelerometer measurement
odaa 0:9b60815f8984 645 norm = sqrt(ax * ax + ay * ay + az * az);
odaa 0:9b60815f8984 646 if (norm == 0.0f) return; // handle NaN
odaa 0:9b60815f8984 647 norm = 1.0f / norm; // use reciprocal for division
odaa 0:9b60815f8984 648 ax *= norm;
odaa 0:9b60815f8984 649 ay *= norm;
odaa 0:9b60815f8984 650 az *= norm;
odaa 0:9b60815f8984 651
odaa 0:9b60815f8984 652 // Normalise magnetometer measurement
odaa 0:9b60815f8984 653 norm = sqrt(mx * mx + my * my + mz * mz);
odaa 0:9b60815f8984 654 if (norm == 0.0f) return; // handle NaN
odaa 0:9b60815f8984 655 norm = 1.0f / norm; // use reciprocal for division
odaa 0:9b60815f8984 656 mx *= norm;
odaa 0:9b60815f8984 657 my *= norm;
odaa 0:9b60815f8984 658 mz *= norm;
odaa 0:9b60815f8984 659
odaa 0:9b60815f8984 660 // Reference direction of Earth's magnetic field
odaa 0:9b60815f8984 661 hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
odaa 0:9b60815f8984 662 hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
odaa 0:9b60815f8984 663 bx = sqrt((hx * hx) + (hy * hy));
odaa 0:9b60815f8984 664 bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
odaa 0:9b60815f8984 665
odaa 0:9b60815f8984 666 // Estimated direction of gravity and magnetic field
odaa 0:9b60815f8984 667 vx = 2.0f * (q2q4 - q1q3);
odaa 0:9b60815f8984 668 vy = 2.0f * (q1q2 + q3q4);
odaa 0:9b60815f8984 669 vz = q1q1 - q2q2 - q3q3 + q4q4;
odaa 0:9b60815f8984 670 wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
odaa 0:9b60815f8984 671 wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
odaa 0:9b60815f8984 672 wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
odaa 0:9b60815f8984 673
odaa 0:9b60815f8984 674 // Error is cross product between estimated direction and measured direction of gravity
odaa 0:9b60815f8984 675 ex = (ay * vz - az * vy) + (my * wz - mz * wy);
odaa 0:9b60815f8984 676 ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
odaa 0:9b60815f8984 677 ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
odaa 0:9b60815f8984 678 if (Ki > 0.0f)
odaa 0:9b60815f8984 679 {
odaa 0:9b60815f8984 680 eInt[0] += ex; // accumulate integral error
odaa 0:9b60815f8984 681 eInt[1] += ey;
odaa 0:9b60815f8984 682 eInt[2] += ez;
odaa 0:9b60815f8984 683 }
odaa 0:9b60815f8984 684 else
odaa 0:9b60815f8984 685 {
odaa 0:9b60815f8984 686 eInt[0] = 0.0f; // prevent integral wind up
odaa 0:9b60815f8984 687 eInt[1] = 0.0f;
odaa 0:9b60815f8984 688 eInt[2] = 0.0f;
odaa 0:9b60815f8984 689 }
odaa 0:9b60815f8984 690
odaa 0:9b60815f8984 691 // Apply feedback terms
odaa 0:9b60815f8984 692 gx = gx + Kp * ex + Ki * eInt[0];
odaa 0:9b60815f8984 693 gy = gy + Kp * ey + Ki * eInt[1];
odaa 0:9b60815f8984 694 gz = gz + Kp * ez + Ki * eInt[2];
odaa 0:9b60815f8984 695
odaa 0:9b60815f8984 696 // Integrate rate of change of quaternion
odaa 0:9b60815f8984 697 pa = q2;
odaa 0:9b60815f8984 698 pb = q3;
odaa 0:9b60815f8984 699 pc = q4;
odaa 0:9b60815f8984 700 q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
odaa 0:9b60815f8984 701 q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
odaa 0:9b60815f8984 702 q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
odaa 0:9b60815f8984 703 q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
odaa 0:9b60815f8984 704
odaa 0:9b60815f8984 705 // Normalise quaternion
odaa 0:9b60815f8984 706 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
odaa 0:9b60815f8984 707 norm = 1.0f / norm;
odaa 0:9b60815f8984 708 q[0] = q1 * norm;
odaa 0:9b60815f8984 709 q[1] = q2 * norm;
odaa 0:9b60815f8984 710 q[2] = q3 * norm;
odaa 0:9b60815f8984 711 q[3] = q4 * norm;
odaa 0:9b60815f8984 712
odaa 0:9b60815f8984 713 }