wenchang fei / mpu9250_i2c

first release fork of MPU9250AHRS from Kris Winer

Dependents:   mbed-os-i2c-test mbed-test-i2c-PCA-biquad-peakdet Mix-code-v2 mbed-os-step-counting ... more

MPU9250.cpp@1:c27bb1a0deca, 2016-10-05 (annotated)

Committer:
elessair
Date:
Wed Oct 05 10:40:48 2016 +0000
Revision:
1:c27bb1a0deca
Parent:
0:76dc2aad77bc
Child:
2:a17f66569378
i2c address update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
elessair 0:76dc2aad77bc 1
elessair 0:76dc2aad77bc 2 #include <mbed.h>
elessair 0:76dc2aad77bc 3 #include "MPU9250.h"
elessair 0:76dc2aad77bc 4
elessair 0:76dc2aad77bc 5
elessair 1:c27bb1a0deca 6 mpu9250::mpu9250(PinName _sda, PinName _scl, uint8_t address) : i2c(_sda, _scl)
elessair 1:c27bb1a0deca 7 {
elessair 1:c27bb1a0deca 8 MPU9250_ADDRESS = address;
elessair 0:76dc2aad77bc 9 i2c.frequency(400000);
elessair 0:76dc2aad77bc 10 }
elessair 0:76dc2aad77bc 11
elessair 0:76dc2aad77bc 12
elessair 0:76dc2aad77bc 13 void mpu9250::writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
elessair 0:76dc2aad77bc 14 {
elessair 0:76dc2aad77bc 15 char data_write[2];
elessair 0:76dc2aad77bc 16 data_write[0] = subAddress;
elessair 0:76dc2aad77bc 17 data_write[1] = data;
elessair 0:76dc2aad77bc 18 i2c.write(address, data_write, 2, 0);
elessair 0:76dc2aad77bc 19 }
elessair 0:76dc2aad77bc 20
elessair 0:76dc2aad77bc 21 char mpu9250::readByte(uint8_t address, uint8_t subAddress)
elessair 0:76dc2aad77bc 22 {
elessair 0:76dc2aad77bc 23 char data[1]; // `data` will store the register data
elessair 0:76dc2aad77bc 24 char data_write[1];
elessair 0:76dc2aad77bc 25 data_write[0] = subAddress;
elessair 0:76dc2aad77bc 26 i2c.write(address, data_write, 1, 1); // no stop
elessair 0:76dc2aad77bc 27 i2c.read(address, data, 1, 0);
elessair 0:76dc2aad77bc 28 return data[0];
elessair 0:76dc2aad77bc 29 }
elessair 0:76dc2aad77bc 30
elessair 0:76dc2aad77bc 31 void mpu9250::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
elessair 0:76dc2aad77bc 32 {
elessair 0:76dc2aad77bc 33 char data[14];
elessair 0:76dc2aad77bc 34 char data_write[1];
elessair 0:76dc2aad77bc 35 data_write[0] = subAddress;
elessair 0:76dc2aad77bc 36 i2c.write(address, data_write, 1, 1); // no stop
elessair 0:76dc2aad77bc 37 i2c.read(address, data, count, 0);
elessair 0:76dc2aad77bc 38 for(int ii = 0; ii < count; ii++) {
elessair 0:76dc2aad77bc 39 dest[ii] = data[ii];
elessair 0:76dc2aad77bc 40 }
elessair 0:76dc2aad77bc 41 }
elessair 0:76dc2aad77bc 42
elessair 0:76dc2aad77bc 43 bool mpu9250::alive()
elessair 0:76dc2aad77bc 44 {
elessair 0:76dc2aad77bc 45 if(readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250) == 0x71)
elessair 0:76dc2aad77bc 46 return true;
elessair 0:76dc2aad77bc 47 else
elessair 0:76dc2aad77bc 48 return false;
elessair 0:76dc2aad77bc 49 }
elessair 0:76dc2aad77bc 50
elessair 0:76dc2aad77bc 51 void mpu9250::getGres(uint8_t Gscale)
elessair 0:76dc2aad77bc 52 {
elessair 0:76dc2aad77bc 53 switch (Gscale) {
elessair 0:76dc2aad77bc 54 case GFS_250DPS:
elessair 0:76dc2aad77bc 55 gRes = 250.0/32768.0;
elessair 0:76dc2aad77bc 56 break;
elessair 0:76dc2aad77bc 57 case GFS_500DPS:
elessair 0:76dc2aad77bc 58 gRes = 500.0/32768.0;
elessair 0:76dc2aad77bc 59 break;
elessair 0:76dc2aad77bc 60 case GFS_1000DPS:
elessair 0:76dc2aad77bc 61 gRes = 1000.0/32768.0;
elessair 0:76dc2aad77bc 62 break;
elessair 0:76dc2aad77bc 63 case GFS_2000DPS:
elessair 0:76dc2aad77bc 64 gRes = 2000.0/32768.0;
elessair 0:76dc2aad77bc 65 break;
elessair 0:76dc2aad77bc 66 }
elessair 0:76dc2aad77bc 67 }
elessair 0:76dc2aad77bc 68
elessair 0:76dc2aad77bc 69 void mpu9250::getAres(uint8_t Ascale)
elessair 0:76dc2aad77bc 70 {
elessair 0:76dc2aad77bc 71 switch (Ascale) {
elessair 0:76dc2aad77bc 72 case AFS_2G:
elessair 0:76dc2aad77bc 73 aRes = 2.0/32768.0;
elessair 0:76dc2aad77bc 74 break;
elessair 0:76dc2aad77bc 75 case AFS_4G:
elessair 0:76dc2aad77bc 76 aRes = 4.0/32768.0;
elessair 0:76dc2aad77bc 77 break;
elessair 0:76dc2aad77bc 78 case AFS_8G:
elessair 0:76dc2aad77bc 79 aRes = 8.0/32768.0;
elessair 0:76dc2aad77bc 80 break;
elessair 0:76dc2aad77bc 81 case AFS_16G:
elessair 0:76dc2aad77bc 82 aRes = 16.0/32768.0;
elessair 0:76dc2aad77bc 83 break;
elessair 0:76dc2aad77bc 84 }
elessair 0:76dc2aad77bc 85 }
elessair 0:76dc2aad77bc 86
elessair 0:76dc2aad77bc 87 void mpu9250::readAccelData(int16_t * destination)
elessair 0:76dc2aad77bc 88 {
elessair 0:76dc2aad77bc 89 uint8_t rawData[6]; // x/y/z accel register data stored here
elessair 0:76dc2aad77bc 90 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
elessair 0:76dc2aad77bc 91 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
elessair 0:76dc2aad77bc 92 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
elessair 0:76dc2aad77bc 93 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
elessair 0:76dc2aad77bc 94 }
elessair 0:76dc2aad77bc 95
elessair 0:76dc2aad77bc 96 void mpu9250::readGyroData(int16_t * destination)
elessair 0:76dc2aad77bc 97 {
elessair 0:76dc2aad77bc 98 uint8_t rawData[6]; // x/y/z gyro register data stored here
elessair 0:76dc2aad77bc 99 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
elessair 0:76dc2aad77bc 100 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
elessair 0:76dc2aad77bc 101 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
elessair 0:76dc2aad77bc 102 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
elessair 0:76dc2aad77bc 103 }
elessair 0:76dc2aad77bc 104
elessair 0:76dc2aad77bc 105 void mpu9250::readTempData(int16_t * destination)
elessair 0:76dc2aad77bc 106 {
elessair 0:76dc2aad77bc 107 uint8_t rawData[2];
elessair 0:76dc2aad77bc 108 readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
elessair 0:76dc2aad77bc 109 destination[0] = (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]) ; // Turn the MSB and LSB into a 16-bit value
elessair 0:76dc2aad77bc 110 }
elessair 0:76dc2aad77bc 111
elessair 0:76dc2aad77bc 112 void mpu9250::readAll(int16_t * destinationAcc, int16_t * destinationGyro, int16_t * destinationTemp)
elessair 0:76dc2aad77bc 113 {
elessair 0:76dc2aad77bc 114 readAccelData(destinationAcc); // Read the x/y/z adc values
elessair 0:76dc2aad77bc 115 readGyroData(destinationGyro); // Read the x/y/z adc values
elessair 0:76dc2aad77bc 116 readTempData(destinationTemp); // Read the adc values
elessair 0:76dc2aad77bc 117 }
elessair 0:76dc2aad77bc 118
elessair 0:76dc2aad77bc 119 void mpu9250::ReadConvertAll(float * destinationAcc, float * destinationGyro, float * destinationTemp)
elessair 0:76dc2aad77bc 120 {
elessair 0:76dc2aad77bc 121 int16_t AccRead[3];
elessair 0:76dc2aad77bc 122 int16_t GyroRead[3];
elessair 0:76dc2aad77bc 123 int16_t TempRead[1];
elessair 0:76dc2aad77bc 124
elessair 0:76dc2aad77bc 125 readAll(AccRead,GyroRead,TempRead);
elessair 0:76dc2aad77bc 126
elessair 0:76dc2aad77bc 127 destinationAcc[0] = -1000*((float)AccRead[1]*aRes - accelBias[1]); // get actual g value, this depends on scale being set
elessair 0:76dc2aad77bc 128 destinationAcc[1] = -1000*((float)AccRead[0]*aRes - accelBias[0]);
elessair 0:76dc2aad77bc 129 destinationAcc[2] = 1000*((float)AccRead[2]*aRes - accelBias[2]);
elessair 0:76dc2aad77bc 130
elessair 0:76dc2aad77bc 131 destinationGyro[0] = (float)GyroRead[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
elessair 0:76dc2aad77bc 132 destinationGyro[1] = (float)GyroRead[1]*gRes - gyroBias[1];
elessair 0:76dc2aad77bc 133 destinationGyro[2] = (float)GyroRead[2]*gRes - gyroBias[2];
elessair 0:76dc2aad77bc 134
elessair 0:76dc2aad77bc 135 destinationTemp[0] = ((float) TempRead[0]) / 333.87f + 21.0f; // Temperature in degrees Centigrade
elessair 0:76dc2aad77bc 136 }
elessair 0:76dc2aad77bc 137
elessair 0:76dc2aad77bc 138 void mpu9250::resetMPU9250()
elessair 0:76dc2aad77bc 139 {
elessair 0:76dc2aad77bc 140 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
elessair 0:76dc2aad77bc 141 wait(0.1);
elessair 0:76dc2aad77bc 142 }
elessair 0:76dc2aad77bc 143
elessair 0:76dc2aad77bc 144 void mpu9250::initMPU9250(uint8_t Ascale,uint8_t Gscale)
elessair 0:76dc2aad77bc 145 {
elessair 0:76dc2aad77bc 146 resetMPU9250();
elessair 0:76dc2aad77bc 147 wait(0.2);
elessair 0:76dc2aad77bc 148
elessair 0:76dc2aad77bc 149 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors
elessair 0:76dc2aad77bc 150 wait(0.1); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt
elessair 0:76dc2aad77bc 151
elessair 0:76dc2aad77bc 152 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
elessair 0:76dc2aad77bc 153 writeByte(MPU9250_ADDRESS, CONFIG, 0x03);
elessair 0:76dc2aad77bc 154
elessair 0:76dc2aad77bc 155 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; the same rate set in CONFIG above
elessair 0:76dc2aad77bc 156
elessair 0:76dc2aad77bc 157 uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG);
elessair 0:76dc2aad77bc 158 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0xE0); // Clear self-test bits [7:5]
elessair 0:76dc2aad77bc 159 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
elessair 0:76dc2aad77bc 160 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c | Gscale); // Set full scale range for the gyro
elessair 0:76dc2aad77bc 161
elessair 0:76dc2aad77bc 162 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG);
elessair 0:76dc2aad77bc 163 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0xE0); // Clear self-test bits [7:5]
elessair 0:76dc2aad77bc 164 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
elessair 0:76dc2aad77bc 165 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c | Ascale); // Set full scale range for the accelerometer
elessair 0:76dc2aad77bc 166
elessair 0:76dc2aad77bc 167 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2);
elessair 0:76dc2aad77bc 168 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c & ~0x0F); // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
elessair 0:76dc2aad77bc 169 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c | 0x03); // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
elessair 0:76dc2aad77bc 170
elessair 0:76dc2aad77bc 171 // writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);
elessair 0:76dc2aad77bc 172 // writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
elessair 0:76dc2aad77bc 173
elessair 0:76dc2aad77bc 174 getAres(Ascale); // Get accelerometer sensitivity
elessair 0:76dc2aad77bc 175 getGres(Gscale); // Get gyro sensitivity
elessair 0:76dc2aad77bc 176
elessair 0:76dc2aad77bc 177 calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
elessair 0:76dc2aad77bc 178 }
elessair 0:76dc2aad77bc 179
elessair 0:76dc2aad77bc 180
elessair 0:76dc2aad77bc 181 void mpu9250::calibrateMPU9250(float * dest1, float * dest2)
elessair 0:76dc2aad77bc 182 {
elessair 0:76dc2aad77bc 183 uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
elessair 0:76dc2aad77bc 184 uint16_t ii, packet_count, fifo_count;
elessair 0:76dc2aad77bc 185 int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
elessair 0:76dc2aad77bc 186
elessair 0:76dc2aad77bc 187 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
elessair 0:76dc2aad77bc 188 wait(0.1);
elessair 0:76dc2aad77bc 189
elessair 0:76dc2aad77bc 190 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
elessair 0:76dc2aad77bc 191 writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
elessair 0:76dc2aad77bc 192 wait(0.2);
elessair 0:76dc2aad77bc 193
elessair 0:76dc2aad77bc 194 // Configure device for bias calculation
elessair 0:76dc2aad77bc 195 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
elessair 0:76dc2aad77bc 196 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
elessair 0:76dc2aad77bc 197 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source
elessair 0:76dc2aad77bc 198 writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
elessair 0:76dc2aad77bc 199 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes
elessair 0:76dc2aad77bc 200 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP
elessair 0:76dc2aad77bc 201 wait(0.015);
elessair 0:76dc2aad77bc 202
elessair 0:76dc2aad77bc 203 // Configure MPU9250 gyro and accelerometer for bias calculation
elessair 0:76dc2aad77bc 204 writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz
elessair 0:76dc2aad77bc 205 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
elessair 0:76dc2aad77bc 206 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity
elessair 0:76dc2aad77bc 207 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
elessair 0:76dc2aad77bc 208
elessair 0:76dc2aad77bc 209 uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
elessair 0:76dc2aad77bc 210 uint16_t accelsensitivity = 16384; // = 16384 LSB/g
elessair 0:76dc2aad77bc 211
elessair 0:76dc2aad77bc 212 // Configure FIFO to capture accelerometer and gyro data for bias calculation
elessair 0:76dc2aad77bc 213 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
elessair 0:76dc2aad77bc 214 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250)
elessair 0:76dc2aad77bc 215 wait(0.04); // accumulate 40 samples in 80 milliseconds = 480 bytes
elessair 0:76dc2aad77bc 216
elessair 0:76dc2aad77bc 217 // At end of sample accumulation, turn off FIFO sensor read
elessair 0:76dc2aad77bc 218 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO
elessair 0:76dc2aad77bc 219 readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
elessair 0:76dc2aad77bc 220 fifo_count = ((uint16_t)data[0] << 8) | data[1];
elessair 0:76dc2aad77bc 221 packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging
elessair 0:76dc2aad77bc 222
elessair 0:76dc2aad77bc 223 for (ii = 0; ii < packet_count; ii++) {
elessair 0:76dc2aad77bc 224 int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
elessair 0:76dc2aad77bc 225 readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
elessair 0:76dc2aad77bc 226 accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO
elessair 0:76dc2aad77bc 227 accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ;
elessair 0:76dc2aad77bc 228 accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ;
elessair 0:76dc2aad77bc 229 gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ;
elessair 0:76dc2aad77bc 230 gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ;
elessair 0:76dc2aad77bc 231 gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
elessair 0:76dc2aad77bc 232
elessair 0:76dc2aad77bc 233 accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
elessair 0:76dc2aad77bc 234 accel_bias[1] += (int32_t) accel_temp[1];
elessair 0:76dc2aad77bc 235 accel_bias[2] += (int32_t) accel_temp[2];
elessair 0:76dc2aad77bc 236 gyro_bias[0] += (int32_t) gyro_temp[0];
elessair 0:76dc2aad77bc 237 gyro_bias[1] += (int32_t) gyro_temp[1];
elessair 0:76dc2aad77bc 238 gyro_bias[2] += (int32_t) gyro_temp[2];
elessair 0:76dc2aad77bc 239
elessair 0:76dc2aad77bc 240 }
elessair 0:76dc2aad77bc 241 accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases
elessair 0:76dc2aad77bc 242 accel_bias[1] /= (int32_t) packet_count;
elessair 0:76dc2aad77bc 243 accel_bias[2] /= (int32_t) packet_count;
elessair 0:76dc2aad77bc 244 gyro_bias[0] /= (int32_t) packet_count;
elessair 0:76dc2aad77bc 245 gyro_bias[1] /= (int32_t) packet_count;
elessair 0:76dc2aad77bc 246 gyro_bias[2] /= (int32_t) packet_count;
elessair 0:76dc2aad77bc 247
elessair 0:76dc2aad77bc 248 if(accel_bias[2] > 0L) {
elessair 0:76dc2aad77bc 249 accel_bias[2] -= (int32_t) accelsensitivity; // Remove gravity from the z-axis accelerometer bias calculation
elessair 0:76dc2aad77bc 250 } else {
elessair 0:76dc2aad77bc 251 accel_bias[2] += (int32_t) accelsensitivity;
elessair 0:76dc2aad77bc 252 }
elessair 0:76dc2aad77bc 253
elessair 0:76dc2aad77bc 254 // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
elessair 0:76dc2aad77bc 255 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
elessair 0:76dc2aad77bc 256 data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
elessair 0:76dc2aad77bc 257 data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
elessair 0:76dc2aad77bc 258 data[3] = (-gyro_bias[1]/4) & 0xFF;
elessair 0:76dc2aad77bc 259 data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
elessair 0:76dc2aad77bc 260 data[5] = (-gyro_bias[2]/4) & 0xFF;
elessair 0:76dc2aad77bc 261
elessair 0:76dc2aad77bc 262 dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction
elessair 0:76dc2aad77bc 263 dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
elessair 0:76dc2aad77bc 264 dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
elessair 0:76dc2aad77bc 265
elessair 0:76dc2aad77bc 266 int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
elessair 0:76dc2aad77bc 267 readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
elessair 0:76dc2aad77bc 268 accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1];
elessair 0:76dc2aad77bc 269 readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
elessair 0:76dc2aad77bc 270 accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1];
elessair 0:76dc2aad77bc 271 readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
elessair 0:76dc2aad77bc 272 accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1];
elessair 0:76dc2aad77bc 273
elessair 0:76dc2aad77bc 274 uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
elessair 0:76dc2aad77bc 275 uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
elessair 0:76dc2aad77bc 276
elessair 0:76dc2aad77bc 277 for(ii = 0; ii < 3; ii++) {
elessair 0:76dc2aad77bc 278 if(accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
elessair 0:76dc2aad77bc 279 }
elessair 0:76dc2aad77bc 280
elessair 0:76dc2aad77bc 281 // Construct total accelerometer bias, including calculated average accelerometer bias from above
elessair 0:76dc2aad77bc 282 accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
elessair 0:76dc2aad77bc 283 accel_bias_reg[1] -= (accel_bias[1]/8);
elessair 0:76dc2aad77bc 284 accel_bias_reg[2] -= (accel_bias[2]/8);
elessair 0:76dc2aad77bc 285
elessair 0:76dc2aad77bc 286 data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
elessair 0:76dc2aad77bc 287 data[1] = (accel_bias_reg[0]) & 0xFF;
elessair 0:76dc2aad77bc 288 data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
elessair 0:76dc2aad77bc 289 data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
elessair 0:76dc2aad77bc 290 data[3] = (accel_bias_reg[1]) & 0xFF;
elessair 0:76dc2aad77bc 291 data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
elessair 0:76dc2aad77bc 292 data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
elessair 0:76dc2aad77bc 293 data[5] = (accel_bias_reg[2]) & 0xFF;
elessair 0:76dc2aad77bc 294 data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
elessair 0:76dc2aad77bc 295
elessair 0:76dc2aad77bc 296 // Output scaled accelerometer biases for manual subtraction in the main program
elessair 0:76dc2aad77bc 297 dest2[0] = (float)accel_bias[0]/(float)accelsensitivity;
elessair 0:76dc2aad77bc 298 dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
elessair 0:76dc2aad77bc 299 dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
elessair 0:76dc2aad77bc 300 }
elessair 0:76dc2aad77bc 301