P R
Library for the MPU9250 9DOF IMU chip.
Dependents: Xadow-M0_Xadow-OLED_Accelerometer MARe_VT_ble_PeakSearch_Kalman_Inativ
Embed:
(wiki syntax)
Show/hide line numbers
MPU9250.h
00001 /***** 00002 Library based on MPU-9250_Snowda library. It has been modified by Josué Olmeda Castelló for imasD Tecnología. It uses the 00003 mbed I2C class for comunications between the sensor and the master controller. 00004 Methods related with data filtering have not been tested. 00005 AD0 should be connected to GND. 00006 04/05/2015 00007 *****/ 00008 00009 #ifndef MPU9250_H 00010 #define MPU9250_H 00011 00012 #include "mbed.h" 00013 #include "math.h" 00014 00015 #define M_PI 3.14159265358979323846 00016 00017 // See also MPU-9250 Register Map and Descriptions, Revision 4.0, RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in 00018 // above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map 00019 // 00020 //Magnetometer Registers 00021 #define AK8963_ADDRESS 0x0C<<1 00022 #define WHO_AM_I_AK8963 0x00 // should return 0x48 00023 #define INFO 0x01 00024 #define AK8963_ST1 0x02 // data ready status bit 0 00025 #define AK8963_XOUT_L 0x03 // data 00026 #define AK8963_XOUT_H 0x04 00027 #define AK8963_YOUT_L 0x05 00028 #define AK8963_YOUT_H 0x06 00029 #define AK8963_ZOUT_L 0x07 00030 #define AK8963_ZOUT_H 0x08 00031 #define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 00032 #define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 00033 #define AK8963_ASTC 0x0C // Self test control 00034 #define AK8963_I2CDIS 0x0F // I2C disable 00035 #define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value 00036 #define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value 00037 #define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value 00038 00039 #define SELF_TEST_X_GYRO 0x00 00040 #define SELF_TEST_Y_GYRO 0x01 00041 #define SELF_TEST_Z_GYRO 0x02 00042 00043 /*#define X_FINE_GAIN 0x03 // [7:0] fine gain 00044 #define Y_FINE_GAIN 0x04 00045 #define Z_FINE_GAIN 0x05 00046 #define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer 00047 #define XA_OFFSET_L_TC 0x07 00048 #define YA_OFFSET_H 0x08 00049 #define YA_OFFSET_L_TC 0x09 00050 #define ZA_OFFSET_H 0x0A 00051 #define ZA_OFFSET_L_TC 0x0B */ 00052 00053 #define SELF_TEST_X_ACCEL 0x0D 00054 #define SELF_TEST_Y_ACCEL 0x0E 00055 #define SELF_TEST_Z_ACCEL 0x0F 00056 00057 #define SELF_TEST_A 0x10 00058 00059 #define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope 00060 #define XG_OFFSET_L 0x14 00061 #define YG_OFFSET_H 0x15 00062 #define YG_OFFSET_L 0x16 00063 #define ZG_OFFSET_H 0x17 00064 #define ZG_OFFSET_L 0x18 00065 #define SMPLRT_DIV 0x19 00066 #define CONFIG 0x1A 00067 #define GYRO_CONFIG 0x1B 00068 #define ACCEL_CONFIG 0x1C 00069 #define ACCEL_CONFIG2 0x1D 00070 #define LP_ACCEL_ODR 0x1E 00071 #define WOM_THR 0x1F 00072 00073 #define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms 00074 #define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] 00075 #define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms 00076 00077 #define FIFO_EN 0x23 00078 #define I2C_MST_CTRL 0x24 00079 #define I2C_SLV0_ADDR 0x25 00080 #define I2C_SLV0_REG 0x26 00081 #define I2C_SLV0_CTRL 0x27 00082 #define I2C_SLV1_ADDR 0x28 00083 #define I2C_SLV1_REG 0x29 00084 #define I2C_SLV1_CTRL 0x2A 00085 #define I2C_SLV2_ADDR 0x2B 00086 #define I2C_SLV2_REG 0x2C 00087 #define I2C_SLV2_CTRL 0x2D 00088 #define I2C_SLV3_ADDR 0x2E 00089 #define I2C_SLV3_REG 0x2F 00090 #define I2C_SLV3_CTRL 0x30 00091 #define I2C_SLV4_ADDR 0x31 00092 #define I2C_SLV4_REG 0x32 00093 #define I2C_SLV4_DO 0x33 00094 #define I2C_SLV4_CTRL 0x34 00095 #define I2C_SLV4_DI 0x35 00096 #define I2C_MST_STATUS 0x36 00097 #define INT_PIN_CFG 0x37 00098 #define INT_ENABLE 0x38 00099 #define DMP_INT_STATUS 0x39 // Check DMP interrupt 00100 #define INT_STATUS 0x3A 00101 #define ACCEL_XOUT_H 0x3B 00102 #define ACCEL_XOUT_L 0x3C 00103 #define ACCEL_YOUT_H 0x3D 00104 #define ACCEL_YOUT_L 0x3E 00105 #define ACCEL_ZOUT_H 0x3F 00106 #define ACCEL_ZOUT_L 0x40 00107 #define TEMP_OUT_H 0x41 00108 #define TEMP_OUT_L 0x42 00109 #define GYRO_XOUT_H 0x43 00110 #define GYRO_XOUT_L 0x44 00111 #define GYRO_YOUT_H 0x45 00112 #define GYRO_YOUT_L 0x46 00113 #define GYRO_ZOUT_H 0x47 00114 #define GYRO_ZOUT_L 0x48 00115 #define EXT_SENS_DATA_00 0x49 00116 #define EXT_SENS_DATA_01 0x4A 00117 #define EXT_SENS_DATA_02 0x4B 00118 #define EXT_SENS_DATA_03 0x4C 00119 #define EXT_SENS_DATA_04 0x4D 00120 #define EXT_SENS_DATA_05 0x4E 00121 #define EXT_SENS_DATA_06 0x4F 00122 #define EXT_SENS_DATA_07 0x50 00123 #define EXT_SENS_DATA_08 0x51 00124 #define EXT_SENS_DATA_09 0x52 00125 #define EXT_SENS_DATA_10 0x53 00126 #define EXT_SENS_DATA_11 0x54 00127 #define EXT_SENS_DATA_12 0x55 00128 #define EXT_SENS_DATA_13 0x56 00129 #define EXT_SENS_DATA_14 0x57 00130 #define EXT_SENS_DATA_15 0x58 00131 #define EXT_SENS_DATA_16 0x59 00132 #define EXT_SENS_DATA_17 0x5A 00133 #define EXT_SENS_DATA_18 0x5B 00134 #define EXT_SENS_DATA_19 0x5C 00135 #define EXT_SENS_DATA_20 0x5D 00136 #define EXT_SENS_DATA_21 0x5E 00137 #define EXT_SENS_DATA_22 0x5F 00138 #define EXT_SENS_DATA_23 0x60 00139 #define MOT_DETECT_STATUS 0x61 00140 #define I2C_SLV0_DO 0x63 00141 #define I2C_SLV1_DO 0x64 00142 #define I2C_SLV2_DO 0x65 00143 #define I2C_SLV3_DO 0x66 00144 #define I2C_MST_DELAY_CTRL 0x67 00145 #define SIGNAL_PATH_RESET 0x68 00146 #define MOT_DETECT_CTRL 0x69 00147 #define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP 00148 #define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode 00149 #define PWR_MGMT_2 0x6C 00150 #define DMP_BANK 0x6D // Activates a specific bank in the DMP 00151 #define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank 00152 #define DMP_REG 0x6F // Register in DMP from which to read or to which to write 00153 #define DMP_REG_1 0x70 00154 #define DMP_REG_2 0x71 00155 #define FIFO_COUNTH 0x72 00156 #define FIFO_COUNTL 0x73 00157 #define FIFO_R_W 0x74 00158 #define WHO_AM_I_MPU9250 0x75 // Should return 0x71 00159 #define XA_OFFSET_H 0x77 00160 #define XA_OFFSET_L 0x78 00161 #define YA_OFFSET_H 0x7A 00162 #define YA_OFFSET_L 0x7B 00163 #define ZA_OFFSET_H 0x7D 00164 #define ZA_OFFSET_L 0x7E 00165 00166 // Using the MSENSR-9250 breakout board, ADO is set to 0 00167 // Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 00168 //mbed uses the eight-bit device address, so shift seven-bit addresses left by one! 00169 #define ADO 0 00170 #if ADO 00171 #define MPU9250_ADDRESS 0x69<<1 // Device address when ADO = 1 00172 #else 00173 #define MPU9250_ADDRESS 0x68<<1 // Device address when ADO = 0 00174 #endif 00175 00176 // Set initial input parameters 00177 enum Ascale { 00178 AFS_2G = 0, 00179 AFS_4G, 00180 AFS_8G, 00181 AFS_16G 00182 }; 00183 00184 enum Gscale { 00185 GFS_250DPS = 0, 00186 GFS_500DPS, 00187 GFS_1000DPS, 00188 GFS_2000DPS 00189 }; 00190 00191 enum Mscale { 00192 MFS_14BITS = 0, // 0.6 mG per LSB 00193 MFS_16BITS // 0.15 mG per LSB 00194 }; 00195 00196 uint8_t Ascale = AFS_2G; // AFS_2G, AFS_4G, AFS_8G, AFS_16G 00197 uint8_t Gscale = GFS_250DPS; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS 00198 uint8_t Mscale = MFS_16BITS; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution 00199 uint8_t Mmode = 0x06; // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR 00200 float aRes, gRes, mRes; // scale resolutions per LSB for the sensors 00201 int I2Cstate=1; // If I2Cstate!=0, I2C read or write operation has failed 00202 00203 //Set up I2C, (SDA,SCL) 00204 //I2C i2c(I2C_SDA, I2C_SCL); 00205 00206 DigitalOut myled(LED1); 00207 00208 // Pin definitions 00209 int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins 00210 00211 int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output 00212 int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output 00213 int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output 00214 int16_t minMagCount[3] = {INT16_MAX, INT16_MAX, INT16_MAX}; 00215 int16_t maxMagCount[3] = {INT16_MIN, INT16_MIN, INT16_MIN}; // Stores the 16-bit signed magnetometer minimum and maximum sensor output 00216 00217 float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0}; // Factory mag calibration and mag bias 00218 float magScale[3] = {1, 1, 1}; // Magnetometer scalig for x, y and z axis 00219 float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer 00220 float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 00221 int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius 00222 float temperature; 00223 float SelfTest[6]; 00224 float orientation[1]; 00225 float magn_x, magn_y; 00226 00227 int delt_t = 0; // used to control display output rate 00228 int count = 0; // used to control display output rate 00229 00230 // parameters for 6 DoF sensor fusion calculations 00231 float PI = 3.14159265358979323846f; 00232 float GyroMeasError = PI * (60.0f / 180.0f); // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3 00233 float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta 00234 float GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) 00235 float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value 00236 #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 00237 #define Ki 0.0f 00238 00239 float pitch, yaw, roll; 00240 float deltat = 0.0f; // integration interval for both filter schemes 00241 int lastUpdate = 0, firstUpdate = 0, Now = 0; // used to calculate integration interval // used to calculate integration interval 00242 float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion 00243 float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method 00244 00245 class MPU9250 { 00246 00247 private: 00248 00249 I2C i2c; 00250 00251 protected: 00252 00253 public: 00254 00255 MPU9250(I2C &i2c_ref) : i2c(i2c_ref) 00256 { 00257 00258 } 00259 00260 //=================================================================================================================== 00261 //====== Set of useful function to access acceleration, gyroscope, and temperature data 00262 //=================================================================================================================== 00263 00264 void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) 00265 { 00266 char data_write[2]; 00267 data_write[0] = subAddress; 00268 data_write[1] = data; 00269 I2Cstate = i2c.write(address, data_write, 2, 0); 00270 } 00271 00272 char readByte(uint8_t address, uint8_t subAddress) 00273 { 00274 char data[1]; // `data` will store the register data 00275 char data_write[1]; 00276 data_write[0] = subAddress; 00277 I2Cstate = i2c.write(address, data_write, 1, 1); // no stop 00278 I2Cstate = i2c.read(address, data, 1, 0); 00279 return data[0]; 00280 } 00281 00282 void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) // count=nº of bytes to read / dest=destiny where data is stored 00283 { 00284 char data[14]; 00285 char data_write[1]; 00286 data_write[0] = subAddress; 00287 I2Cstate = i2c.write(address, data_write, 1, 1); // no stop 00288 I2Cstate = i2c.read(address, data, count, 0); 00289 for(int ii = 0; ii < count; ii++) { 00290 dest[ii] = data[ii]; 00291 } 00292 } 00293 00294 void getMres() { 00295 switch (Mscale) 00296 { 00297 // Possible magnetometer scales (and their register bit settings) are: 00298 // 14 bit resolution (0) and 16 bit resolution (1) 00299 case MFS_14BITS: 00300 mRes = 10.0*4219.0/8190.0; // Proper scale to return milliGauss 00301 break; 00302 case MFS_16BITS: 00303 mRes = 10.0*4219.0/32760.0; // Proper scale to return milliGauss 00304 break; 00305 } 00306 } 00307 00308 void getGres() { 00309 switch (Gscale) 00310 { 00311 // Possible gyro scales (and their register bit settings) are: 00312 // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). 00313 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: 00314 case GFS_250DPS: 00315 gRes = 250.0/32768.0; 00316 break; 00317 case GFS_500DPS: 00318 gRes = 500.0/32768.0; 00319 break; 00320 case GFS_1000DPS: 00321 gRes = 1000.0/32768.0; 00322 break; 00323 case GFS_2000DPS: 00324 gRes = 2000.0/32768.0; 00325 break; 00326 } 00327 } 00328 00329 void getAres() { 00330 switch (Ascale) 00331 { 00332 // Possible accelerometer scales (and their register bit settings) are: 00333 // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). 00334 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: 00335 case AFS_2G: 00336 aRes = 2.0/32768.0; 00337 break; 00338 case AFS_4G: 00339 aRes = 4.0/32768.0; 00340 break; 00341 case AFS_8G: 00342 aRes = 8.0/32768.0; 00343 break; 00344 case AFS_16G: 00345 aRes = 16.0/32768.0; 00346 break; 00347 } 00348 } 00349 00350 void readAccelData(int16_t * destination){ 00351 00352 uint8_t rawData[6]; // x/y/z accel register data stored here 00353 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array 00354 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00355 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00356 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00357 } 00358 00359 void readGyroData(int16_t * destination){ 00360 uint8_t rawData[6]; // x/y/z gyro register data stored here 00361 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array 00362 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00363 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00364 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00365 } 00366 00367 void readMagData(int16_t * destination){ 00368 uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition 00369 if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set 00370 readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array 00371 uint8_t c = rawData[6]; // End data read by reading ST2 register 00372 if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data 00373 destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value 00374 destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian 00375 destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ; 00376 } 00377 } 00378 } 00379 00380 int16_t readTempData(){ 00381 uint8_t rawData[2]; // x/y/z gyro register data stored here 00382 readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array 00383 return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]) ; // Turn the MSB and LSB into a 16-bit value 00384 } 00385 00386 void resetMPU9250(){ 00387 // reset device 00388 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device 00389 wait(0.1); 00390 } 00391 00392 void initAK8963(float * destination){ 00393 // First extract the factory calibration for each magnetometer axis 00394 uint8_t rawData[3]; // x/y/z gyro calibration data stored here 00395 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer 00396 wait(0.01); 00397 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode 00398 wait(0.01); 00399 readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values 00400 destination[0] = (float)(rawData[0] - 128)/256.0f + 1.0f; // Return x-axis sensitivity adjustment values, etc. 00401 destination[1] = (float)(rawData[1] - 128)/256.0f + 1.0f; 00402 destination[2] = (float)(rawData[2] - 128)/256.0f + 1.0f; 00403 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer 00404 wait(0.01); 00405 // Configure the magnetometer for continuous read and highest resolution 00406 // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, 00407 // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates 00408 writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR 00409 wait(0.01); 00410 } 00411 00412 void initMPU9250(){ 00413 // Initialize MPU9250 device 00414 // wake up device 00415 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors 00416 wait(0.1); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt 00417 00418 // get stable time source 00419 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 00420 00421 // Configure Gyro and Accelerometer 00422 // Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively; 00423 // DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both 00424 // Maximum delay is 4.9 ms which is just over a 200 Hz maximum rate 00425 writeByte(MPU9250_ADDRESS, CONFIG, 0x03); 00426 00427 // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) 00428 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; the same rate set in CONFIG above 00429 00430 // Set gyroscope full scale range 00431 // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 00432 uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); 00433 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] 00434 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0x18); // Clear AFS bits [4:3] 00435 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c | Gscale << 3); // Set full scale range for the gyro 00436 00437 // Set accelerometer configuration 00438 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); 00439 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] 00440 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0x18); // Clear AFS bits [4:3] 00441 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c | Ascale << 3); // Set full scale range for the accelerometer 00442 00443 // Set accelerometer sample rate configuration 00444 // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for 00445 // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz 00446 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); 00447 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c & ~0x0F); // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) 00448 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c | 0x03); // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz 00449 00450 // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, 00451 // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting 00452 00453 // Configure Interrupts and Bypass Enable 00454 // Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips 00455 // can join the I2C bus and all can be controlled by the Arduino as master 00456 writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22); 00457 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt 00458 } 00459 00460 // Function which accumulates gyro and accelerometer data after device initialization. It calculates the average 00461 // of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. 00462 void calibrateMPU9250(float * dest1, float * dest2) 00463 { 00464 uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data 00465 uint16_t ii, packet_count, fifo_count; 00466 int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; 00467 00468 // reset device, reset all registers, clear gyro and accelerometer bias registers 00469 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device 00470 wait(0.1); 00471 00472 // get stable time source 00473 // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 00474 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); 00475 writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); 00476 wait(0.2); 00477 00478 // Configure device for bias calculation 00479 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts 00480 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO 00481 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source 00482 writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master 00483 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes 00484 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP 00485 wait(0.015); 00486 00487 // Configure MPU9250 gyro and accelerometer for bias calculation 00488 writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz 00489 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz 00490 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity 00491 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity 00492 00493 uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec 00494 uint16_t accelsensitivity = 16384; // = 16384 LSB/g 00495 00496 // Configure FIFO to capture accelerometer and gyro data. This data will be used for bias calculation 00497 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO 00498 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250) 00499 wait(0.04); // accumulate 40 samples in 80 milliseconds = 480 bytes 00500 00501 // At end of sample accumulation, turn off FIFO sensor read 00502 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO 00503 readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count 00504 fifo_count = ((uint16_t)data[0] << 8) | data[1]; 00505 packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging 00506 00507 for (ii = 0; ii < packet_count; ii++) { 00508 int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; 00509 readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging 00510 accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO 00511 accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; 00512 accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; 00513 gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; 00514 gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; 00515 gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; 00516 00517 accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases 00518 accel_bias[1] += (int32_t) accel_temp[1]; 00519 accel_bias[2] += (int32_t) accel_temp[2]; 00520 gyro_bias[0] += (int32_t) gyro_temp[0]; 00521 gyro_bias[1] += (int32_t) gyro_temp[1]; 00522 gyro_bias[2] += (int32_t) gyro_temp[2]; 00523 00524 } 00525 accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases 00526 accel_bias[1] /= (int32_t) packet_count; 00527 accel_bias[2] /= (int32_t) packet_count; 00528 gyro_bias[0] /= (int32_t) packet_count; 00529 gyro_bias[1] /= (int32_t) packet_count; 00530 gyro_bias[2] /= (int32_t) packet_count; 00531 00532 if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation 00533 else {accel_bias[2] += (int32_t) accelsensitivity;} 00534 00535 // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup 00536 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 00537 data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases 00538 data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; 00539 data[3] = (-gyro_bias[1]/4) & 0xFF; 00540 data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; 00541 data[5] = (-gyro_bias[2]/4) & 0xFF; 00542 00543 /// Push gyro biases to hardware registers 00544 /* writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]); 00545 writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]); 00546 writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]); 00547 writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]); 00548 writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]); 00549 writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]); 00550 */ 00551 dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction 00552 dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; 00553 dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; 00554 00555 // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain 00556 // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold 00557 // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature 00558 // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that 00559 // the accelerometer biases calculated above must be divided by 8. 00560 00561 int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases 00562 readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values 00563 accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 00564 readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); 00565 accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 00566 readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); 00567 accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 00568 00569 uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers 00570 uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis 00571 00572 for(ii = 0; ii < 3; ii++) { 00573 if(accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit 00574 } 00575 00576 // Construct total accelerometer bias, including calculated average accelerometer bias from above 00577 accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) 00578 accel_bias_reg[1] -= (accel_bias[1]/8); 00579 accel_bias_reg[2] -= (accel_bias[2]/8); 00580 00581 data[0] = (accel_bias_reg[0] >> 8) & 0xFF; 00582 data[1] = (accel_bias_reg[0]) & 0xFF; 00583 data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers 00584 data[2] = (accel_bias_reg[1] >> 8) & 0xFF; 00585 data[3] = (accel_bias_reg[1]) & 0xFF; 00586 data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers 00587 data[4] = (accel_bias_reg[2] >> 8) & 0xFF; 00588 data[5] = (accel_bias_reg[2]) & 0xFF; 00589 data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers 00590 00591 // Apparently this is not working for the acceleration biases in the MPU-9250 00592 // Are we handling the temperature correction bit properly? 00593 // Push accelerometer biases to hardware registers 00594 /* writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]); 00595 writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]); 00596 writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]); 00597 writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]); 00598 writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]); 00599 writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); 00600 */ 00601 // Output scaled accelerometer biases for manual subtraction in the main program 00602 dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; 00603 dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; 00604 dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; 00605 } 00606 00607 00608 // Accelerometer and gyroscope self test; check calibration wrt factory settings 00609 void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass 00610 { 00611 uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; 00612 uint8_t selfTest[6]; 00613 int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3]; 00614 float factoryTrim[6]; 00615 uint8_t FS = 0; 00616 00617 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz 00618 writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz 00619 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<<FS); // Set full scale range for the gyro to 250 dps 00620 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz 00621 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g 00622 00623 for( int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer 00624 00625 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array 00626 aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00627 aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00628 aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00629 00630 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array 00631 gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00632 gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00633 gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00634 } 00635 00636 for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average current readings 00637 aAvg[ii] /= 200; 00638 gAvg[ii] /= 200; 00639 } 00640 00641 // Configure the accelerometer for self-test 00642 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g 00643 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s 00644 wait_ms(25); // Delay a while to let the device stabilize 00645 00646 for( int ii = 0; ii < 200; ii++) { // get average self-test values of gyro and acclerometer 00647 00648 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array 00649 aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00650 aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00651 aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00652 00653 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array 00654 gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00655 gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00656 gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00657 } 00658 00659 for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average self-test readings 00660 aSTAvg[ii] /= 200; 00661 gSTAvg[ii] /= 200; 00662 } 00663 00664 // Configure the gyro and accelerometer for normal operation 00665 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); 00666 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); 00667 //delay(25); // Delay a while to let the device stabilize 00668 wait_ms(25); // Delay a while to let the device stabilize 00669 00670 // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg 00671 selfTest[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results 00672 selfTest[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results 00673 selfTest[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results 00674 selfTest[3] = readByte(MPU9250_ADDRESS, SELF_TEST_X_GYRO); // X-axis gyro self-test results 00675 selfTest[4] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_GYRO); // Y-axis gyro self-test results 00676 selfTest[5] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_GYRO); // Z-axis gyro self-test results 00677 00678 // Retrieve factory self-test value from self-test code reads 00679 factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation 00680 factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation 00681 factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation 00682 factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation 00683 factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation 00684 factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation 00685 00686 // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response 00687 // To get percent, must multiply by 100 00688 for (int i = 0; i < 3; i++) { 00689 destination[i] = 100.0*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i]; // Report percent differences 00690 destination[i+3] = 100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3]; // Report percent differences 00691 } 00692 } 00693 00694 00695 00696 void getCompassOrientation(float * orient){ // Obtains the orientation of the device in degrees. 0 degrees North. 180 degrees South. 00697 /* 00698 Remember that it is the earth's rotational axis that defines the geographic north and south poles that we use for map references. 00699 It turns out that there is a discrepancy of about 11.5 degrees between the geographic poles and the magnetic poles. The last is 00700 what the magnetometer will read. A value, called the declination angle, can be applied to the magnetic direction to correct for this. 00701 On Valencia (Spain) this value is about 0 degrees. 00702 */ 00703 00704 // First of all measure 3 axis magnetometer values (only X and Y axis is used): 00705 readMagData(magCount); // Read the x/y/z adc values 00706 // Calculate the magnetometer values in milliGauss 00707 // Include factory calibration per data sheet and user environmental corrections 00708 if (I2Cstate == 0){ // no error on I2C 00709 I2Cstate = 1; 00710 magn_x = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set 00711 magn_y = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]; 00712 } 00713 00714 // Now obtains the orientation value: 00715 if (magn_y>0) 00716 orient[0] = 90.0 - (float) ( atan(magn_x/magn_y)*180/M_PI ); 00717 else if (magn_y<0) 00718 orient[0] = 270.0 - (float) ( atan(magn_x/magn_y)*180/M_PI ); 00719 else if (magn_y == 0){ 00720 if (magn_x<0) 00721 orient[0] = 180.0; 00722 else 00723 orient[0] = 0.0; 00724 } 00725 } 00726 00727 00728 00729 00730 00731 00732 // Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays" 00733 // (see http://www.x-io.co.uk/category/open-source/ for examples and more details) 00734 // which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute 00735 // device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc. 00736 // The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms 00737 // but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz! 00738 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) 00739 { 00740 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability 00741 float norm; 00742 float hx, hy, _2bx, _2bz; 00743 float s1, s2, s3, s4; 00744 float qDot1, qDot2, qDot3, qDot4; 00745 00746 // Auxiliary variables to avoid repeated arithmetic 00747 float _2q1mx; 00748 float _2q1my; 00749 float _2q1mz; 00750 float _2q2mx; 00751 float _4bx; 00752 float _4bz; 00753 float _2q1 = 2.0f * q1; 00754 float _2q2 = 2.0f * q2; 00755 float _2q3 = 2.0f * q3; 00756 float _2q4 = 2.0f * q4; 00757 float _2q1q3 = 2.0f * q1 * q3; 00758 float _2q3q4 = 2.0f * q3 * q4; 00759 float q1q1 = q1 * q1; 00760 float q1q2 = q1 * q2; 00761 float q1q3 = q1 * q3; 00762 float q1q4 = q1 * q4; 00763 float q2q2 = q2 * q2; 00764 float q2q3 = q2 * q3; 00765 float q2q4 = q2 * q4; 00766 float q3q3 = q3 * q3; 00767 float q3q4 = q3 * q4; 00768 float q4q4 = q4 * q4; 00769 00770 // Normalise accelerometer measurement 00771 norm = sqrt(ax * ax + ay * ay + az * az); 00772 if (norm == 0.0f) return; // handle NaN 00773 norm = 1.0f/norm; 00774 ax *= norm; 00775 ay *= norm; 00776 az *= norm; 00777 00778 // Normalise magnetometer measurement 00779 norm = sqrt(mx * mx + my * my + mz * mz); 00780 if (norm == 0.0f) return; // handle NaN 00781 norm = 1.0f/norm; 00782 mx *= norm; 00783 my *= norm; 00784 mz *= norm; 00785 00786 // Reference direction of Earth's magnetic field 00787 _2q1mx = 2.0f * q1 * mx; 00788 _2q1my = 2.0f * q1 * my; 00789 _2q1mz = 2.0f * q1 * mz; 00790 _2q2mx = 2.0f * q2 * mx; 00791 hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; 00792 hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; 00793 _2bx = sqrt(hx * hx + hy * hy); 00794 _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; 00795 _4bx = 2.0f * _2bx; 00796 _4bz = 2.0f * _2bz; 00797 00798 // Gradient decent algorithm corrective step 00799 s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 00800 s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 00801 s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 00802 s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 00803 norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude 00804 norm = 1.0f/norm; 00805 s1 *= norm; 00806 s2 *= norm; 00807 s3 *= norm; 00808 s4 *= norm; 00809 00810 // Compute rate of change of quaternion 00811 qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1; 00812 qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2; 00813 qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3; 00814 qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4; 00815 00816 // Integrate to yield quaternion 00817 q1 += qDot1 * deltat; 00818 q2 += qDot2 * deltat; 00819 q3 += qDot3 * deltat; 00820 q4 += qDot4 * deltat; 00821 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion 00822 norm = 1.0f/norm; 00823 q[0] = q1 * norm; 00824 q[1] = q2 * norm; 00825 q[2] = q3 * norm; 00826 q[3] = q4 * norm; 00827 00828 } 00829 00830 00831 00832 // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and 00833 // measured ones. 00834 void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) 00835 { 00836 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability 00837 float norm; 00838 float hx, hy, bx, bz; 00839 float vx, vy, vz, wx, wy, wz; 00840 float ex, ey, ez; 00841 float pa, pb, pc; 00842 00843 // Auxiliary variables to avoid repeated arithmetic 00844 float q1q1 = q1 * q1; 00845 float q1q2 = q1 * q2; 00846 float q1q3 = q1 * q3; 00847 float q1q4 = q1 * q4; 00848 float q2q2 = q2 * q2; 00849 float q2q3 = q2 * q3; 00850 float q2q4 = q2 * q4; 00851 float q3q3 = q3 * q3; 00852 float q3q4 = q3 * q4; 00853 float q4q4 = q4 * q4; 00854 00855 // Normalise accelerometer measurement 00856 norm = sqrt(ax * ax + ay * ay + az * az); 00857 if (norm == 0.0f) return; // handle NaN 00858 norm = 1.0f / norm; // use reciprocal for division 00859 ax *= norm; 00860 ay *= norm; 00861 az *= norm; 00862 00863 // Normalise magnetometer measurement 00864 norm = sqrt(mx * mx + my * my + mz * mz); 00865 if (norm == 0.0f) return; // handle NaN 00866 norm = 1.0f / norm; // use reciprocal for division 00867 mx *= norm; 00868 my *= norm; 00869 mz *= norm; 00870 00871 // Reference direction of Earth's magnetic field 00872 hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3); 00873 hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2); 00874 bx = sqrt((hx * hx) + (hy * hy)); 00875 bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3); 00876 00877 // Estimated direction of gravity and magnetic field 00878 vx = 2.0f * (q2q4 - q1q3); 00879 vy = 2.0f * (q1q2 + q3q4); 00880 vz = q1q1 - q2q2 - q3q3 + q4q4; 00881 wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3); 00882 wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4); 00883 wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3); 00884 00885 // Error is cross product between estimated direction and measured direction of gravity 00886 ex = (ay * vz - az * vy) + (my * wz - mz * wy); 00887 ey = (az * vx - ax * vz) + (mz * wx - mx * wz); 00888 ez = (ax * vy - ay * vx) + (mx * wy - my * wx); 00889 if (Ki > 0.0f) 00890 { 00891 eInt[0] += ex; // accumulate integral error 00892 eInt[1] += ey; 00893 eInt[2] += ez; 00894 } 00895 else 00896 { 00897 eInt[0] = 0.0f; // prevent integral wind up 00898 eInt[1] = 0.0f; 00899 eInt[2] = 0.0f; 00900 } 00901 00902 // Apply feedback terms 00903 gx = gx + Kp * ex + Ki * eInt[0]; 00904 gy = gy + Kp * ey + Ki * eInt[1]; 00905 gz = gz + Kp * ez + Ki * eInt[2]; 00906 00907 // Integrate rate of change of quaternion 00908 pa = q2; 00909 pb = q3; 00910 pc = q4; 00911 q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat); 00912 q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat); 00913 q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat); 00914 q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat); 00915 00916 // Normalise quaternion 00917 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); 00918 norm = 1.0f / norm; 00919 q[0] = q1 * norm; 00920 q[1] = q2 * norm; 00921 q[2] = q3 * norm; 00922 q[3] = q4 * norm; 00923 00924 } 00925 }; 00926 #endif
Generated on Thu Jul 21 2022 09:22:51 by
1.7.2