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@3:4f6c69e52534, 2019-11-06 (annotated)

Committer:
castlefei
Date:
Wed Nov 06 12:36:33 2019 +0000
Revision:
3:4f6c69e52534
Parent:
2:a17f66569378 
test changes

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