航空研究会 / MPU9255

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

MPU9255.cpp@0:5a3104f02775, 2020-06-28 (annotated)

Committer:
imanomadao
Date:
Sun Jun 28 11:10:43 2020 +0000
Revision:
0:5a3104f02775
a;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
imanomadao 0:5a3104f02775 1 #include"MPU9255.h"
imanomadao 0:5a3104f02775 2
imanomadao 0:5a3104f02775 3 //-----------------
imanomadao 0:5a3104f02775 4 //private functions
imanomadao 0:5a3104f02775 5 //-----------------
imanomadao 0:5a3104f02775 6
imanomadao 0:5a3104f02775 7 void MPU9255::writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
imanomadao 0:5a3104f02775 8 {
imanomadao 0:5a3104f02775 9 char data_write[2];
imanomadao 0:5a3104f02775 10 data_write[0] = subAddress;
imanomadao 0:5a3104f02775 11 data_write[1] = data;
imanomadao 0:5a3104f02775 12 i2c.write(address, data_write, 2, 0);
imanomadao 0:5a3104f02775 13 }
imanomadao 0:5a3104f02775 14
imanomadao 0:5a3104f02775 15 char MPU9255::readByte(uint8_t address, uint8_t subAddress)
imanomadao 0:5a3104f02775 16 {
imanomadao 0:5a3104f02775 17 char data[1]; // `data` will store the register data
imanomadao 0:5a3104f02775 18 char data_write[1];
imanomadao 0:5a3104f02775 19 data_write[0] = subAddress;
imanomadao 0:5a3104f02775 20 i2c.write(address, data_write, 1, 1); // no stop
imanomadao 0:5a3104f02775 21 i2c.read(address, data, 1, 0);
imanomadao 0:5a3104f02775 22 return data[0];
imanomadao 0:5a3104f02775 23 }
imanomadao 0:5a3104f02775 24
imanomadao 0:5a3104f02775 25 void MPU9255::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
imanomadao 0:5a3104f02775 26 {
imanomadao 0:5a3104f02775 27 char data[14];
imanomadao 0:5a3104f02775 28 char data_write[1];
imanomadao 0:5a3104f02775 29 data_write[0] = subAddress;
imanomadao 0:5a3104f02775 30 i2c.write(address, data_write, 1, 1); // no stop
imanomadao 0:5a3104f02775 31 i2c.read(address, data, count, 0);
imanomadao 0:5a3104f02775 32 for(int ii = 0; ii < count; ii++)
imanomadao 0:5a3104f02775 33 {
imanomadao 0:5a3104f02775 34 dest[ii] = data[ii];
imanomadao 0:5a3104f02775 35 }
imanomadao 0:5a3104f02775 36 }
imanomadao 0:5a3104f02775 37
imanomadao 0:5a3104f02775 38
imanomadao 0:5a3104f02775 39 //----------------
imanomadao 0:5a3104f02775 40 //public functions
imanomadao 0:5a3104f02775 41 //----------------
imanomadao 0:5a3104f02775 42
imanomadao 0:5a3104f02775 43 MPU9255::MPU9255(PinName sda, PinName scl, RawSerial* serial_p)
imanomadao 0:5a3104f02775 44 : i2c_p(new I2C(sda,scl)), i2c(*i2c_p), pc_p(serial_p)
imanomadao 0:5a3104f02775 45 {
imanomadao 0:5a3104f02775 46 i2c.frequency(40000);
imanomadao 0:5a3104f02775 47 }
imanomadao 0:5a3104f02775 48
imanomadao 0:5a3104f02775 49 MPU9255::~MPU9255() {}
imanomadao 0:5a3104f02775 50
imanomadao 0:5a3104f02775 51 uint8_t MPU9255::whoami_mpu9255()
imanomadao 0:5a3104f02775 52 {
imanomadao 0:5a3104f02775 53 uint8_t a = readByte(MPU9255_ADDRESS, WHO_AM_I_MPU9255);
imanomadao 0:5a3104f02775 54 return a;
imanomadao 0:5a3104f02775 55 }
imanomadao 0:5a3104f02775 56
imanomadao 0:5a3104f02775 57 void MPU9255::reset_mpu9255()
imanomadao 0:5a3104f02775 58 {
imanomadao 0:5a3104f02775 59 writeByte(MPU9255_ADDRESS, PWR_MGMT_1, 0x80);
imanomadao 0:5a3104f02775 60 wait_ms(10);
imanomadao 0:5a3104f02775 61 }
imanomadao 0:5a3104f02775 62
imanomadao 0:5a3104f02775 63 void MPU9255::selftest_mpu9255(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
imanomadao 0:5a3104f02775 64 {
imanomadao 0:5a3104f02775 65 uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
imanomadao 0:5a3104f02775 66 uint8_t selfTest[6];
imanomadao 0:5a3104f02775 67 int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0};
imanomadao 0:5a3104f02775 68 float factoryTrim[6];
imanomadao 0:5a3104f02775 69 uint8_t FS = 0;
imanomadao 0:5a3104f02775 70
imanomadao 0:5a3104f02775 71 writeByte(MPU9255_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz
imanomadao 0:5a3104f02775 72 writeByte(MPU9255_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
imanomadao 0:5a3104f02775 73 writeByte(MPU9255_ADDRESS, GYRO_CONFIG, 1<<FS); // Set full scale range for the gyro to 250 dps
imanomadao 0:5a3104f02775 74 writeByte(MPU9255_ADDRESS, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
imanomadao 0:5a3104f02775 75 writeByte(MPU9255_ADDRESS, ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g
imanomadao 0:5a3104f02775 76
imanomadao 0:5a3104f02775 77 for( int ii = 0; ii < 200; ii++) // get average current values of gyro and acclerometer
imanomadao 0:5a3104f02775 78 {
imanomadao 0:5a3104f02775 79 readBytes(MPU9255_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
imanomadao 0:5a3104f02775 80 aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
imanomadao 0:5a3104f02775 81 aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
imanomadao 0:5a3104f02775 82 aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
imanomadao 0:5a3104f02775 83
imanomadao 0:5a3104f02775 84 readBytes(MPU9255_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
imanomadao 0:5a3104f02775 85 gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
imanomadao 0:5a3104f02775 86 gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
imanomadao 0:5a3104f02775 87 gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
imanomadao 0:5a3104f02775 88 }
imanomadao 0:5a3104f02775 89
imanomadao 0:5a3104f02775 90 for (int ii =0; ii < 3; ii++) // Get average of 200 values and store as average current readings
imanomadao 0:5a3104f02775 91 {
imanomadao 0:5a3104f02775 92 aAvg[ii] /= 200;
imanomadao 0:5a3104f02775 93 gAvg[ii] /= 200;
imanomadao 0:5a3104f02775 94 }
imanomadao 0:5a3104f02775 95
imanomadao 0:5a3104f02775 96 // Configure the accelerometer for self-test
imanomadao 0:5a3104f02775 97 writeByte(MPU9255_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
imanomadao 0:5a3104f02775 98 writeByte(MPU9255_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
imanomadao 0:5a3104f02775 99 wait_ms(25); // Delay a while to let the device stabilize
imanomadao 0:5a3104f02775 100
imanomadao 0:5a3104f02775 101 for( int ii = 0; ii < 200; ii++) // get average self-test values of gyro and acclerometer
imanomadao 0:5a3104f02775 102 {
imanomadao 0:5a3104f02775 103 readBytes(MPU9255_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
imanomadao 0:5a3104f02775 104 aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
imanomadao 0:5a3104f02775 105 aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
imanomadao 0:5a3104f02775 106 aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
imanomadao 0:5a3104f02775 107
imanomadao 0:5a3104f02775 108 readBytes(MPU9255_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
imanomadao 0:5a3104f02775 109 gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
imanomadao 0:5a3104f02775 110 gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
imanomadao 0:5a3104f02775 111 gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
imanomadao 0:5a3104f02775 112 }
imanomadao 0:5a3104f02775 113
imanomadao 0:5a3104f02775 114 for (int ii =0; ii < 3; ii++) // Get average of 200 values and store as average self-test readings
imanomadao 0:5a3104f02775 115 {
imanomadao 0:5a3104f02775 116 aSTAvg[ii] /= 200;
imanomadao 0:5a3104f02775 117 gSTAvg[ii] /= 200;
imanomadao 0:5a3104f02775 118 }
imanomadao 0:5a3104f02775 119
imanomadao 0:5a3104f02775 120 // Configure the gyro and accelerometer for normal operation
imanomadao 0:5a3104f02775 121 writeByte(MPU9255_ADDRESS, ACCEL_CONFIG, 0x00);
imanomadao 0:5a3104f02775 122 writeByte(MPU9255_ADDRESS, GYRO_CONFIG, 0x00);
imanomadao 0:5a3104f02775 123 wait_ms(25); // Delay a while to let the device stabilize
imanomadao 0:5a3104f02775 124
imanomadao 0:5a3104f02775 125 // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
imanomadao 0:5a3104f02775 126 selfTest[0] = readByte(MPU9255_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results
imanomadao 0:5a3104f02775 127 selfTest[1] = readByte(MPU9255_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results
imanomadao 0:5a3104f02775 128 selfTest[2] = readByte(MPU9255_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results
imanomadao 0:5a3104f02775 129 selfTest[3] = readByte(MPU9255_ADDRESS, SELF_TEST_X_GYRO); // X-axis gyro self-test results
imanomadao 0:5a3104f02775 130 selfTest[4] = readByte(MPU9255_ADDRESS, SELF_TEST_Y_GYRO); // Y-axis gyro self-test results
imanomadao 0:5a3104f02775 131 selfTest[5] = readByte(MPU9255_ADDRESS, SELF_TEST_Z_GYRO); // Z-axis gyro self-test results
imanomadao 0:5a3104f02775 132
imanomadao 0:5a3104f02775 133 // Retrieve factory self-test value from self-test code reads
imanomadao 0:5a3104f02775 134 factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation
imanomadao 0:5a3104f02775 135 factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation
imanomadao 0:5a3104f02775 136 factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation
imanomadao 0:5a3104f02775 137 factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation
imanomadao 0:5a3104f02775 138 factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation
imanomadao 0:5a3104f02775 139 factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation
imanomadao 0:5a3104f02775 140
imanomadao 0:5a3104f02775 141 // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
imanomadao 0:5a3104f02775 142 // To get percent, must multiply by 100
imanomadao 0:5a3104f02775 143 for (int i = 0; i < 3; i++)
imanomadao 0:5a3104f02775 144 {
imanomadao 0:5a3104f02775 145 destination[i] = 100.0f*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i] - 100.0f; // Report percent differences
imanomadao 0:5a3104f02775 146 destination[i+3] = 100.0f*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3] - 100.0f; // Report percent differences
imanomadao 0:5a3104f02775 147 }
imanomadao 0:5a3104f02775 148
imanomadao 0:5a3104f02775 149 }
imanomadao 0:5a3104f02775 150
imanomadao 0:5a3104f02775 151 float MPU9255::getMres(uint8_t Mscale)
imanomadao 0:5a3104f02775 152 {
imanomadao 0:5a3104f02775 153 float _mRes;
imanomadao 0:5a3104f02775 154 switch (Mscale)
imanomadao 0:5a3104f02775 155 {
imanomadao 0:5a3104f02775 156 // Possible magnetometer scales (and their register bit settings) are:
imanomadao 0:5a3104f02775 157 // 14 bit resolution (0) and 16 bit resolution (1)
imanomadao 0:5a3104f02775 158 case MFS_14BITS:
imanomadao 0:5a3104f02775 159 _mRes = 10.*4912./8190.; // Proper scale to return milliGauss
imanomadao 0:5a3104f02775 160 return _mRes;
imanomadao 0:5a3104f02775 161 //break;
imanomadao 0:5a3104f02775 162 case MFS_16BITS:
imanomadao 0:5a3104f02775 163 _mRes = 10.*4912./32760.0; // Proper scale to return milliGauss (4912/32760=0.15)
imanomadao 0:5a3104f02775 164 return _mRes; // convert 'μT' to 'mG'
imanomadao 0:5a3104f02775 165 //break;
imanomadao 0:5a3104f02775 166 }
imanomadao 0:5a3104f02775 167 }
imanomadao 0:5a3104f02775 168
imanomadao 0:5a3104f02775 169 float MPU9255::getGres(uint8_t Gscale)
imanomadao 0:5a3104f02775 170 {
imanomadao 0:5a3104f02775 171 float _gRes;
imanomadao 0:5a3104f02775 172 switch (Gscale)
imanomadao 0:5a3104f02775 173 {
imanomadao 0:5a3104f02775 174 // Possible gyro scales (and their register bit settings) are:
imanomadao 0:5a3104f02775 175 // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
imanomadao 0:5a3104f02775 176 case GFS_250DPS:
imanomadao 0:5a3104f02775 177 _gRes = 250.0/32768.0;
imanomadao 0:5a3104f02775 178 return _gRes;
imanomadao 0:5a3104f02775 179 //break;
imanomadao 0:5a3104f02775 180 case GFS_500DPS:
imanomadao 0:5a3104f02775 181 _gRes = 500.0/32768.0;
imanomadao 0:5a3104f02775 182 return _gRes;
imanomadao 0:5a3104f02775 183 //break;
imanomadao 0:5a3104f02775 184 case GFS_1000DPS:
imanomadao 0:5a3104f02775 185 _gRes = 1000.0/32768.0;
imanomadao 0:5a3104f02775 186 return _gRes;
imanomadao 0:5a3104f02775 187 //break;
imanomadao 0:5a3104f02775 188 case GFS_2000DPS:
imanomadao 0:5a3104f02775 189 _gRes = 2000.0/32768.0;
imanomadao 0:5a3104f02775 190 return _gRes;
imanomadao 0:5a3104f02775 191 //break;
imanomadao 0:5a3104f02775 192 }
imanomadao 0:5a3104f02775 193 }
imanomadao 0:5a3104f02775 194
imanomadao 0:5a3104f02775 195 float MPU9255::getAres(uint8_t Ascale)
imanomadao 0:5a3104f02775 196 {
imanomadao 0:5a3104f02775 197 float _aRes;
imanomadao 0:5a3104f02775 198 switch (Ascale)
imanomadao 0:5a3104f02775 199 {
imanomadao 0:5a3104f02775 200 // Possible accelerometer scales (and their register bit settings) are:
imanomadao 0:5a3104f02775 201 // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
imanomadao 0:5a3104f02775 202 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
imanomadao 0:5a3104f02775 203 case AFS_2G:
imanomadao 0:5a3104f02775 204 _aRes = 2.0f/32768.0f;
imanomadao 0:5a3104f02775 205 return _aRes;
imanomadao 0:5a3104f02775 206 //break;
imanomadao 0:5a3104f02775 207 case AFS_4G:
imanomadao 0:5a3104f02775 208 _aRes = 4.0f/32768.0f;
imanomadao 0:5a3104f02775 209 return _aRes;
imanomadao 0:5a3104f02775 210 //break;
imanomadao 0:5a3104f02775 211 case AFS_8G:
imanomadao 0:5a3104f02775 212 _aRes = 8.0f/32768.0f;
imanomadao 0:5a3104f02775 213 return _aRes;
imanomadao 0:5a3104f02775 214 //break;
imanomadao 0:5a3104f02775 215 case AFS_16G:
imanomadao 0:5a3104f02775 216 _aRes = 16.0f/32768.0f;
imanomadao 0:5a3104f02775 217 return _aRes;
imanomadao 0:5a3104f02775 218 //break;
imanomadao 0:5a3104f02775 219 }
imanomadao 0:5a3104f02775 220 }
imanomadao 0:5a3104f02775 221
imanomadao 0:5a3104f02775 222 void MPU9255::calibrate_mpu9255(float * dest1, float * dest2)
imanomadao 0:5a3104f02775 223 {
imanomadao 0:5a3104f02775 224 uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
imanomadao 0:5a3104f02775 225 uint16_t ii, packet_count, fifo_count;
imanomadao 0:5a3104f02775 226 int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
imanomadao 0:5a3104f02775 227
imanomadao 0:5a3104f02775 228 // reset device
imanomadao 0:5a3104f02775 229 writeByte(MPU9255_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
imanomadao 0:5a3104f02775 230 wait_ms(100);
imanomadao 0:5a3104f02775 231
imanomadao 0:5a3104f02775 232 // get stable time source; Auto select clock source to be PLL gyroscope reference if ready
imanomadao 0:5a3104f02775 233 // else use the internal oscillator, bits 2:0 = 001
imanomadao 0:5a3104f02775 234 writeByte(MPU9255_ADDRESS, PWR_MGMT_1, 0x01);
imanomadao 0:5a3104f02775 235 writeByte(MPU9255_ADDRESS, PWR_MGMT_2, 0x00);
imanomadao 0:5a3104f02775 236 wait_ms(200);
imanomadao 0:5a3104f02775 237
imanomadao 0:5a3104f02775 238 // Configure device for bias calculation
imanomadao 0:5a3104f02775 239 writeByte(MPU9255_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
imanomadao 0:5a3104f02775 240 writeByte(MPU9255_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
imanomadao 0:5a3104f02775 241 writeByte(MPU9255_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source
imanomadao 0:5a3104f02775 242 writeByte(MPU9255_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
imanomadao 0:5a3104f02775 243 writeByte(MPU9255_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes
imanomadao 0:5a3104f02775 244 writeByte(MPU9255_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP
imanomadao 0:5a3104f02775 245 wait_ms(15);
imanomadao 0:5a3104f02775 246
imanomadao 0:5a3104f02775 247 // Configure MPU6050 gyro and accelerometer for bias calculation
imanomadao 0:5a3104f02775 248 writeByte(MPU9255_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz
imanomadao 0:5a3104f02775 249 writeByte(MPU9255_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
imanomadao 0:5a3104f02775 250 writeByte(MPU9255_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity
imanomadao 0:5a3104f02775 251 writeByte(MPU9255_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
imanomadao 0:5a3104f02775 252
imanomadao 0:5a3104f02775 253 uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
imanomadao 0:5a3104f02775 254 uint16_t accelsensitivity = 16384; // = 16384 LSB/g
imanomadao 0:5a3104f02775 255
imanomadao 0:5a3104f02775 256 // Configure FIFO to capture accelerometer and gyro data for bias calculation
imanomadao 0:5a3104f02775 257 writeByte(MPU9255_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
imanomadao 0:5a3104f02775 258 writeByte(MPU9255_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150)
imanomadao 0:5a3104f02775 259 wait_ms(40); // accumulate 40 samples in 40 milliseconds = 480 bytes
imanomadao 0:5a3104f02775 260
imanomadao 0:5a3104f02775 261 // At end of sample accumulation, turn off FIFO sensor read
imanomadao 0:5a3104f02775 262 writeByte(MPU9255_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO
imanomadao 0:5a3104f02775 263 readBytes(MPU9255_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
imanomadao 0:5a3104f02775 264 fifo_count = ((uint16_t)data[0] << 8) | data[1];
imanomadao 0:5a3104f02775 265 packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging
imanomadao 0:5a3104f02775 266
imanomadao 0:5a3104f02775 267 for (ii = 0; ii < packet_count; ii++)
imanomadao 0:5a3104f02775 268 {
imanomadao 0:5a3104f02775 269 int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
imanomadao 0:5a3104f02775 270 readBytes(MPU9255_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
imanomadao 0:5a3104f02775 271 accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO
imanomadao 0:5a3104f02775 272 accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ;
imanomadao 0:5a3104f02775 273 accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ;
imanomadao 0:5a3104f02775 274 gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ;
imanomadao 0:5a3104f02775 275 gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ;
imanomadao 0:5a3104f02775 276 gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
imanomadao 0:5a3104f02775 277
imanomadao 0:5a3104f02775 278 accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
imanomadao 0:5a3104f02775 279 accel_bias[1] += (int32_t) accel_temp[1];
imanomadao 0:5a3104f02775 280 accel_bias[2] += (int32_t) accel_temp[2];
imanomadao 0:5a3104f02775 281 gyro_bias[0] += (int32_t) gyro_temp[0];
imanomadao 0:5a3104f02775 282 gyro_bias[1] += (int32_t) gyro_temp[1];
imanomadao 0:5a3104f02775 283 gyro_bias[2] += (int32_t) gyro_temp[2];
imanomadao 0:5a3104f02775 284
imanomadao 0:5a3104f02775 285 }
imanomadao 0:5a3104f02775 286
imanomadao 0:5a3104f02775 287 accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases
imanomadao 0:5a3104f02775 288 accel_bias[1] /= (int32_t) packet_count;
imanomadao 0:5a3104f02775 289 accel_bias[2] /= (int32_t) packet_count;
imanomadao 0:5a3104f02775 290 gyro_bias[0] /= (int32_t) packet_count;
imanomadao 0:5a3104f02775 291 gyro_bias[1] /= (int32_t) packet_count;
imanomadao 0:5a3104f02775 292 gyro_bias[2] /= (int32_t) packet_count;
imanomadao 0:5a3104f02775 293
imanomadao 0:5a3104f02775 294 if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation
imanomadao 0:5a3104f02775 295 else {accel_bias[2] += (int32_t) accelsensitivity;}
imanomadao 0:5a3104f02775 296
imanomadao 0:5a3104f02775 297 // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
imanomadao 0:5a3104f02775 298 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
imanomadao 0:5a3104f02775 299 data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
imanomadao 0:5a3104f02775 300 data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
imanomadao 0:5a3104f02775 301 data[3] = (-gyro_bias[1]/4) & 0xFF;
imanomadao 0:5a3104f02775 302 data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
imanomadao 0:5a3104f02775 303 data[5] = (-gyro_bias[2]/4) & 0xFF;
imanomadao 0:5a3104f02775 304
imanomadao 0:5a3104f02775 305 // Push gyro biases to hardware registers
imanomadao 0:5a3104f02775 306 writeByte(MPU9255_ADDRESS, XG_OFFSET_H, data[0]);
imanomadao 0:5a3104f02775 307 writeByte(MPU9255_ADDRESS, XG_OFFSET_L, data[1]);
imanomadao 0:5a3104f02775 308 writeByte(MPU9255_ADDRESS, YG_OFFSET_H, data[2]);
imanomadao 0:5a3104f02775 309 writeByte(MPU9255_ADDRESS, YG_OFFSET_L, data[3]);
imanomadao 0:5a3104f02775 310 writeByte(MPU9255_ADDRESS, ZG_OFFSET_H, data[4]);
imanomadao 0:5a3104f02775 311 writeByte(MPU9255_ADDRESS, ZG_OFFSET_L, data[5]);
imanomadao 0:5a3104f02775 312
imanomadao 0:5a3104f02775 313 // Output scaled gyro biases for display in the main program
imanomadao 0:5a3104f02775 314 dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity;
imanomadao 0:5a3104f02775 315 dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
imanomadao 0:5a3104f02775 316 dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
imanomadao 0:5a3104f02775 317
imanomadao 0:5a3104f02775 318 // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
imanomadao 0:5a3104f02775 319 // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
imanomadao 0:5a3104f02775 320 // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
imanomadao 0:5a3104f02775 321 // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
imanomadao 0:5a3104f02775 322 // the accelerometer biases calculated above must be divided by 8.
imanomadao 0:5a3104f02775 323
imanomadao 0:5a3104f02775 324 int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
imanomadao 0:5a3104f02775 325 readBytes(MPU9255_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
imanomadao 0:5a3104f02775 326 accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
imanomadao 0:5a3104f02775 327 readBytes(MPU9255_ADDRESS, YA_OFFSET_H, 2, &data[0]);
imanomadao 0:5a3104f02775 328 accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
imanomadao 0:5a3104f02775 329 readBytes(MPU9255_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
imanomadao 0:5a3104f02775 330 accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
imanomadao 0:5a3104f02775 331
imanomadao 0:5a3104f02775 332 uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
imanomadao 0:5a3104f02775 333 uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
imanomadao 0:5a3104f02775 334
imanomadao 0:5a3104f02775 335 for(ii = 0; ii < 3; ii++)
imanomadao 0:5a3104f02775 336 {
imanomadao 0:5a3104f02775 337 if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
imanomadao 0:5a3104f02775 338 }
imanomadao 0:5a3104f02775 339
imanomadao 0:5a3104f02775 340 // Construct total accelerometer bias, including calculated average accelerometer bias from above
imanomadao 0:5a3104f02775 341 accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
imanomadao 0:5a3104f02775 342 accel_bias_reg[1] -= (accel_bias[1]/8);
imanomadao 0:5a3104f02775 343 accel_bias_reg[2] -= (accel_bias[2]/8);
imanomadao 0:5a3104f02775 344
imanomadao 0:5a3104f02775 345 data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
imanomadao 0:5a3104f02775 346 data[1] = (accel_bias_reg[0]) & 0xFF;
imanomadao 0:5a3104f02775 347 data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
imanomadao 0:5a3104f02775 348 data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
imanomadao 0:5a3104f02775 349 data[3] = (accel_bias_reg[1]) & 0xFF;
imanomadao 0:5a3104f02775 350 data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
imanomadao 0:5a3104f02775 351 data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
imanomadao 0:5a3104f02775 352 data[5] = (accel_bias_reg[2]) & 0xFF;
imanomadao 0:5a3104f02775 353 data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
imanomadao 0:5a3104f02775 354
imanomadao 0:5a3104f02775 355 // Apparently this is not working for the acceleration biases in the MPU-9255
imanomadao 0:5a3104f02775 356 // Are we handling the temperature correction bit properly?
imanomadao 0:5a3104f02775 357 // Push accelerometer biases to hardware registers
imanomadao 0:5a3104f02775 358 // writeByte(MPU9255_ADDRESS, XA_OFFSET_H, data[0]);
imanomadao 0:5a3104f02775 359 // writeByte(MPU9255_ADDRESS, XA_OFFSET_L, data[1]);
imanomadao 0:5a3104f02775 360 // writeByte(MPU9255_ADDRESS, YA_OFFSET_H, data[2]);
imanomadao 0:5a3104f02775 361 // writeByte(MPU9255_ADDRESS, YA_OFFSET_L, data[3]);
imanomadao 0:5a3104f02775 362 // writeByte(MPU9255_ADDRESS, ZA_OFFSET_H, data[4]);
imanomadao 0:5a3104f02775 363 // writeByte(MPU9255_ADDRESS, ZA_OFFSET_L, data[5]);
imanomadao 0:5a3104f02775 364
imanomadao 0:5a3104f02775 365 // Output scaled accelerometer biases for display in the main program
imanomadao 0:5a3104f02775 366 dest2[0] = (float)accel_bias[0]/(float)accelsensitivity;
imanomadao 0:5a3104f02775 367 dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
imanomadao 0:5a3104f02775 368 dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
imanomadao 0:5a3104f02775 369 }
imanomadao 0:5a3104f02775 370
imanomadao 0:5a3104f02775 371 void MPU9255::init_mpu9255(uint8_t Ascale, uint8_t Gscale, uint8_t sampleRate)
imanomadao 0:5a3104f02775 372 {
imanomadao 0:5a3104f02775 373 // wake up device
imanomadao 0:5a3104f02775 374 writeByte(MPU9255_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors
imanomadao 0:5a3104f02775 375 wait_ms(100); // Wait for all registers to reset
imanomadao 0:5a3104f02775 376
imanomadao 0:5a3104f02775 377 // get stable time source
imanomadao 0:5a3104f02775 378 writeByte(MPU9255_ADDRESS, PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else
imanomadao 0:5a3104f02775 379 wait_ms(200);
imanomadao 0:5a3104f02775 380
imanomadao 0:5a3104f02775 381 // Configure Gyro and Thermometer
imanomadao 0:5a3104f02775 382 // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively;
imanomadao 0:5a3104f02775 383 // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot
imanomadao 0:5a3104f02775 384 // be higher than 1 / 0.0059 = 170 Hz
imanomadao 0:5a3104f02775 385 // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both
imanomadao 0:5a3104f02775 386 // With the MPU9255, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz
imanomadao 0:5a3104f02775 387 writeByte(MPU9255_ADDRESS, CONFIG, 0x03);
imanomadao 0:5a3104f02775 388
imanomadao 0:5a3104f02775 389 // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
imanomadao 0:5a3104f02775 390 writeByte(MPU9255_ADDRESS, SMPLRT_DIV, sampleRate); // Use a 200 Hz rate; a rate consistent with the filter update rate
imanomadao 0:5a3104f02775 391 // determined inset in CONFIG above
imanomadao 0:5a3104f02775 392
imanomadao 0:5a3104f02775 393 // Set gyroscope full scale range
imanomadao 0:5a3104f02775 394 // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
imanomadao 0:5a3104f02775 395 uint8_t c = readByte(MPU9255_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value
imanomadao 0:5a3104f02775 396 // c = c & ~0xE0; // Clear self-test bits [7:5]
imanomadao 0:5a3104f02775 397 c = c & ~0x02; // Clear Fchoice bits [1:0]
imanomadao 0:5a3104f02775 398 c = c & ~0x18; // Clear AFS bits [4:3]
imanomadao 0:5a3104f02775 399 c = c | Gscale << 3; // Set full scale range for the gyro
imanomadao 0:5a3104f02775 400 // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG
imanomadao 0:5a3104f02775 401 writeByte(MPU9255_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register
imanomadao 0:5a3104f02775 402
imanomadao 0:5a3104f02775 403 // Set accelerometer full-scale range configuration
imanomadao 0:5a3104f02775 404 c = readByte(MPU9255_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value
imanomadao 0:5a3104f02775 405 // c = c & ~0xE0; // Clear self-test bits [7:5]
imanomadao 0:5a3104f02775 406 c = c & ~0x18; // Clear AFS bits [4:3]
imanomadao 0:5a3104f02775 407 c = c | Ascale << 3; // Set full scale range for the accelerometer
imanomadao 0:5a3104f02775 408 writeByte(MPU9255_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value
imanomadao 0:5a3104f02775 409
imanomadao 0:5a3104f02775 410 // Set accelerometer sample rate configuration
imanomadao 0:5a3104f02775 411 // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
imanomadao 0:5a3104f02775 412 // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
imanomadao 0:5a3104f02775 413 c = readByte(MPU9255_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value
imanomadao 0:5a3104f02775 414 c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
imanomadao 0:5a3104f02775 415 c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
imanomadao 0:5a3104f02775 416 writeByte(MPU9255_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value
imanomadao 0:5a3104f02775 417
imanomadao 0:5a3104f02775 418 // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
imanomadao 0:5a3104f02775 419 // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting
imanomadao 0:5a3104f02775 420
imanomadao 0:5a3104f02775 421 // Configure Interrupts and Bypass Enable
imanomadao 0:5a3104f02775 422 // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared,
imanomadao 0:5a3104f02775 423 // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips
imanomadao 0:5a3104f02775 424 // can join the I2C bus and all can be controlled by the Arduino as master
imanomadao 0:5a3104f02775 425 writeByte(MPU9255_ADDRESS, INT_PIN_CFG, 0x10); // INT is 50 microsecond pulse and any read to clear
imanomadao 0:5a3104f02775 426 writeByte(MPU9255_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
imanomadao 0:5a3104f02775 427 wait_ms(100);
imanomadao 0:5a3104f02775 428
imanomadao 0:5a3104f02775 429 writeByte(MPU9255_ADDRESS, USER_CTRL, 0x20); // Enable I2C Master mode
imanomadao 0:5a3104f02775 430 writeByte(MPU9255_ADDRESS, I2C_MST_CTRL, 0x1D); // I2C configuration STOP after each transaction, master I2C bus at 400 KHz
imanomadao 0:5a3104f02775 431 writeByte(MPU9255_ADDRESS, I2C_MST_DELAY_CTRL, 0x81); // Use blocking data retreival and enable delay for mag sample rate mismatch
imanomadao 0:5a3104f02775 432 writeByte(MPU9255_ADDRESS, I2C_SLV4_CTRL, 0x01); // Delay mag data retrieval to once every other accel/gyro data sample
imanomadao 0:5a3104f02775 433 }
imanomadao 0:5a3104f02775 434
imanomadao 0:5a3104f02775 435
imanomadao 0:5a3104f02775 436 uint8_t MPU9255::get_AK8963CID()
imanomadao 0:5a3104f02775 437 {
imanomadao 0:5a3104f02775 438 writeByte(MPU9255_ADDRESS, USER_CTRL, 0x20); // Enable I2C Master mode
imanomadao 0:5a3104f02775 439 writeByte(MPU9255_ADDRESS, I2C_MST_CTRL, 0x0D); // I2C configuration multi-master I2C 400KHz
imanomadao 0:5a3104f02775 440
imanomadao 0:5a3104f02775 441 writeByte(MPU9255_ADDRESS, I2C_SLV0_ADDR, AK8963_ADDRESS | 0x80); // Set the I2C slave address of AK8963 and set for read.
imanomadao 0:5a3104f02775 442 writeByte(MPU9255_ADDRESS, I2C_SLV0_REG, WHO_AM_I_AK8963); // I2C slave 0 register address from where to begin data transfer
imanomadao 0:5a3104f02775 443 writeByte(MPU9255_ADDRESS, I2C_SLV0_CTRL, 0x81); // Enable I2C and transfer 1 byte
imanomadao 0:5a3104f02775 444 wait_ms(10);
imanomadao 0:5a3104f02775 445 uint8_t c = readByte(MPU9255_ADDRESS, EXT_SENS_DATA_00); // Read the WHO_AM_I byte
imanomadao 0:5a3104f02775 446 return c;
imanomadao 0:5a3104f02775 447 }
imanomadao 0:5a3104f02775 448
imanomadao 0:5a3104f02775 449 void MPU9255::init_AK8963Slave(uint8_t Mscale, uint8_t Mmode, float * magCalibration)
imanomadao 0:5a3104f02775 450 {
imanomadao 0:5a3104f02775 451 // First extract the factory calibration for each magnetometer axis
imanomadao 0:5a3104f02775 452 uint8_t rawData[3]; // x/y/z gyro calibration data stored here
imanomadao 0:5a3104f02775 453
imanomadao 0:5a3104f02775 454 writeByte(MPU9255_ADDRESS, I2C_SLV0_ADDR, AK8963_ADDRESS); // Set the I2C slave address of AK8963 and set for write.
imanomadao 0:5a3104f02775 455 writeByte(MPU9255_ADDRESS, I2C_SLV0_REG, AK8963_CNTL2); // I2C slave 0 register address from where to begin data transfer
imanomadao 0:5a3104f02775 456 writeByte(MPU9255_ADDRESS, I2C_SLV0_DO, 0x01); // Reset AK8963
imanomadao 0:5a3104f02775 457 writeByte(MPU9255_ADDRESS, I2C_SLV0_CTRL, 0x81); // Enable I2C and write 1 byte
imanomadao 0:5a3104f02775 458 wait_ms(50);
imanomadao 0:5a3104f02775 459 writeByte(MPU9255_ADDRESS, I2C_SLV0_ADDR, AK8963_ADDRESS); // Set the I2C slave address of AK8963 and set for write.
imanomadao 0:5a3104f02775 460 writeByte(MPU9255_ADDRESS, I2C_SLV0_REG, AK8963_CNTL); // I2C slave 0 register address from where to begin data transfer
imanomadao 0:5a3104f02775 461 writeByte(MPU9255_ADDRESS, I2C_SLV0_DO, 0x00); // Power down magnetometer
imanomadao 0:5a3104f02775 462 writeByte(MPU9255_ADDRESS, I2C_SLV0_CTRL, 0x81); // Enable I2C and write 1 byte
imanomadao 0:5a3104f02775 463 wait_ms(50);
imanomadao 0:5a3104f02775 464 writeByte(MPU9255_ADDRESS, I2C_SLV0_ADDR, AK8963_ADDRESS); // Set the I2C slave address of AK8963 and set for write.
imanomadao 0:5a3104f02775 465 writeByte(MPU9255_ADDRESS, I2C_SLV0_REG, AK8963_CNTL); // I2C slave 0 register address from where to begin data transfer
imanomadao 0:5a3104f02775 466 writeByte(MPU9255_ADDRESS, I2C_SLV0_DO, 0x0F); // Enter fuze mode
imanomadao 0:5a3104f02775 467 writeByte(MPU9255_ADDRESS, I2C_SLV0_CTRL, 0x81); // Enable I2C and write 1 byte
imanomadao 0:5a3104f02775 468 wait_ms(50);
imanomadao 0:5a3104f02775 469
imanomadao 0:5a3104f02775 470 writeByte(MPU9255_ADDRESS, I2C_SLV0_ADDR, AK8963_ADDRESS | 0x80); // Set the I2C slave address of AK8963 and set for read.
imanomadao 0:5a3104f02775 471 writeByte(MPU9255_ADDRESS, I2C_SLV0_REG, AK8963_ASAX); // I2C slave 0 register address from where to begin data transfer
imanomadao 0:5a3104f02775 472 writeByte(MPU9255_ADDRESS, I2C_SLV0_CTRL, 0x83); // Enable I2C and read 3 bytes
imanomadao 0:5a3104f02775 473 wait_ms(50);
imanomadao 0:5a3104f02775 474 readBytes(MPU9255_ADDRESS, EXT_SENS_DATA_00, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values
imanomadao 0:5a3104f02775 475 magCalibration[0] = (float)(rawData[0] - 128)/256.0f + 1.0f; // Return x-axis sensitivity adjustment values, etc.
imanomadao 0:5a3104f02775 476 magCalibration[1] = (float)(rawData[1] - 128)/256.0f + 1.0f;
imanomadao 0:5a3104f02775 477 magCalibration[2] = (float)(rawData[2] - 128)/256.0f + 1.0f;
imanomadao 0:5a3104f02775 478 /*_magCalibration[0] = magCalibration[0];
imanomadao 0:5a3104f02775 479 _magCalibration[1] = magCalibration[1];
imanomadao 0:5a3104f02775 480 _magCalibration[2] = magCalibration[2];
imanomadao 0:5a3104f02775 481 _Mmode = Mmode;*/
imanomadao 0:5a3104f02775 482
imanomadao 0:5a3104f02775 483 writeByte(MPU9255_ADDRESS, I2C_SLV0_ADDR, AK8963_ADDRESS); // Set the I2C slave address of AK8963 and set for write.
imanomadao 0:5a3104f02775 484 writeByte(MPU9255_ADDRESS, I2C_SLV0_REG, AK8963_CNTL); // I2C slave 0 register address from where to begin data transfer
imanomadao 0:5a3104f02775 485 writeByte(MPU9255_ADDRESS, I2C_SLV0_DO, 0x00); // Power down magnetometer
imanomadao 0:5a3104f02775 486 writeByte(MPU9255_ADDRESS, I2C_SLV0_CTRL, 0x81); // Enable I2C and transfer 1 byte
imanomadao 0:5a3104f02775 487 wait_ms(50);
imanomadao 0:5a3104f02775 488
imanomadao 0:5a3104f02775 489 writeByte(MPU9255_ADDRESS, I2C_SLV0_ADDR, AK8963_ADDRESS); // Set the I2C slave address of AK8963 and set for write.
imanomadao 0:5a3104f02775 490 writeByte(MPU9255_ADDRESS, I2C_SLV0_REG, AK8963_CNTL); // I2C slave 0 register address from where to begin data transfer
imanomadao 0:5a3104f02775 491 // Configure the magnetometer for continuous read and highest resolution
imanomadao 0:5a3104f02775 492 // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
imanomadao 0:5a3104f02775 493 // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
imanomadao 0:5a3104f02775 494 writeByte(MPU9255_ADDRESS, I2C_SLV0_DO, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR
imanomadao 0:5a3104f02775 495 writeByte(MPU9255_ADDRESS, I2C_SLV0_CTRL, 0x81); // Enable I2C and transfer 1 byte
imanomadao 0:5a3104f02775 496 wait_ms(50);
imanomadao 0:5a3104f02775 497 writeByte(MPU9255_ADDRESS, I2C_SLV0_ADDR, AK8963_ADDRESS | 0x80); // Set the I2C slave address of AK8963 and set for read.
imanomadao 0:5a3104f02775 498 writeByte(MPU9255_ADDRESS, I2C_SLV0_REG, AK8963_CNTL); // I2C slave 0 register address from where to begin data transfer
imanomadao 0:5a3104f02775 499 writeByte(MPU9255_ADDRESS, I2C_SLV0_CTRL, 0x81); // Enable I2C and transfer 1 byte
imanomadao 0:5a3104f02775 500 wait_ms(50);
imanomadao 0:5a3104f02775 501 }
imanomadao 0:5a3104f02775 502
imanomadao 0:5a3104f02775 503 void MPU9255::readMagData_mpu9255(int16_t * destination)
imanomadao 0:5a3104f02775 504 {
imanomadao 0:5a3104f02775 505 uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
imanomadao 0:5a3104f02775 506 // readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array
imanomadao 0:5a3104f02775 507 writeByte(MPU9255_ADDRESS, I2C_SLV0_ADDR, AK8963_ADDRESS | 0x80); // Set the I2C slave address of AK8963 and set for read.
imanomadao 0:5a3104f02775 508 writeByte(MPU9255_ADDRESS, I2C_SLV0_REG, AK8963_XOUT_L); // I2C slave 0 register address from where to begin data transfer
imanomadao 0:5a3104f02775 509 writeByte(MPU9255_ADDRESS, I2C_SLV0_CTRL, 0x87); // Enable I2C and read 7 bytes
imanomadao 0:5a3104f02775 510 wait_ms(2);
imanomadao 0:5a3104f02775 511 readBytes(MPU9255_ADDRESS, EXT_SENS_DATA_00, 7, &rawData[0]); // Read the x-, y-, and z-axis calibration values
imanomadao 0:5a3104f02775 512 uint8_t c = rawData[6]; // End data read by reading ST2 register
imanomadao 0:5a3104f02775 513 if(!(c & 0x08)) // Check if magnetic sensor overflow set, if not then report data
imanomadao 0:5a3104f02775 514 {
imanomadao 0:5a3104f02775 515 destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value
imanomadao 0:5a3104f02775 516 destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian
imanomadao 0:5a3104f02775 517 destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
imanomadao 0:5a3104f02775 518 }
imanomadao 0:5a3104f02775 519 }
imanomadao 0:5a3104f02775 520
imanomadao 0:5a3104f02775 521 void MPU9255::readaccgyrodata_mpu9255(int16_t * destination)
imanomadao 0:5a3104f02775 522 {
imanomadao 0:5a3104f02775 523 uint8_t rawData[14]; // x/y/z accel register data stored here
imanomadao 0:5a3104f02775 524 readBytes(MPU9255_ADDRESS, ACCEL_XOUT_H, 14, &rawData[0]); // Read the 14 raw data registers into data array
imanomadao 0:5a3104f02775 525 destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value
imanomadao 0:5a3104f02775 526 destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;
imanomadao 0:5a3104f02775 527 destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ;
imanomadao 0:5a3104f02775 528 destination[3] = ((int16_t)rawData[6] << 8) | rawData[7] ;
imanomadao 0:5a3104f02775 529 destination[4] = ((int16_t)rawData[8] << 8) | rawData[9] ;
imanomadao 0:5a3104f02775 530 destination[5] = ((int16_t)rawData[10] << 8) | rawData[11] ;
imanomadao 0:5a3104f02775 531 destination[6] = ((int16_t)rawData[12] << 8) | rawData[13] ;
imanomadao 0:5a3104f02775 532 }