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@0:76dc2aad77bc, 2016-10-05 (annotated)

Committer:
elessair
Date:
Wed Oct 05 10:33:45 2016 +0000
Revision:
0:76dc2aad77bc
Child:
1:c27bb1a0deca
fork of MPU9250AHRS from Kris Winer

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