Pakorn Vongseela
MP3 PLAYER
Dependencies: DebouncedInterrupt SDFileSystem SPI_TFT_ILI9341 ST_401_84MHZ TFT_fonts VS1053 mbed
Fork of
MP3333
by 
FRA221_B18
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(D14, D15); 00205 00206 DigitalOut myled(LED1); 00207 00208 // Pin definitions 00209 int intPin = 2; // 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 float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0}; // Factory mag calibration and mag bias 00215 float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer 00216 float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 00217 int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius 00218 float temperature; 00219 float SelfTest[6]; 00220 float orientation[1]; 00221 float magn_x, magn_y; 00222 00223 int delt_t = 0; // used to control display output rate 00224 int count = 0; // used to control display output rate 00225 00226 // parameters for 6 DoF sensor fusion calculations 00227 float PI = 3.14159265358979323846f; 00228 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 00229 float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta 00230 float GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) 00231 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 00232 #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 00233 #define Ki 0.0f 00234 00235 float pitch, yaw, roll; 00236 float deltat = 0.0f; // integration interval for both filter schemes 00237 int lastUpdate = 0, firstUpdate = 0, Now = 0; // used to calculate integration interval // used to calculate integration interval 00238 float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion 00239 float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method 00240 00241 class MPU9250 { 00242 00243 protected: 00244 00245 public: 00246 //=================================================================================================================== 00247 //====== Set of useful function to access acceleration, gyroscope, and temperature data 00248 //=================================================================================================================== 00249 00250 void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) 00251 { 00252 char data_write[2]; 00253 data_write[0] = subAddress; 00254 data_write[1] = data; 00255 I2Cstate = i2c.write(address, data_write, 2, 0); 00256 } 00257 00258 char readByte(uint8_t address, uint8_t subAddress) 00259 { 00260 char data[1]; // `data` will store the register data 00261 char data_write[1]; 00262 data_write[0] = subAddress; 00263 I2Cstate = i2c.write(address, data_write, 1, 1); // no stop 00264 I2Cstate = i2c.read(address, data, 1, 0); 00265 return data[0]; 00266 } 00267 00268 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 00269 { 00270 char data[14]; 00271 char data_write[1]; 00272 data_write[0] = subAddress; 00273 I2Cstate = i2c.write(address, data_write, 1, 1); // no stop 00274 I2Cstate = i2c.read(address, data, count, 0); 00275 for(int ii = 0; ii < count; ii++) { 00276 dest[ii] = data[ii]; 00277 } 00278 } 00279 00280 void getMres() { 00281 switch (Mscale) 00282 { 00283 // Possible magnetometer scales (and their register bit settings) are: 00284 // 14 bit resolution (0) and 16 bit resolution (1) 00285 case MFS_14BITS: 00286 mRes = 10.0*4219.0/8190.0; // Proper scale to return milliGauss 00287 break; 00288 case MFS_16BITS: 00289 mRes = 10.0*4219.0/32760.0; // Proper scale to return milliGauss 00290 break; 00291 } 00292 } 00293 00294 void getGres() { 00295 switch (Gscale) 00296 { 00297 // Possible gyro scales (and their register bit settings) are: 00298 // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). 00299 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: 00300 case GFS_250DPS: 00301 gRes = 250.0/32768.0; 00302 break; 00303 case GFS_500DPS: 00304 gRes = 500.0/32768.0; 00305 break; 00306 case GFS_1000DPS: 00307 gRes = 1000.0/32768.0; 00308 break; 00309 case GFS_2000DPS: 00310 gRes = 2000.0/32768.0; 00311 break; 00312 } 00313 } 00314 00315 void getAres() { 00316 switch (Ascale) 00317 { 00318 // Possible accelerometer scales (and their register bit settings) are: 00319 // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). 00320 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: 00321 case AFS_2G: 00322 aRes = 2.0/32768.0; 00323 break; 00324 case AFS_4G: 00325 aRes = 4.0/32768.0; 00326 break; 00327 case AFS_8G: 00328 aRes = 8.0/32768.0; 00329 break; 00330 case AFS_16G: 00331 aRes = 16.0/32768.0; 00332 break; 00333 } 00334 } 00335 00336 void readAccelData(int16_t * destination){ 00337 00338 uint8_t rawData[6]; // x/y/z accel register data stored here 00339 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array 00340 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00341 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00342 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00343 } 00344 00345 void readGyroData(int16_t * destination){ 00346 uint8_t rawData[6]; // x/y/z gyro register data stored here 00347 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array 00348 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00349 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00350 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00351 } 00352 00353 void readMagData(int16_t * destination){ 00354 uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition 00355 if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set 00356 readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array 00357 uint8_t c = rawData[6]; // End data read by reading ST2 register 00358 if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data 00359 destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value 00360 destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian 00361 destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ; 00362 } 00363 } 00364 } 00365 00366 int16_t readTempData(){ 00367 uint8_t rawData[2]; // x/y/z gyro register data stored here 00368 readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array 00369 return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]) ; // Turn the MSB and LSB into a 16-bit value 00370 } 00371 00372 void resetMPU9250(){ 00373 // reset device 00374 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device 00375 wait(0.1); 00376 } 00377 00378 void initAK8963(float * destination){ 00379 // First extract the factory calibration for each magnetometer axis 00380 uint8_t rawData[3]; // x/y/z gyro calibration data stored here 00381 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer 00382 wait(0.01); 00383 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode 00384 wait(0.01); 00385 readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values 00386 destination[0] = (float)(rawData[0] - 128)/256.0f + 1.0f; // Return x-axis sensitivity adjustment values, etc. 00387 destination[1] = (float)(rawData[1] - 128)/256.0f + 1.0f; 00388 destination[2] = (float)(rawData[2] - 128)/256.0f + 1.0f; 00389 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer 00390 wait(0.01); 00391 // Configure the magnetometer for continuous read and highest resolution 00392 // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, 00393 // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates 00394 writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR 00395 wait(0.01); 00396 } 00397 00398 void initMPU9250(){ 00399 // Initialize MPU9250 device 00400 // wake up device 00401 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors 00402 wait(0.1); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt 00403 00404 // get stable time source 00405 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 00406 00407 // Configure Gyro and Accelerometer 00408 // Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively; 00409 // DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both 00410 // Maximum delay is 4.9 ms which is just over a 200 Hz maximum rate 00411 writeByte(MPU9250_ADDRESS, CONFIG, 0x03); 00412 00413 // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) 00414 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; the same rate set in CONFIG above 00415 00416 // Set gyroscope full scale range 00417 // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 00418 uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); 00419 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] 00420 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0x18); // Clear AFS bits [4:3] 00421 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c | Gscale << 3); // Set full scale range for the gyro 00422 00423 // Set accelerometer configuration 00424 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); 00425 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] 00426 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0x18); // Clear AFS bits [4:3] 00427 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c | Ascale << 3); // Set full scale range for the accelerometer 00428 00429 // Set accelerometer sample rate configuration 00430 // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for 00431 // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz 00432 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); 00433 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c & ~0x0F); // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) 00434 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c | 0x03); // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz 00435 00436 // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, 00437 // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting 00438 00439 // Configure Interrupts and Bypass Enable 00440 // Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips 00441 // can join the I2C bus and all can be controlled by the Arduino as master 00442 writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22); 00443 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt 00444 } 00445 00446 // Function which accumulates gyro and accelerometer data after device initialization. It calculates the average 00447 // of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. 00448 void calibrateMPU9250(float * dest1, float * dest2) 00449 { 00450 uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data 00451 uint16_t ii, packet_count, fifo_count; 00452 int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; 00453 00454 // reset device, reset all registers, clear gyro and accelerometer bias registers 00455 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device 00456 wait(0.1); 00457 00458 // get stable time source 00459 // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 00460 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); 00461 writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); 00462 wait(0.2); 00463 00464 // Configure device for bias calculation 00465 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts 00466 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO 00467 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source 00468 writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master 00469 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes 00470 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP 00471 wait(0.015); 00472 00473 // Configure MPU9250 gyro and accelerometer for bias calculation 00474 writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz 00475 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz 00476 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity 00477 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity 00478 00479 uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec 00480 uint16_t accelsensitivity = 16384; // = 16384 LSB/g 00481 00482 // Configure FIFO to capture accelerometer and gyro data. This data will be used for bias calculation 00483 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO 00484 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250) 00485 wait(0.04); // accumulate 40 samples in 80 milliseconds = 480 bytes 00486 00487 // At end of sample accumulation, turn off FIFO sensor read 00488 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO 00489 readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count 00490 fifo_count = ((uint16_t)data[0] << 8) | data[1]; 00491 packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging 00492 00493 for (ii = 0; ii < packet_count; ii++) { 00494 int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; 00495 readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging 00496 accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO 00497 accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; 00498 accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; 00499 gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; 00500 gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; 00501 gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; 00502 00503 accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases 00504 accel_bias[1] += (int32_t) accel_temp[1]; 00505 accel_bias[2] += (int32_t) accel_temp[2]; 00506 gyro_bias[0] += (int32_t) gyro_temp[0]; 00507 gyro_bias[1] += (int32_t) gyro_temp[1]; 00508 gyro_bias[2] += (int32_t) gyro_temp[2]; 00509 00510 } 00511 accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases 00512 accel_bias[1] /= (int32_t) packet_count; 00513 accel_bias[2] /= (int32_t) packet_count; 00514 gyro_bias[0] /= (int32_t) packet_count; 00515 gyro_bias[1] /= (int32_t) packet_count; 00516 gyro_bias[2] /= (int32_t) packet_count; 00517 00518 if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation 00519 else {accel_bias[2] += (int32_t) accelsensitivity;} 00520 00521 // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup 00522 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 00523 data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases 00524 data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; 00525 data[3] = (-gyro_bias[1]/4) & 0xFF; 00526 data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; 00527 data[5] = (-gyro_bias[2]/4) & 0xFF; 00528 00529 /// Push gyro biases to hardware registers 00530 /* writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]); 00531 writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]); 00532 writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]); 00533 writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]); 00534 writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]); 00535 writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]); 00536 */ 00537 dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction 00538 dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; 00539 dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; 00540 00541 // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain 00542 // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold 00543 // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature 00544 // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that 00545 // the accelerometer biases calculated above must be divided by 8. 00546 00547 int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases 00548 readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values 00549 accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 00550 readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); 00551 accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 00552 readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); 00553 accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 00554 00555 uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers 00556 uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis 00557 00558 for(ii = 0; ii < 3; ii++) { 00559 if(accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit 00560 } 00561 00562 // Construct total accelerometer bias, including calculated average accelerometer bias from above 00563 accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) 00564 accel_bias_reg[1] -= (accel_bias[1]/8); 00565 accel_bias_reg[2] -= (accel_bias[2]/8); 00566 00567 data[0] = (accel_bias_reg[0] >> 8) & 0xFF; 00568 data[1] = (accel_bias_reg[0]) & 0xFF; 00569 data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers 00570 data[2] = (accel_bias_reg[1] >> 8) & 0xFF; 00571 data[3] = (accel_bias_reg[1]) & 0xFF; 00572 data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers 00573 data[4] = (accel_bias_reg[2] >> 8) & 0xFF; 00574 data[5] = (accel_bias_reg[2]) & 0xFF; 00575 data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers 00576 00577 // Apparently this is not working for the acceleration biases in the MPU-9250 00578 // Are we handling the temperature correction bit properly? 00579 // Push accelerometer biases to hardware registers 00580 /* writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]); 00581 writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]); 00582 writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]); 00583 writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]); 00584 writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]); 00585 writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); 00586 */ 00587 // Output scaled accelerometer biases for manual subtraction in the main program 00588 dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; 00589 dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; 00590 dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; 00591 } 00592 00593 00594 // Accelerometer and gyroscope self test; check calibration wrt factory settings 00595 void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass 00596 { 00597 uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; 00598 uint8_t selfTest[6]; 00599 int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3]; 00600 float factoryTrim[6]; 00601 uint8_t FS = 0; 00602 00603 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz 00604 writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz 00605 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<<FS); // Set full scale range for the gyro to 250 dps 00606 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz 00607 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g 00608 00609 for( int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer 00610 00611 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array 00612 aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00613 aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00614 aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00615 00616 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array 00617 gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00618 gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00619 gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00620 } 00621 00622 for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average current readings 00623 aAvg[ii] /= 200; 00624 gAvg[ii] /= 200; 00625 } 00626 00627 // Configure the accelerometer for self-test 00628 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g 00629 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s 00630 wait_ms(25); // Delay a while to let the device stabilize 00631 00632 for( int ii = 0; ii < 200; ii++) { // get average self-test values of gyro and acclerometer 00633 00634 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array 00635 aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00636 aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00637 aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00638 00639 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array 00640 gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00641 gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00642 gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00643 } 00644 00645 for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average self-test readings 00646 aSTAvg[ii] /= 200; 00647 gSTAvg[ii] /= 200; 00648 } 00649 00650 // Configure the gyro and accelerometer for normal operation 00651 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); 00652 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); 00653 //delay(25); // Delay a while to let the device stabilize 00654 wait_ms(25); // Delay a while to let the device stabilize 00655 00656 // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg 00657 selfTest[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results 00658 selfTest[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results 00659 selfTest[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results 00660 selfTest[3] = readByte(MPU9250_ADDRESS, SELF_TEST_X_GYRO); // X-axis gyro self-test results 00661 selfTest[4] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_GYRO); // Y-axis gyro self-test results 00662 selfTest[5] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_GYRO); // Z-axis gyro self-test results 00663 00664 // Retrieve factory self-test value from self-test code reads 00665 factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation 00666 factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation 00667 factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation 00668 factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation 00669 factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation 00670 factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation 00671 00672 // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response 00673 // To get percent, must multiply by 100 00674 for (int i = 0; i < 3; i++) { 00675 destination[i] = 100.0*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i]; // Report percent differences 00676 destination[i+3] = 100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3]; // Report percent differences 00677 } 00678 } 00679 00680 00681 00682 void getCompassOrientation(float * orient){ // Obtains the orientation of the device in degrees. 0 degrees North. 180 degrees South. 00683 /* 00684 Remember that it is the earth's rotational axis that defines the geographic north and south poles that we use for map references. 00685 It turns out that there is a discrepancy of about 11.5 degrees between the geographic poles and the magnetic poles. The last is 00686 what the magnetometer will read. A value, called the declination angle, can be applied to the magnetic direction to correct for this. 00687 On Valencia (Spain) this value is about 0 degrees. 00688 */ 00689 00690 // First of all measure 3 axis magnetometer values (only X and Y axis is used): 00691 readMagData(magCount); // Read the x/y/z adc values 00692 // Calculate the magnetometer values in milliGauss 00693 // Include factory calibration per data sheet and user environmental corrections 00694 if (I2Cstate == 0){ // no error on I2C 00695 I2Cstate = 1; 00696 magn_x = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set 00697 magn_y = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]; 00698 } 00699 00700 // Now obtains the orientation value: 00701 if (magn_y>0) 00702 orient[0] = 90.0 - (float) ( atan(magn_x/magn_y)*180/M_PI ); 00703 else if (magn_y<0) 00704 orient[0] = 270.0 - (float) ( atan(magn_x/magn_y)*180/M_PI ); 00705 else if (magn_y == 0){ 00706 if (magn_x<0) 00707 orient[0] = 180.0; 00708 else 00709 orient[0] = 0.0; 00710 } 00711 } 00712 00713 00714 00715 00716 00717 00718 // Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays" 00719 // (see http://www.x-io.co.uk/category/open-source/ for examples and more details) 00720 // which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute 00721 // device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc. 00722 // The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms 00723 // but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz! 00724 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) 00725 { 00726 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability 00727 float norm; 00728 float hx, hy, _2bx, _2bz; 00729 float s1, s2, s3, s4; 00730 float qDot1, qDot2, qDot3, qDot4; 00731 00732 // Auxiliary variables to avoid repeated arithmetic 00733 float _2q1mx; 00734 float _2q1my; 00735 float _2q1mz; 00736 float _2q2mx; 00737 float _4bx; 00738 float _4bz; 00739 float _2q1 = 2.0f * q1; 00740 float _2q2 = 2.0f * q2; 00741 float _2q3 = 2.0f * q3; 00742 float _2q4 = 2.0f * q4; 00743 float _2q1q3 = 2.0f * q1 * q3; 00744 float _2q3q4 = 2.0f * q3 * q4; 00745 float q1q1 = q1 * q1; 00746 float q1q2 = q1 * q2; 00747 float q1q3 = q1 * q3; 00748 float q1q4 = q1 * q4; 00749 float q2q2 = q2 * q2; 00750 float q2q3 = q2 * q3; 00751 float q2q4 = q2 * q4; 00752 float q3q3 = q3 * q3; 00753 float q3q4 = q3 * q4; 00754 float q4q4 = q4 * q4; 00755 00756 // Normalise accelerometer measurement 00757 norm = sqrt(ax * ax + ay * ay + az * az); 00758 if (norm == 0.0f) return; // handle NaN 00759 norm = 1.0f/norm; 00760 ax *= norm; 00761 ay *= norm; 00762 az *= norm; 00763 00764 // Normalise magnetometer measurement 00765 norm = sqrt(mx * mx + my * my + mz * mz); 00766 if (norm == 0.0f) return; // handle NaN 00767 norm = 1.0f/norm; 00768 mx *= norm; 00769 my *= norm; 00770 mz *= norm; 00771 00772 // Reference direction of Earth's magnetic field 00773 _2q1mx = 2.0f * q1 * mx; 00774 _2q1my = 2.0f * q1 * my; 00775 _2q1mz = 2.0f * q1 * mz; 00776 _2q2mx = 2.0f * q2 * mx; 00777 hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; 00778 hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; 00779 _2bx = sqrt(hx * hx + hy * hy); 00780 _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; 00781 _4bx = 2.0f * _2bx; 00782 _4bz = 2.0f * _2bz; 00783 00784 // Gradient decent algorithm corrective step 00785 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); 00786 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); 00787 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); 00788 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); 00789 norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude 00790 norm = 1.0f/norm; 00791 s1 *= norm; 00792 s2 *= norm; 00793 s3 *= norm; 00794 s4 *= norm; 00795 00796 // Compute rate of change of quaternion 00797 qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1; 00798 qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2; 00799 qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3; 00800 qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4; 00801 00802 // Integrate to yield quaternion 00803 q1 += qDot1 * deltat; 00804 q2 += qDot2 * deltat; 00805 q3 += qDot3 * deltat; 00806 q4 += qDot4 * deltat; 00807 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion 00808 norm = 1.0f/norm; 00809 q[0] = q1 * norm; 00810 q[1] = q2 * norm; 00811 q[2] = q3 * norm; 00812 q[3] = q4 * norm; 00813 00814 } 00815 00816 00817 00818 // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and 00819 // measured ones. 00820 void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) 00821 { 00822 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability 00823 float norm; 00824 float hx, hy, bx, bz; 00825 float vx, vy, vz, wx, wy, wz; 00826 float ex, ey, ez; 00827 float pa, pb, pc; 00828 00829 // Auxiliary variables to avoid repeated arithmetic 00830 float q1q1 = q1 * q1; 00831 float q1q2 = q1 * q2; 00832 float q1q3 = q1 * q3; 00833 float q1q4 = q1 * q4; 00834 float q2q2 = q2 * q2; 00835 float q2q3 = q2 * q3; 00836 float q2q4 = q2 * q4; 00837 float q3q3 = q3 * q3; 00838 float q3q4 = q3 * q4; 00839 float q4q4 = q4 * q4; 00840 00841 // Normalise accelerometer measurement 00842 norm = sqrt(ax * ax + ay * ay + az * az); 00843 if (norm == 0.0f) return; // handle NaN 00844 norm = 1.0f / norm; // use reciprocal for division 00845 ax *= norm; 00846 ay *= norm; 00847 az *= norm; 00848 00849 // Normalise magnetometer measurement 00850 norm = sqrt(mx * mx + my * my + mz * mz); 00851 if (norm == 0.0f) return; // handle NaN 00852 norm = 1.0f / norm; // use reciprocal for division 00853 mx *= norm; 00854 my *= norm; 00855 mz *= norm; 00856 00857 // Reference direction of Earth's magnetic field 00858 hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3); 00859 hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2); 00860 bx = sqrt((hx * hx) + (hy * hy)); 00861 bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3); 00862 00863 // Estimated direction of gravity and magnetic field 00864 vx = 2.0f * (q2q4 - q1q3); 00865 vy = 2.0f * (q1q2 + q3q4); 00866 vz = q1q1 - q2q2 - q3q3 + q4q4; 00867 wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3); 00868 wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4); 00869 wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3); 00870 00871 // Error is cross product between estimated direction and measured direction of gravity 00872 ex = (ay * vz - az * vy) + (my * wz - mz * wy); 00873 ey = (az * vx - ax * vz) + (mz * wx - mx * wz); 00874 ez = (ax * vy - ay * vx) + (mx * wy - my * wx); 00875 if (Ki > 0.0f) 00876 { 00877 eInt[0] += ex; // accumulate integral error 00878 eInt[1] += ey; 00879 eInt[2] += ez; 00880 } 00881 else 00882 { 00883 eInt[0] = 0.0f; // prevent integral wind up 00884 eInt[1] = 0.0f; 00885 eInt[2] = 0.0f; 00886 } 00887 00888 // Apply feedback terms 00889 gx = gx + Kp * ex + Ki * eInt[0]; 00890 gy = gy + Kp * ey + Ki * eInt[1]; 00891 gz = gz + Kp * ez + Ki * eInt[2]; 00892 00893 // Integrate rate of change of quaternion 00894 pa = q2; 00895 pb = q3; 00896 pc = q4; 00897 q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat); 00898 q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat); 00899 q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat); 00900 q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat); 00901 00902 // Normalise quaternion 00903 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); 00904 norm = 1.0f / norm; 00905 q[0] = q1 * norm; 00906 q[1] = q2 * norm; 00907 q[2] = q3 * norm; 00908 q[3] = q4 * norm; 00909 00910 } 00911 }; 00912 #endif
Generated on Mon Jul 18 2022 23:39:31 by
1.7.2