CansatB_2021 / Mbed 2 deprecated cansat

地磁気センサーのプログラム

Dependencies:   mbed

gm_sensor/MPU9250.cpp@0:af969b92a87c, 2021-10-20 (annotated)

Committer:
minanao
Date:
Wed Oct 20 08:15:44 2021 +0000
Revision:
0:af969b92a87c
gm_sensor

Who changed what in which revision?

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