Nora Vazbyte / Mbed OS 99Problems-BLEAint1

app critics will say it's money, cash, toes

source/MPU9250.cpp@23:7126ef3503e2, 2018-12-16 (annotated)

Committer:
vazbyte
Date:
Sun Dec 16 14:35:50 2018 +0000
Revision:
23:7126ef3503e2
Parent:
2:1957a4985d6e 
now step_buffer resets on button press

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vazbyte 2:1957a4985d6e 1 #include "MPU9250.h"
vazbyte 2:1957a4985d6e 2
vazbyte 2:1957a4985d6e 3 #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
vazbyte 2:1957a4985d6e 4 #define Ki 0.0f
vazbyte 2:1957a4985d6e 5
vazbyte 2:1957a4985d6e 6 //******************************************************************************
vazbyte 2:1957a4985d6e 7 MPU9250::MPU9250(PinName sda, PinName scl)
vazbyte 2:1957a4985d6e 8 {
vazbyte 2:1957a4985d6e 9 i2c_ = new I2C(sda, scl);
vazbyte 2:1957a4985d6e 10 i2c_->frequency(400000);
vazbyte 2:1957a4985d6e 11 }
vazbyte 2:1957a4985d6e 12
vazbyte 2:1957a4985d6e 13 //******************************************************************************
vazbyte 2:1957a4985d6e 14 MPU9250::MPU9250(I2C *i2c):i2c_(i2c){}
vazbyte 2:1957a4985d6e 15
vazbyte 2:1957a4985d6e 16 //******************************************************************************
vazbyte 2:1957a4985d6e 17 MPU9250::~MPU9250()
vazbyte 2:1957a4985d6e 18 {
vazbyte 2:1957a4985d6e 19 delete i2c_;
vazbyte 2:1957a4985d6e 20 }
vazbyte 2:1957a4985d6e 21
vazbyte 2:1957a4985d6e 22 void MPU9250::writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
vazbyte 2:1957a4985d6e 23 {
vazbyte 2:1957a4985d6e 24 char data_write[2];
vazbyte 2:1957a4985d6e 25 data_write[0] = subAddress;
vazbyte 2:1957a4985d6e 26 data_write[1] = data;
vazbyte 2:1957a4985d6e 27 i2c_->write(address, data_write, 2, 0);
vazbyte 2:1957a4985d6e 28 }
vazbyte 2:1957a4985d6e 29
vazbyte 2:1957a4985d6e 30 char MPU9250::readByte(uint8_t address, uint8_t subAddress)
vazbyte 2:1957a4985d6e 31 {
vazbyte 2:1957a4985d6e 32 char data[1]; // `data` will store the register data
vazbyte 2:1957a4985d6e 33 char data_write[1];
vazbyte 2:1957a4985d6e 34 data_write[0] = subAddress;
vazbyte 2:1957a4985d6e 35 i2c_->write(address, data_write, 1, 1); // no stop
vazbyte 2:1957a4985d6e 36 i2c_->read(address, data, 1, 0);
vazbyte 2:1957a4985d6e 37 return data[0];
vazbyte 2:1957a4985d6e 38 }
vazbyte 2:1957a4985d6e 39
vazbyte 2:1957a4985d6e 40 void MPU9250::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
vazbyte 2:1957a4985d6e 41 {
vazbyte 2:1957a4985d6e 42 char data[14];
vazbyte 2:1957a4985d6e 43 char data_write[1];
vazbyte 2:1957a4985d6e 44 data_write[0] = subAddress;
vazbyte 2:1957a4985d6e 45 i2c_->write(address, data_write, 1, 1); // no stop
vazbyte 2:1957a4985d6e 46 i2c_->read(address, data, count, 0);
vazbyte 2:1957a4985d6e 47 for(int ii = 0; ii < count; ii++) {
vazbyte 2:1957a4985d6e 48 dest[ii] = data[ii];
vazbyte 2:1957a4985d6e 49 }
vazbyte 2:1957a4985d6e 50 }
vazbyte 2:1957a4985d6e 51
vazbyte 2:1957a4985d6e 52 void MPU9250::getMres() {
vazbyte 2:1957a4985d6e 53 switch (Mscale)
vazbyte 2:1957a4985d6e 54 {
vazbyte 2:1957a4985d6e 55 // Possible magnetometer scales (and their register bit settings) are:
vazbyte 2:1957a4985d6e 56 // 14 bit resolution (0) and 16 bit resolution (1)
vazbyte 2:1957a4985d6e 57 case MFS_14BITS:
vazbyte 2:1957a4985d6e 58 mRes = 10.0*4219.0/8190.0; // Proper scale to return milliGauss
vazbyte 2:1957a4985d6e 59 break;
vazbyte 2:1957a4985d6e 60 case MFS_16BITS:
vazbyte 2:1957a4985d6e 61 mRes = 10.0*4219.0/32760.0; // Proper scale to return milliGauss
vazbyte 2:1957a4985d6e 62 break;
vazbyte 2:1957a4985d6e 63 }
vazbyte 2:1957a4985d6e 64 }
vazbyte 2:1957a4985d6e 65
vazbyte 2:1957a4985d6e 66
vazbyte 2:1957a4985d6e 67 void MPU9250::getGres() {
vazbyte 2:1957a4985d6e 68 switch (Gscale)
vazbyte 2:1957a4985d6e 69 {
vazbyte 2:1957a4985d6e 70 // Possible gyro scales (and their register bit settings) are:
vazbyte 2:1957a4985d6e 71 // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
vazbyte 2:1957a4985d6e 72 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
vazbyte 2:1957a4985d6e 73 case GFS_250DPS:
vazbyte 2:1957a4985d6e 74 gRes = 250.0/32768.0;
vazbyte 2:1957a4985d6e 75 break;
vazbyte 2:1957a4985d6e 76 case GFS_500DPS:
vazbyte 2:1957a4985d6e 77 gRes = 500.0/32768.0;
vazbyte 2:1957a4985d6e 78 break;
vazbyte 2:1957a4985d6e 79 case GFS_1000DPS:
vazbyte 2:1957a4985d6e 80 gRes = 1000.0/32768.0;
vazbyte 2:1957a4985d6e 81 break;
vazbyte 2:1957a4985d6e 82 case GFS_2000DPS:
vazbyte 2:1957a4985d6e 83 gRes = 2000.0/32768.0;
vazbyte 2:1957a4985d6e 84 break;
vazbyte 2:1957a4985d6e 85 }
vazbyte 2:1957a4985d6e 86 }
vazbyte 2:1957a4985d6e 87
vazbyte 2:1957a4985d6e 88
vazbyte 2:1957a4985d6e 89 void MPU9250::getAres() {
vazbyte 2:1957a4985d6e 90 switch (Ascale)
vazbyte 2:1957a4985d6e 91 {
vazbyte 2:1957a4985d6e 92 // Possible accelerometer scales (and their register bit settings) are:
vazbyte 2:1957a4985d6e 93 // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
vazbyte 2:1957a4985d6e 94 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
vazbyte 2:1957a4985d6e 95 case AFS_2G:
vazbyte 2:1957a4985d6e 96 aRes = 2.0/32768.0;
vazbyte 2:1957a4985d6e 97 break;
vazbyte 2:1957a4985d6e 98 case AFS_4G:
vazbyte 2:1957a4985d6e 99 aRes = 4.0/32768.0;
vazbyte 2:1957a4985d6e 100 break;
vazbyte 2:1957a4985d6e 101 case AFS_8G:
vazbyte 2:1957a4985d6e 102 aRes = 8.0/32768.0;
vazbyte 2:1957a4985d6e 103 break;
vazbyte 2:1957a4985d6e 104 case AFS_16G:
vazbyte 2:1957a4985d6e 105 aRes = 16.0/32768.0;
vazbyte 2:1957a4985d6e 106 break;
vazbyte 2:1957a4985d6e 107 }
vazbyte 2:1957a4985d6e 108 }
vazbyte 2:1957a4985d6e 109
vazbyte 2:1957a4985d6e 110
vazbyte 2:1957a4985d6e 111 void MPU9250::readAccelData(int16_t * destination)
vazbyte 2:1957a4985d6e 112 {
vazbyte 2:1957a4985d6e 113 uint8_t rawData[6]; // x/y/z accel register data stored here
vazbyte 2:1957a4985d6e 114 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
vazbyte 2:1957a4985d6e 115 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
vazbyte 2:1957a4985d6e 116 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
vazbyte 2:1957a4985d6e 117 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
vazbyte 2:1957a4985d6e 118 }
vazbyte 2:1957a4985d6e 119
vazbyte 2:1957a4985d6e 120 void MPU9250::readGyroData(int16_t * destination)
vazbyte 2:1957a4985d6e 121 {
vazbyte 2:1957a4985d6e 122 uint8_t rawData[6]; // x/y/z gyro register data stored here
vazbyte 2:1957a4985d6e 123 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
vazbyte 2:1957a4985d6e 124 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
vazbyte 2:1957a4985d6e 125 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
vazbyte 2:1957a4985d6e 126 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
vazbyte 2:1957a4985d6e 127 }
vazbyte 2:1957a4985d6e 128
vazbyte 2:1957a4985d6e 129 void MPU9250::readMagData(int16_t * destination)
vazbyte 2:1957a4985d6e 130 {
vazbyte 2:1957a4985d6e 131 uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
vazbyte 2:1957a4985d6e 132 if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
vazbyte 2:1957a4985d6e 133 readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array
vazbyte 2:1957a4985d6e 134 uint8_t c = rawData[6]; // End data read by reading ST2 register
vazbyte 2:1957a4985d6e 135 if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
vazbyte 2:1957a4985d6e 136 destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
vazbyte 2:1957a4985d6e 137 destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian
vazbyte 2:1957a4985d6e 138 destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ;
vazbyte 2:1957a4985d6e 139 }
vazbyte 2:1957a4985d6e 140 }
vazbyte 2:1957a4985d6e 141 }
vazbyte 2:1957a4985d6e 142
vazbyte 2:1957a4985d6e 143 int16_t MPU9250::readTempData()
vazbyte 2:1957a4985d6e 144 {
vazbyte 2:1957a4985d6e 145 uint8_t rawData[2]; // x/y/z gyro register data stored here
vazbyte 2:1957a4985d6e 146 readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
vazbyte 2:1957a4985d6e 147 return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]) ; // Turn the MSB and LSB into a 16-bit value
vazbyte 2:1957a4985d6e 148 }
vazbyte 2:1957a4985d6e 149
vazbyte 2:1957a4985d6e 150
vazbyte 2:1957a4985d6e 151 void MPU9250::resetMPU9250() {
vazbyte 2:1957a4985d6e 152 // reset device
vazbyte 2:1957a4985d6e 153 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
vazbyte 2:1957a4985d6e 154 wait(0.1);
vazbyte 2:1957a4985d6e 155 }
vazbyte 2:1957a4985d6e 156
vazbyte 2:1957a4985d6e 157 void MPU9250::initAK8963(float * destination)
vazbyte 2:1957a4985d6e 158 {
vazbyte 2:1957a4985d6e 159 // First extract the factory calibration for each magnetometer axis
vazbyte 2:1957a4985d6e 160 uint8_t rawData[3]; // x/y/z gyro calibration data stored here
vazbyte 2:1957a4985d6e 161 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
vazbyte 2:1957a4985d6e 162 wait(0.01);
vazbyte 2:1957a4985d6e 163 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
vazbyte 2:1957a4985d6e 164 wait(0.01);
vazbyte 2:1957a4985d6e 165 readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values
vazbyte 2:1957a4985d6e 166 destination[0] = (float)(rawData[0] - 128)/256.0f + 1.0f; // Return x-axis sensitivity adjustment values, etc.
vazbyte 2:1957a4985d6e 167 destination[1] = (float)(rawData[1] - 128)/256.0f + 1.0f;
vazbyte 2:1957a4985d6e 168 destination[2] = (float)(rawData[2] - 128)/256.0f + 1.0f;
vazbyte 2:1957a4985d6e 169 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
vazbyte 2:1957a4985d6e 170 wait(0.01);
vazbyte 2:1957a4985d6e 171 // Configure the magnetometer for continuous read and highest resolution
vazbyte 2:1957a4985d6e 172 // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
vazbyte 2:1957a4985d6e 173 // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
vazbyte 2:1957a4985d6e 174 writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR
vazbyte 2:1957a4985d6e 175 wait(0.01);
vazbyte 2:1957a4985d6e 176 }
vazbyte 2:1957a4985d6e 177
vazbyte 2:1957a4985d6e 178
vazbyte 2:1957a4985d6e 179 void MPU9250::initMPU9250()
vazbyte 2:1957a4985d6e 180 {
vazbyte 2:1957a4985d6e 181 // Initialize MPU9250 device
vazbyte 2:1957a4985d6e 182 // wake up device
vazbyte 2:1957a4985d6e 183 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors
vazbyte 2:1957a4985d6e 184 wait(0.1); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt
vazbyte 2:1957a4985d6e 185
vazbyte 2:1957a4985d6e 186 // get stable time source
vazbyte 2:1957a4985d6e 187 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
vazbyte 2:1957a4985d6e 188
vazbyte 2:1957a4985d6e 189 // Configure Gyro and Accelerometer
vazbyte 2:1957a4985d6e 190 // Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively;
vazbyte 2:1957a4985d6e 191 // DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both
vazbyte 2:1957a4985d6e 192 // Maximum delay is 4.9 ms which is just over a 200 Hz maximum rate
vazbyte 2:1957a4985d6e 193 writeByte(MPU9250_ADDRESS, CONFIG, 0x03);
vazbyte 2:1957a4985d6e 194
vazbyte 2:1957a4985d6e 195 // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
vazbyte 2:1957a4985d6e 196 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; the same rate set in CONFIG above
vazbyte 2:1957a4985d6e 197
vazbyte 2:1957a4985d6e 198 // Set gyroscope full scale range
vazbyte 2:1957a4985d6e 199 // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
vazbyte 2:1957a4985d6e 200 uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG);
vazbyte 2:1957a4985d6e 201 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0xE0); // Clear self-test bits [7:5]
vazbyte 2:1957a4985d6e 202 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
vazbyte 2:1957a4985d6e 203 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c | Gscale << 3); // Set full scale range for the gyro
vazbyte 2:1957a4985d6e 204
vazbyte 2:1957a4985d6e 205 // Set accelerometer configuration
vazbyte 2:1957a4985d6e 206 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG);
vazbyte 2:1957a4985d6e 207 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0xE0); // Clear self-test bits [7:5]
vazbyte 2:1957a4985d6e 208 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
vazbyte 2:1957a4985d6e 209 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c | Ascale << 3); // Set full scale range for the accelerometer
vazbyte 2:1957a4985d6e 210
vazbyte 2:1957a4985d6e 211 // Set accelerometer sample rate configuration
vazbyte 2:1957a4985d6e 212 // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
vazbyte 2:1957a4985d6e 213 // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
vazbyte 2:1957a4985d6e 214 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2);
vazbyte 2:1957a4985d6e 215 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c & ~0x0F); // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
vazbyte 2:1957a4985d6e 216 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c | 0x03); // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
vazbyte 2:1957a4985d6e 217
vazbyte 2:1957a4985d6e 218 // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
vazbyte 2:1957a4985d6e 219 // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting
vazbyte 2:1957a4985d6e 220
vazbyte 2:1957a4985d6e 221 // Configure Interrupts and Bypass Enable
vazbyte 2:1957a4985d6e 222 // Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips
vazbyte 2:1957a4985d6e 223 // can join the I2C bus and all can be controlled by the Arduino as master
vazbyte 2:1957a4985d6e 224 writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);
vazbyte 2:1957a4985d6e 225 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
vazbyte 2:1957a4985d6e 226 }
vazbyte 2:1957a4985d6e 227
vazbyte 2:1957a4985d6e 228 // Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
vazbyte 2:1957a4985d6e 229 // of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
vazbyte 2:1957a4985d6e 230 void MPU9250::calibrateMPU9250(float * dest1, float * dest2)
vazbyte 2:1957a4985d6e 231 {
vazbyte 2:1957a4985d6e 232 uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
vazbyte 2:1957a4985d6e 233 uint16_t ii, packet_count, fifo_count;
vazbyte 2:1957a4985d6e 234 int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
vazbyte 2:1957a4985d6e 235
vazbyte 2:1957a4985d6e 236 // reset device, reset all registers, clear gyro and accelerometer bias registers
vazbyte 2:1957a4985d6e 237 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
vazbyte 2:1957a4985d6e 238 wait(0.1);
vazbyte 2:1957a4985d6e 239
vazbyte 2:1957a4985d6e 240 // get stable time source
vazbyte 2:1957a4985d6e 241 // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
vazbyte 2:1957a4985d6e 242 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
vazbyte 2:1957a4985d6e 243 writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
vazbyte 2:1957a4985d6e 244 wait(0.2);
vazbyte 2:1957a4985d6e 245
vazbyte 2:1957a4985d6e 246 // Configure device for bias calculation
vazbyte 2:1957a4985d6e 247 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
vazbyte 2:1957a4985d6e 248 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
vazbyte 2:1957a4985d6e 249 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source
vazbyte 2:1957a4985d6e 250 writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
vazbyte 2:1957a4985d6e 251 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes
vazbyte 2:1957a4985d6e 252 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP
vazbyte 2:1957a4985d6e 253 wait(0.015);
vazbyte 2:1957a4985d6e 254
vazbyte 2:1957a4985d6e 255 // Configure MPU9250 gyro and accelerometer for bias calculation
vazbyte 2:1957a4985d6e 256 writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz
vazbyte 2:1957a4985d6e 257 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
vazbyte 2:1957a4985d6e 258 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity
vazbyte 2:1957a4985d6e 259 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
vazbyte 2:1957a4985d6e 260
vazbyte 2:1957a4985d6e 261 uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
vazbyte 2:1957a4985d6e 262 uint16_t accelsensitivity = 16384; // = 16384 LSB/g
vazbyte 2:1957a4985d6e 263
vazbyte 2:1957a4985d6e 264 // Configure FIFO to capture accelerometer and gyro data for bias calculation
vazbyte 2:1957a4985d6e 265 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
vazbyte 2:1957a4985d6e 266 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250)
vazbyte 2:1957a4985d6e 267 wait(0.04); // accumulate 40 samples in 80 milliseconds = 480 bytes
vazbyte 2:1957a4985d6e 268
vazbyte 2:1957a4985d6e 269 // At end of sample accumulation, turn off FIFO sensor read
vazbyte 2:1957a4985d6e 270 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO
vazbyte 2:1957a4985d6e 271 readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
vazbyte 2:1957a4985d6e 272 fifo_count = ((uint16_t)data[0] << 8) | data[1];
vazbyte 2:1957a4985d6e 273 packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging
vazbyte 2:1957a4985d6e 274
vazbyte 2:1957a4985d6e 275 for (ii = 0; ii < packet_count; ii++) {
vazbyte 2:1957a4985d6e 276 int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
vazbyte 2:1957a4985d6e 277 readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
vazbyte 2:1957a4985d6e 278 accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO
vazbyte 2:1957a4985d6e 279 accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ;
vazbyte 2:1957a4985d6e 280 accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ;
vazbyte 2:1957a4985d6e 281 gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ;
vazbyte 2:1957a4985d6e 282 gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ;
vazbyte 2:1957a4985d6e 283 gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
vazbyte 2:1957a4985d6e 284
vazbyte 2:1957a4985d6e 285 accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
vazbyte 2:1957a4985d6e 286 accel_bias[1] += (int32_t) accel_temp[1];
vazbyte 2:1957a4985d6e 287 accel_bias[2] += (int32_t) accel_temp[2];
vazbyte 2:1957a4985d6e 288 gyro_bias[0] += (int32_t) gyro_temp[0];
vazbyte 2:1957a4985d6e 289 gyro_bias[1] += (int32_t) gyro_temp[1];
vazbyte 2:1957a4985d6e 290 gyro_bias[2] += (int32_t) gyro_temp[2];
vazbyte 2:1957a4985d6e 291
vazbyte 2:1957a4985d6e 292 }
vazbyte 2:1957a4985d6e 293 accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases
vazbyte 2:1957a4985d6e 294 accel_bias[1] /= (int32_t) packet_count;
vazbyte 2:1957a4985d6e 295 accel_bias[2] /= (int32_t) packet_count;
vazbyte 2:1957a4985d6e 296 gyro_bias[0] /= (int32_t) packet_count;
vazbyte 2:1957a4985d6e 297 gyro_bias[1] /= (int32_t) packet_count;
vazbyte 2:1957a4985d6e 298 gyro_bias[2] /= (int32_t) packet_count;
vazbyte 2:1957a4985d6e 299
vazbyte 2:1957a4985d6e 300 if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation
vazbyte 2:1957a4985d6e 301 else {accel_bias[2] += (int32_t) accelsensitivity;}
vazbyte 2:1957a4985d6e 302
vazbyte 2:1957a4985d6e 303 // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
vazbyte 2:1957a4985d6e 304 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
vazbyte 2:1957a4985d6e 305 data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
vazbyte 2:1957a4985d6e 306 data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
vazbyte 2:1957a4985d6e 307 data[3] = (-gyro_bias[1]/4) & 0xFF;
vazbyte 2:1957a4985d6e 308 data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
vazbyte 2:1957a4985d6e 309 data[5] = (-gyro_bias[2]/4) & 0xFF;
vazbyte 2:1957a4985d6e 310
vazbyte 2:1957a4985d6e 311 /// Push gyro biases to hardware registers
vazbyte 2:1957a4985d6e 312 /* writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);
vazbyte 2:1957a4985d6e 313 writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);
vazbyte 2:1957a4985d6e 314 writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);
vazbyte 2:1957a4985d6e 315 writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);
vazbyte 2:1957a4985d6e 316 writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);
vazbyte 2:1957a4985d6e 317 writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);
vazbyte 2:1957a4985d6e 318 */
vazbyte 2:1957a4985d6e 319 dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction
vazbyte 2:1957a4985d6e 320 dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
vazbyte 2:1957a4985d6e 321 dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
vazbyte 2:1957a4985d6e 322
vazbyte 2:1957a4985d6e 323 // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
vazbyte 2:1957a4985d6e 324 // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
vazbyte 2:1957a4985d6e 325 // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
vazbyte 2:1957a4985d6e 326 // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
vazbyte 2:1957a4985d6e 327 // the accelerometer biases calculated above must be divided by 8.
vazbyte 2:1957a4985d6e 328
vazbyte 2:1957a4985d6e 329 int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
vazbyte 2:1957a4985d6e 330 readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
vazbyte 2:1957a4985d6e 331 accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1];
vazbyte 2:1957a4985d6e 332 readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
vazbyte 2:1957a4985d6e 333 accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1];
vazbyte 2:1957a4985d6e 334 readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
vazbyte 2:1957a4985d6e 335 accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1];
vazbyte 2:1957a4985d6e 336
vazbyte 2:1957a4985d6e 337 uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
vazbyte 2:1957a4985d6e 338 uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
vazbyte 2:1957a4985d6e 339
vazbyte 2:1957a4985d6e 340 for(ii = 0; ii < 3; ii++) {
vazbyte 2:1957a4985d6e 341 if(accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
vazbyte 2:1957a4985d6e 342 }
vazbyte 2:1957a4985d6e 343
vazbyte 2:1957a4985d6e 344 // Construct total accelerometer bias, including calculated average accelerometer bias from above
vazbyte 2:1957a4985d6e 345 accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
vazbyte 2:1957a4985d6e 346 accel_bias_reg[1] -= (accel_bias[1]/8);
vazbyte 2:1957a4985d6e 347 accel_bias_reg[2] -= (accel_bias[2]/8);
vazbyte 2:1957a4985d6e 348
vazbyte 2:1957a4985d6e 349 data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
vazbyte 2:1957a4985d6e 350 data[1] = (accel_bias_reg[0]) & 0xFF;
vazbyte 2:1957a4985d6e 351 data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
vazbyte 2:1957a4985d6e 352 data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
vazbyte 2:1957a4985d6e 353 data[3] = (accel_bias_reg[1]) & 0xFF;
vazbyte 2:1957a4985d6e 354 data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
vazbyte 2:1957a4985d6e 355 data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
vazbyte 2:1957a4985d6e 356 data[5] = (accel_bias_reg[2]) & 0xFF;
vazbyte 2:1957a4985d6e 357 data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
vazbyte 2:1957a4985d6e 358
vazbyte 2:1957a4985d6e 359 // Apparently this is not working for the acceleration biases in the MPU-9250
vazbyte 2:1957a4985d6e 360 // Are we handling the temperature correction bit properly?
vazbyte 2:1957a4985d6e 361 // Push accelerometer biases to hardware registers
vazbyte 2:1957a4985d6e 362 /* writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]);
vazbyte 2:1957a4985d6e 363 writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]);
vazbyte 2:1957a4985d6e 364 writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]);
vazbyte 2:1957a4985d6e 365 writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]);
vazbyte 2:1957a4985d6e 366 writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);
vazbyte 2:1957a4985d6e 367 writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]);
vazbyte 2:1957a4985d6e 368 */
vazbyte 2:1957a4985d6e 369 // Output scaled accelerometer biases for manual subtraction in the main program
vazbyte 2:1957a4985d6e 370 dest2[0] = (float)accel_bias[0]/(float)accelsensitivity;
vazbyte 2:1957a4985d6e 371 dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
vazbyte 2:1957a4985d6e 372 dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
vazbyte 2:1957a4985d6e 373 }
vazbyte 2:1957a4985d6e 374
vazbyte 2:1957a4985d6e 375
vazbyte 2:1957a4985d6e 376 // Accelerometer and gyroscope self test; check calibration wrt factory settings
vazbyte 2:1957a4985d6e 377 void MPU9250::MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
vazbyte 2:1957a4985d6e 378 {
vazbyte 2:1957a4985d6e 379 uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
vazbyte 2:1957a4985d6e 380 uint8_t selfTest[6];
vazbyte 2:1957a4985d6e 381 int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3];
vazbyte 2:1957a4985d6e 382 float factoryTrim[6];
vazbyte 2:1957a4985d6e 383 uint8_t FS = 0;
vazbyte 2:1957a4985d6e 384
vazbyte 2:1957a4985d6e 385 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz
vazbyte 2:1957a4985d6e 386 writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
vazbyte 2:1957a4985d6e 387 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<<FS); // Set full scale range for the gyro to 250 dps
vazbyte 2:1957a4985d6e 388 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
vazbyte 2:1957a4985d6e 389 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g
vazbyte 2:1957a4985d6e 390
vazbyte 2:1957a4985d6e 391 for( int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer
vazbyte 2:1957a4985d6e 392
vazbyte 2:1957a4985d6e 393 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
vazbyte 2:1957a4985d6e 394 aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
vazbyte 2:1957a4985d6e 395 aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
vazbyte 2:1957a4985d6e 396 aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
vazbyte 2:1957a4985d6e 397
vazbyte 2:1957a4985d6e 398 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
vazbyte 2:1957a4985d6e 399 gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
vazbyte 2:1957a4985d6e 400 gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
vazbyte 2:1957a4985d6e 401 gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
vazbyte 2:1957a4985d6e 402 }
vazbyte 2:1957a4985d6e 403
vazbyte 2:1957a4985d6e 404 for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average current readings
vazbyte 2:1957a4985d6e 405 aAvg[ii] /= 200;
vazbyte 2:1957a4985d6e 406 gAvg[ii] /= 200;
vazbyte 2:1957a4985d6e 407 }
vazbyte 2:1957a4985d6e 408
vazbyte 2:1957a4985d6e 409 // Configure the accelerometer for self-test
vazbyte 2:1957a4985d6e 410 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
vazbyte 2:1957a4985d6e 411 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
vazbyte 2:1957a4985d6e 412 wait_ms(25); // Delay a while to let the device stabilize
vazbyte 2:1957a4985d6e 413
vazbyte 2:1957a4985d6e 414 for( int ii = 0; ii < 200; ii++) { // get average self-test values of gyro and acclerometer
vazbyte 2:1957a4985d6e 415
vazbyte 2:1957a4985d6e 416 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
vazbyte 2:1957a4985d6e 417 aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
vazbyte 2:1957a4985d6e 418 aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
vazbyte 2:1957a4985d6e 419 aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
vazbyte 2:1957a4985d6e 420
vazbyte 2:1957a4985d6e 421 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
vazbyte 2:1957a4985d6e 422 gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
vazbyte 2:1957a4985d6e 423 gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
vazbyte 2:1957a4985d6e 424 gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
vazbyte 2:1957a4985d6e 425 }
vazbyte 2:1957a4985d6e 426
vazbyte 2:1957a4985d6e 427 for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average self-test readings
vazbyte 2:1957a4985d6e 428 aSTAvg[ii] /= 200;
vazbyte 2:1957a4985d6e 429 gSTAvg[ii] /= 200;
vazbyte 2:1957a4985d6e 430 }
vazbyte 2:1957a4985d6e 431
vazbyte 2:1957a4985d6e 432 // Configure the gyro and accelerometer for normal operation
vazbyte 2:1957a4985d6e 433 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);
vazbyte 2:1957a4985d6e 434 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);
vazbyte 2:1957a4985d6e 435 wait_ms(25); // Delay a while to let the device stabilize
vazbyte 2:1957a4985d6e 436
vazbyte 2:1957a4985d6e 437 // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
vazbyte 2:1957a4985d6e 438 selfTest[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results
vazbyte 2:1957a4985d6e 439 selfTest[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results
vazbyte 2:1957a4985d6e 440 selfTest[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results
vazbyte 2:1957a4985d6e 441 selfTest[3] = readByte(MPU9250_ADDRESS, SELF_TEST_X_GYRO); // X-axis gyro self-test results
vazbyte 2:1957a4985d6e 442 selfTest[4] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_GYRO); // Y-axis gyro self-test results
vazbyte 2:1957a4985d6e 443 selfTest[5] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_GYRO); // Z-axis gyro self-test results
vazbyte 2:1957a4985d6e 444
vazbyte 2:1957a4985d6e 445 // Retrieve factory self-test value from self-test code reads
vazbyte 2:1957a4985d6e 446 factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation
vazbyte 2:1957a4985d6e 447 factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation
vazbyte 2:1957a4985d6e 448 factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation
vazbyte 2:1957a4985d6e 449 factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation
vazbyte 2:1957a4985d6e 450 factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation
vazbyte 2:1957a4985d6e 451 factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation
vazbyte 2:1957a4985d6e 452
vazbyte 2:1957a4985d6e 453 // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
vazbyte 2:1957a4985d6e 454 // To get percent, must multiply by 100
vazbyte 2:1957a4985d6e 455 for (int i = 0; i < 3; i++) {
vazbyte 2:1957a4985d6e 456 destination[i] = 100.0*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i]; // Report percent differences
vazbyte 2:1957a4985d6e 457 destination[i+3] = 100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3]; // Report percent differences
vazbyte 2:1957a4985d6e 458 }
vazbyte 2:1957a4985d6e 459
vazbyte 2:1957a4985d6e 460 }