├── MPU6050HMC5883AHRS.ino ├── README.md └── quaternion Filters.ino /MPU6050HMC5883AHRS.ino: -------------------------------------------------------------------------------- 1 | /* MPU6050 Basic Example with HMC5883L magnetometer and 9 DoF Sensor Fusion 2 | by: Kris Winer 3 | date: May 10, 2014 4 | license: Beerware - Use this code however you'd like. If you 5 | find it useful you can buy me a beer some time. 6 | 7 | Demonstrate MPU-6050 basic functionality including initialization, accelerometer trimming, sleep mode functionality as well as 8 | parameterizing the register addresses. Added display functions to allow display to on-breadboard monitor. 9 | No DMP use. We just want to get out the accelerations, temperature, and gyro readings. 10 | Added the HMC5883L magnetometer for true 9 DoF sensor fusion using the open-source Madgwick and Mahony quaternion filtering 11 | algorithms. Out put as Yaw, Pitch, and Roll with absolute orientation to fixed true North (magnetic North plus declination) 12 | and Down (gravity). Easy 9 DoF sensor fusion with inexpensive and ubiquitous sensors. 13 | 14 | SDA and SCL should have external pull-up resistors (to 3.3V). 15 | 10k resistors worked for me. They should be on the breakout 16 | board. 17 | 18 | Hardware setup: 19 | MPU6050 Breakout --------- Arduino 20 | 3.3V --------------------- 3.3V 21 | SDA ----------------------- A4 22 | SCL ----------------------- A5 23 | GND ---------------------- GND 24 | 25 | Note: The MPU6050 is an I2C sensor and uses the Arduino Wire library. 26 | Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1. 27 | We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file. 28 | We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file. 29 | */ 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | // Using NOKIA 5110 monochrome 84 x 48 pixel display 36 | // pin 9 - Serial clock out (SCLK) 37 | // pin 8 - Serial data out (DIN) 38 | // pin 7 - Data/Command select (D/C) 39 | // pin 5 - LCD chip select (CS) 40 | // pin 6 - LCD reset (RST) 41 | Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); 42 | 43 | // Define registers per MPU6050, Register Map and Descriptions, Rev 4.2, 08/19/2013 6 DOF Motion sensor fusion device 44 | // Invensense Inc., www.invensense.com 45 | // See also MPU-6050 Register Map and Descriptions, Revision 4.0, RM-MPU-6050A-00, 9/12/2012 for registers not listed in 46 | // above document; the MPU6050 and MPU 9150 are virtually identical but the latter has an on-board magnetic sensor 47 | // 48 | #define XGOFFS_TC 0x00 // Bit 7 PWR_MODE, bits 6:1 XG_OFFS_TC, bit 0 OTP_BNK_VLD 49 | #define YGOFFS_TC 0x01 50 | #define ZGOFFS_TC 0x02 51 | #define X_FINE_GAIN 0x03 // [7:0] fine gain 52 | #define Y_FINE_GAIN 0x04 53 | #define Z_FINE_GAIN 0x05 54 | #define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer 55 | #define XA_OFFSET_L_TC 0x07 56 | #define YA_OFFSET_H 0x08 57 | #define YA_OFFSET_L_TC 0x09 58 | #define ZA_OFFSET_H 0x0A 59 | #define ZA_OFFSET_L_TC 0x0B 60 | #define SELF_TEST_X 0x0D 61 | #define SELF_TEST_Y 0x0E 62 | #define SELF_TEST_Z 0x0F 63 | #define SELF_TEST_A 0x10 64 | #define XG_OFFS_USRH 0x13 // User-defined trim values for gyroscope; supported in MPU-6050? 65 | #define XG_OFFS_USRL 0x14 66 | #define YG_OFFS_USRH 0x15 67 | #define YG_OFFS_USRL 0x16 68 | #define ZG_OFFS_USRH 0x17 69 | #define ZG_OFFS_USRL 0x18 70 | #define SMPLRT_DIV 0x19 71 | #define CONFIG 0x1A 72 | #define GYRO_CONFIG 0x1B 73 | #define ACCEL_CONFIG 0x1C 74 | #define FF_THR 0x1D // Free-fall 75 | #define FF_DUR 0x1E // Free-fall 76 | #define MOT_THR 0x1F // Motion detection threshold bits [7:0] 77 | #define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms 78 | #define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] 79 | #define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms 80 | #define FIFO_EN 0x23 81 | #define I2C_MST_CTRL 0x24 82 | #define I2C_SLV0_ADDR 0x25 83 | #define I2C_SLV0_REG 0x26 84 | #define I2C_SLV0_CTRL 0x27 85 | #define I2C_SLV1_ADDR 0x28 86 | #define I2C_SLV1_REG 0x29 87 | #define I2C_SLV1_CTRL 0x2A 88 | #define I2C_SLV2_ADDR 0x2B 89 | #define I2C_SLV2_REG 0x2C 90 | #define I2C_SLV2_CTRL 0x2D 91 | #define I2C_SLV3_ADDR 0x2E 92 | #define I2C_SLV3_REG 0x2F 93 | #define I2C_SLV3_CTRL 0x30 94 | #define I2C_SLV4_ADDR 0x31 95 | #define I2C_SLV4_REG 0x32 96 | #define I2C_SLV4_DO 0x33 97 | #define I2C_SLV4_CTRL 0x34 98 | #define I2C_SLV4_DI 0x35 99 | #define I2C_MST_STATUS 0x36 100 | #define INT_PIN_CFG 0x37 101 | #define INT_ENABLE 0x38 102 | #define DMP_INT_STATUS 0x39 // Check DMP interrupt 103 | #define INT_STATUS 0x3A 104 | #define ACCEL_XOUT_H 0x3B 105 | #define ACCEL_XOUT_L 0x3C 106 | #define ACCEL_YOUT_H 0x3D 107 | #define ACCEL_YOUT_L 0x3E 108 | #define ACCEL_ZOUT_H 0x3F 109 | #define ACCEL_ZOUT_L 0x40 110 | #define TEMP_OUT_H 0x41 111 | #define TEMP_OUT_L 0x42 112 | #define GYRO_XOUT_H 0x43 113 | #define GYRO_XOUT_L 0x44 114 | #define GYRO_YOUT_H 0x45 115 | #define GYRO_YOUT_L 0x46 116 | #define GYRO_ZOUT_H 0x47 117 | #define GYRO_ZOUT_L 0x48 118 | #define EXT_SENS_DATA_00 0x49 119 | #define EXT_SENS_DATA_01 0x4A 120 | #define EXT_SENS_DATA_02 0x4B 121 | #define EXT_SENS_DATA_03 0x4C 122 | #define EXT_SENS_DATA_04 0x4D 123 | #define EXT_SENS_DATA_05 0x4E 124 | #define EXT_SENS_DATA_06 0x4F 125 | #define EXT_SENS_DATA_07 0x50 126 | #define EXT_SENS_DATA_08 0x51 127 | #define EXT_SENS_DATA_09 0x52 128 | #define EXT_SENS_DATA_10 0x53 129 | #define EXT_SENS_DATA_11 0x54 130 | #define EXT_SENS_DATA_12 0x55 131 | #define EXT_SENS_DATA_13 0x56 132 | #define EXT_SENS_DATA_14 0x57 133 | #define EXT_SENS_DATA_15 0x58 134 | #define EXT_SENS_DATA_16 0x59 135 | #define EXT_SENS_DATA_17 0x5A 136 | #define EXT_SENS_DATA_18 0x5B 137 | #define EXT_SENS_DATA_19 0x5C 138 | #define EXT_SENS_DATA_20 0x5D 139 | #define EXT_SENS_DATA_21 0x5E 140 | #define EXT_SENS_DATA_22 0x5F 141 | #define EXT_SENS_DATA_23 0x60 142 | #define MOT_DETECT_STATUS 0x61 143 | #define I2C_SLV0_DO 0x63 144 | #define I2C_SLV1_DO 0x64 145 | #define I2C_SLV2_DO 0x65 146 | #define I2C_SLV3_DO 0x66 147 | #define I2C_MST_DELAY_CTRL 0x67 148 | #define SIGNAL_PATH_RESET 0x68 149 | #define MOT_DETECT_CTRL 0x69 150 | #define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP 151 | #define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode 152 | #define PWR_MGMT_2 0x6C 153 | #define DMP_BANK 0x6D // Activates a specific bank in the DMP 154 | #define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank 155 | #define DMP_REG 0x6F // Register in DMP from which to read or to which to write 156 | #define DMP_REG_1 0x70 157 | #define DMP_REG_2 0x71 158 | #define FIFO_COUNTH 0x72 159 | #define FIFO_COUNTL 0x73 160 | #define FIFO_R_W 0x74 161 | #define WHO_AM_I_MPU6050 0x75 // Should return 0x68 162 | 163 | #define HMC5883L_ADDRESS 0x1E 164 | #define HMC5883L_CONFIG_A 0x00 165 | #define HMC5883L_CONFIG_B 0x01 166 | #define HMC5883L_MODE 0x02 167 | #define HMC5883L_OUT_X_H 0x03 168 | #define HMC5883L_OUT_X_L 0x04 169 | #define HMC5883L_OUT_Z_H 0x05 170 | #define HMC5883L_OUT_Z_L 0x06 171 | #define HMC5883L_OUT_Y_H 0x07 172 | #define HMC5883L_OUT_Y_L 0x08 173 | #define HMC5883L_STATUS 0x09 174 | #define HMC5883L_IDA 0x0A // should return 0x48 175 | #define HMC5883L_IDB 0x0B // should return 0x34 176 | #define HMC5883L_IDC 0x0C // should return 0x33 177 | 178 | 179 | // Using the GY-521 breakout board, I set ADO to 0 by grounding through a 4k7 resistor 180 | // Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 181 | #define ADO 0 182 | #if ADO 183 | #define MPU6050_ADDRESS 0x69 // Device address when ADO = 1 184 | #else 185 | #define MPU6050_ADDRESS 0x68 // Device address when ADO = 0 186 | #endif 187 | 188 | // Set initial input parameters 189 | enum Ascale { 190 | AFS_2G = 0, 191 | AFS_4G, 192 | AFS_8G, 193 | AFS_16G 194 | }; 195 | 196 | enum Gscale { 197 | GFS_250DPS = 0, 198 | GFS_500DPS, 199 | GFS_1000DPS, 200 | GFS_2000DPS 201 | }; 202 | 203 | enum Mrate { // set magnetometer ODR 204 | MRT_0075 = 0, // 0.75 Hz ODR 205 | MRT_015, // 1.5 Hz 206 | MRT_030, // 3.0 Hz 207 | MRT_075, // 7.5 Hz 208 | MRT_15, // 15 Hz 209 | MRT_30, // 30 Hz 210 | MRT_75, // 75 Hz ODR 211 | }; 212 | 213 | // Specify sensor full scale 214 | int Gscale = GFS_250DPS; 215 | int Ascale = AFS_2G; 216 | uint8_t Mrate = MRT_15; // 15 Hz ODR 217 | float aRes, gRes, mRes; // scale resolutions per LSB for the sensors 218 | 219 | // Pin definitions 220 | int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins 221 | #define blinkPin 13 // Blink LED on Teensy or Pro Mini when updating 222 | boolean blinkOn = false; 223 | 224 | int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output 225 | int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output 226 | float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer 227 | int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius 228 | float temperature; 229 | float SelfTest[6]; 230 | int16_t magCount[3]; // 16-bit signed magnetometer sensor output 231 | float magbias[3]; // User-specified magnetometer corrections values 232 | 233 | // global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System) 234 | #define GyroMeasError PI * (40.0f / 180.0f) // gyroscope measurement error in rads/s (shown as 40 deg/s) 235 | #define GyroMeasDrift PI * (2.0f / 180.0f) // gyroscope measurement drift in rad/s/s (shown as 2.0 deg/s/s) 236 | #define beta sqrt(3.0f / 4.0f) * GyroMeasError // compute beta 237 | #define 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 238 | #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 239 | #define Ki 0.0f 240 | 241 | uint32_t delt_t = 0; // used to control display output rate 242 | uint32_t count = 0; // used to control display output rate 243 | boolean toggle = false; 244 | 245 | float pitch, yaw, roll; 246 | float deltat = 0.0f; // integration interval for both filter schemes 247 | 248 | uint32_t lastUpdate = 0; // used to calculate integration interval 249 | uint32_t Now = 0; // used to calculate integration interval 250 | 251 | float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 252 | float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion 253 | float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method 254 | 255 | void setup() 256 | { 257 | Wire.begin(); 258 | Serial.begin(38400); 259 | 260 | // Set up the interrupt pin, its set as active high, push-pull 261 | pinMode(intPin, INPUT); 262 | digitalWrite(intPin, LOW); 263 | pinMode(blinkPin, OUTPUT); 264 | digitalWrite(blinkPin, HIGH); 265 | 266 | display.begin(); // Initialize the display 267 | display.setContrast(58); // Set the contrast 268 | display.setRotation(2); // 0 or 2) width = width, 1 or 3) width = height, swapped etc. 269 | 270 | 271 | // Start device display with ID of sensor 272 | display.clearDisplay(); 273 | display.setTextSize(2); 274 | display.setCursor(0,0); display.print("MPU6050"); 275 | display.setTextSize(1); 276 | display.setCursor(0, 20); display.print("6-DOF 16-bit"); 277 | display.setCursor(0, 30); display.print("motion sensor"); 278 | display.setCursor(20,40); display.print("60 ug LSB"); 279 | display.display(); 280 | delay(1000); 281 | 282 | // Set up for data display 283 | display.setTextSize(1); // Set text size to normal, 2 is twice normal etc. 284 | display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen 285 | display.clearDisplay(); // clears the screen and buffer 286 | 287 | // Read the WHO_AM_I register, this is a good test of communication 288 | uint8_t c = readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 289 | display.setCursor(20,0); display.print("MPU6050"); 290 | display.setCursor(0,10); display.print("I AM"); 291 | display.setCursor(0,20); display.print(c, HEX); 292 | display.setCursor(0,30); display.print("I Should Be"); 293 | display.setCursor(0,40); display.print(0x68, HEX); 294 | display.display(); 295 | delay(1000); 296 | 297 | // Read the WHO_AM_I register of the HMC5883L, this is a good test of communication 298 | uint8_t e = readByte(HMC5883L_ADDRESS, HMC5883L_IDA); // Read WHO_AM_I register A for HMC5883L 299 | uint8_t f = readByte(HMC5883L_ADDRESS, HMC5883L_IDB); // Read WHO_AM_I register B for HMC5883L 300 | uint8_t g = readByte(HMC5883L_ADDRESS, HMC5883L_IDC); // Read WHO_AM_I register C for HMC5883L 301 | display.clearDisplay(); // clears the screen and buffer 302 | display.setCursor(20,0); display.print("HMC5883L"); 303 | display.setCursor(0,10); display.print("I AM"); 304 | display.setCursor( 0,20); display.print(e, HEX); 305 | display.setCursor(20,20); display.print(f, HEX); 306 | display.setCursor(40,20); display.print(g, HEX); 307 | display.setCursor(0,30); display.print("I Should Be"); 308 | display.setCursor(0,40); display.print(0x48, HEX); 309 | display.setCursor(20,40); display.print(0x34, HEX); 310 | display.setCursor(40,40); display.print(0x33, HEX); 311 | display.display(); 312 | delay(1000); 313 | 314 | if (c == 0x68 && e == 0x48 && f == 0x34 && g == 0x33) // WHO_AM_I should always be 0x68 315 | { 316 | Serial.println("MPU6050 is online..."); 317 | 318 | MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values 319 | // Serial.print("x-axis self test: acceleration trim within : "); Serial.print(SelfTest[0],1); Serial.println("% of factory value"); 320 | // Serial.print("y-axis self test: acceleration trim within : "); Serial.print(SelfTest[1],1); Serial.println("% of factory value"); 321 | // Serial.print("z-axis self test: acceleration trim within : "); Serial.print(SelfTest[2],1); Serial.println("% of factory value"); 322 | // Serial.print("x-axis self test: gyration trim within : "); Serial.print(SelfTest[3],1); Serial.println("% of factory value"); 323 | // Serial.print("y-axis self test: gyration trim within : "); Serial.print(SelfTest[4],1); Serial.println("% of factory value"); 324 | // Serial.print("z-axis self test: gyration trim within : "); Serial.print(SelfTest[5],1); Serial.println("% of factory value"); 325 | 326 | if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) { 327 | display.clearDisplay(); 328 | display.setCursor(0, 30); display.print("Pass Selftest!"); 329 | display.display(); 330 | delay(1000); 331 | 332 | calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers 333 | display.clearDisplay(); 334 | 335 | display.setCursor(0, 0); display.print("MPU6050 bias"); 336 | display.setCursor(0, 8); display.print(" x y z "); 337 | 338 | display.setCursor(0, 16); display.print((int)(1000*accelBias[0])); 339 | display.setCursor(24, 16); display.print((int)(1000*accelBias[1])); 340 | display.setCursor(48, 16); display.print((int)(1000*accelBias[2])); 341 | display.setCursor(72, 16); display.print("mg"); 342 | 343 | display.setCursor(0, 24); display.print(gyroBias[0], 1); 344 | display.setCursor(24, 24); display.print(gyroBias[1], 1); 345 | display.setCursor(48, 24); display.print(gyroBias[2], 1); 346 | display.setCursor(66, 24); display.print("o/s"); 347 | 348 | display.display(); 349 | delay(1000); 350 | 351 | initMPU6050(); Serial.println("MPU6050 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature 352 | 353 | if(selfTestHMC5883L()) { // perform magnetometer self test 354 | Serial.print(" HMC5883L passed self test!"); 355 | display.clearDisplay(); 356 | display.setCursor(0, 20); display.print("HMC5883L pass"); 357 | display.display(); 358 | delay(1000); 359 | } 360 | else { 361 | Serial.print(" HMC5883L failed self test!"); 362 | display.clearDisplay(); 363 | display.setCursor(0, 20); display.print("HMC5883L fail"); 364 | display.display(); 365 | delay(1000); 366 | } 367 | initHMC5883L(); // Initialize and configure magnetometer 368 | Serial.println("HMC5883L initialized for active data mode...."); 369 | } 370 | else 371 | { 372 | Serial.print("Could not connect to MPU6050: 0x"); 373 | Serial.println(c, HEX); 374 | while(1) ; // Loop forever if communication doesn't happen 375 | } 376 | } 377 | } 378 | 379 | void loop() 380 | { 381 | // If data ready bit set, all data registers have new data 382 | if(readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt 383 | readAccelData(accelCount); // Read the x/y/z adc values 384 | getAres(); 385 | 386 | // Now we'll calculate the accleration value into actual g's 387 | ax = (float)accelCount[0]*aRes; // get actual g value, this depends on scale being set 388 | ay = (float)accelCount[1]*aRes; 389 | az = (float)accelCount[2]*aRes; 390 | 391 | readGyroData(gyroCount); // Read the x/y/z adc values 392 | getGres(); 393 | 394 | // Calculate the gyro value into actual degrees per second 395 | gx = (float)gyroCount[0]*gRes; // get actual gyro value, this depends on scale being set 396 | gy = (float)gyroCount[1]*gRes; 397 | gz = (float)gyroCount[2]*gRes; 398 | 399 | tempCount = readTempData(); // Read the x/y/z adc values 400 | temperature = ((float) tempCount) / 340. + 36.53; // Temperature in degrees Centigrade 401 | } 402 | 403 | if(readByte(HMC5883L_ADDRESS, HMC5883L_STATUS) & 0x01) { // If data ready bit set, then read magnetometer data 404 | readMagData(magCount); // Read the x/y/z adc values 405 | mRes = 0.73; // Conversion to milliGauss, 0.73 mG/LSB in highest resolution mode 406 | // So far, magnetometer bias is calculated and subtracted here manually, should construct an algorithm to do it automatically 407 | // like the gyro and accelerometer biases 408 | magbias[0] = +56.; // User environmental x-axis correction in milliGauss 409 | magbias[1] = -118.; // User environmental y-axis correction in milliGauss 410 | magbias[2] = +35.; // User environmental z-axis correction in milliGauss 411 | 412 | // Calculate the magnetometer values in milliGauss 413 | // Include factory calibration per data sheet and user environmental corrections 414 | mx = (float)magCount[0]*mRes - magbias[0]; // get actual magnetometer value, this depends on scale being set 415 | my = (float)magCount[1]*mRes - magbias[1]; 416 | mz = (float)magCount[2]*mRes - magbias[2]; 417 | } 418 | 419 | Now = micros(); 420 | deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update 421 | lastUpdate = Now; 422 | 423 | // The HMC5883 y-axis is aligned with the MPU-6050 x-axis; the HMC5883 x-axis is aligned with the MPU-6050 -y-axis; 424 | // The HMC5883 z-axis is pointing up, 180 degrees from where it should. 425 | // We have to make some allowance for this orientation mismatch in feeding the output to the quaternion filter. 426 | // For the MPU-6060/HMC5883 system, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like 427 | // in the LSM9DS0 sensor. We negate the z-axis magnetic field to conform to AHRS convention of magnetic z-axis down. 428 | // This rotation can be modified to allow any convenient orientation convention. 429 | // This is ok by aircraft orientation standards! 430 | // Pass gyro rate as rad/s 431 | MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, -mx, -mz); 432 | // MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, -mx, -mz); 433 | 434 | // Serial print and/or display at 0.5 s rate independent of data rates 435 | delt_t = millis() - count; 436 | if (delt_t > 500) { // update LCD once per half-second independent of read rate 437 | digitalWrite(blinkPin, blinkOn); 438 | 439 | Serial.print("ax = "); Serial.print((int)1000*ax); 440 | Serial.print(" ay = "); Serial.print((int)1000*ay); 441 | Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg"); 442 | 443 | Serial.print("gx = "); Serial.print( gx, 1); 444 | Serial.print(" gy = "); Serial.print( gy, 1); 445 | Serial.print(" gz = "); Serial.print( gz, 1); Serial.println(" deg/s"); 446 | 447 | Serial.print("mx = "); Serial.print(mx); 448 | Serial.print(" my = "); Serial.print(my); 449 | Serial.print(" mz = "); Serial.print(mz); Serial.println(" mG"); 450 | 451 | Serial.print("q0 = "); Serial.print(q[0]); 452 | Serial.print(" qx = "); Serial.print(q[1]); 453 | Serial.print(" qy = "); Serial.print(q[2]); 454 | Serial.print(" qz = "); Serial.println(q[3]); 455 | 456 | // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. 457 | // In this coordinate system, the positive z-axis is down toward Earth. 458 | // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise. 459 | // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. 460 | // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. 461 | // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. 462 | // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be 463 | // applied in the correct order which for this configuration is yaw, pitch, and then roll. 464 | // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. 465 | yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); 466 | pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); 467 | roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); 468 | pitch *= 180.0f / PI; 469 | yaw *= 180.0f / PI; 470 | yaw -= 13.8; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 471 | roll *= 180.0f / PI; 472 | 473 | // Serial.print("Yaw, Pitch, Roll: "); 474 | Serial.print(yaw, 2); 475 | Serial.print(", "); 476 | Serial.print(pitch, 2); 477 | Serial.print(", "); 478 | Serial.println(roll, 2); 479 | 480 | // Serial.print("average rate = "); Serial.print(1.0f/deltat, 2); Serial.println(" Hz"); 481 | 482 | display.clearDisplay(); 483 | 484 | display.setCursor(0, 0); display.print(" x y z "); 485 | 486 | display.setCursor(0, 8); display.print((int)(1000*ax)); 487 | display.setCursor(24, 8); display.print((int)(1000*ay)); 488 | display.setCursor(48, 8); display.print((int)(1000*az)); 489 | display.setCursor(72, 8); display.print("mg"); 490 | 491 | display.setCursor(0, 16); display.print((int)(gx)); 492 | display.setCursor(24, 16); display.print((int)(gy)); 493 | display.setCursor(48, 16); display.print((int)(gz)); 494 | display.setCursor(66, 16); display.print("o/s"); 495 | 496 | display.setCursor(0, 24); display.print((int)(mx)); 497 | display.setCursor(24, 24); display.print((int)(my)); 498 | display.setCursor(48, 24); display.print((int)(mz)); 499 | display.setCursor(72, 24); display.print("mG"); 500 | 501 | display.setCursor(0, 32); display.print((int)(yaw)); 502 | display.setCursor(24, 32); display.print((int)(pitch)); 503 | display.setCursor(48, 32); display.print((int)(roll)); 504 | display.setCursor(66, 32); display.print("ypr"); 505 | 506 | display.setCursor(0, 40); display.print("rt: "); display.print(1.0f/deltat, 2); display.print(" Hz"); 507 | display.display(); 508 | 509 | blinkOn = ~blinkOn; 510 | count = millis(); 511 | } 512 | } 513 | 514 | //=================================================================================================================== 515 | //====== Set of useful function to access acceleratio, gyroscope, and temperature data 516 | //=================================================================================================================== 517 | 518 | void getGres() { 519 | switch (Gscale) 520 | { 521 | // Possible gyro scales (and their register bit settings) are: 522 | // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). 523 | // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: 524 | case GFS_250DPS: 525 | gRes = 250.0/32768.0; 526 | break; 527 | case GFS_500DPS: 528 | gRes = 500.0/32768.0; 529 | break; 530 | case GFS_1000DPS: 531 | gRes = 1000.0/32768.0; 532 | break; 533 | case GFS_2000DPS: 534 | gRes = 2000.0/32768.0; 535 | break; 536 | } 537 | } 538 | 539 | void getAres() { 540 | switch (Ascale) 541 | { 542 | // Possible accelerometer scales (and their register bit settings) are: 543 | // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). 544 | // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: 545 | case AFS_2G: 546 | aRes = 2.0/32768.0; 547 | break; 548 | case AFS_4G: 549 | aRes = 4.0/32768.0; 550 | break; 551 | case AFS_8G: 552 | aRes = 8.0/32768.0; 553 | break; 554 | case AFS_16G: 555 | aRes = 16.0/32768.0; 556 | break; 557 | } 558 | } 559 | 560 | 561 | void readAccelData(int16_t * destination) 562 | { 563 | uint8_t rawData[6]; // x/y/z accel register data stored here 564 | readBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array 565 | destination[0] = (int16_t)((rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 566 | destination[1] = (int16_t)((rawData[2] << 8) | rawData[3]) ; 567 | destination[2] = (int16_t)((rawData[4] << 8) | rawData[5]) ; 568 | } 569 | 570 | void readGyroData(int16_t * destination) 571 | { 572 | uint8_t rawData[6]; // x/y/z gyro register data stored here 573 | readBytes(MPU6050_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array 574 | destination[0] = (int16_t)((rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 575 | destination[1] = (int16_t)((rawData[2] << 8) | rawData[3]) ; 576 | destination[2] = (int16_t)((rawData[4] << 8) | rawData[5]) ; 577 | } 578 | 579 | void readMagData(int16_t * destination) 580 | { 581 | uint8_t rawData[6]; // x/y/z gyro register data stored here 582 | readBytes(HMC5883L_ADDRESS, HMC5883L_OUT_X_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array 583 | destination[0] = ((int16_t)rawData[0] << 8) | rawData[1]; // Turn the MSB and LSB into a signed 16-bit value 584 | destination[1] = ((int16_t)rawData[4] << 8) | rawData[5]; 585 | destination[2] = ((int16_t)rawData[2] << 8) | rawData[3]; 586 | } 587 | 588 | int16_t readTempData() 589 | { 590 | uint8_t rawData[2]; // x/y/z gyro register data stored here 591 | readBytes(MPU6050_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array 592 | return ((int16_t)rawData[0]) << 8 | rawData[1] ; // Turn the MSB and LSB into a 16-bit value 593 | } 594 | 595 | void initHMC5883L() 596 | { 597 | // Set magnetomer ODR; default is 15 Hz 598 | writeByte(HMC5883L_ADDRESS, HMC5883L_CONFIG_A, Mrate << 2); 599 | writeByte(HMC5883L_ADDRESS, HMC5883L_CONFIG_B, 0x00); // set gain (bits[7:5]) to maximum resolution of 0.73 mG/LSB 600 | writeByte(HMC5883L_ADDRESS, HMC5883L_MODE, 0x80 ); // enable continuous data mode 601 | } 602 | 603 | byte selfTestHMC5883L() 604 | { 605 | int16_t selfTest[3] = {0, 0, 0}; 606 | // Perform self-test and calculate temperature compensation bias 607 | writeByte(HMC5883L_ADDRESS, HMC5883L_CONFIG_A, 0x71); // set 8-average, 15 Hz default, positive self-test measurement 608 | writeByte(HMC5883L_ADDRESS, HMC5883L_CONFIG_B, 0xA0); // set gain (bits[7:5]) to 5 609 | writeByte(HMC5883L_ADDRESS, HMC5883L_MODE, 0x80 ); // enable continuous data mode 610 | delay(150); // wait 150 ms 611 | 612 | uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; // x/y/z gyro register data stored here 613 | readBytes(HMC5883L_ADDRESS, HMC5883L_OUT_X_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array 614 | selfTest[0] = ((int16_t)rawData[0] << 8) | rawData[1]; // Turn the MSB and LSB into a signed 16-bit value 615 | selfTest[1] = ((int16_t)rawData[4] << 8) | rawData[5]; 616 | selfTest[2] = ((int16_t)rawData[2] << 8) | rawData[3]; 617 | writeByte(HMC5883L_ADDRESS, HMC5883L_CONFIG_A, 0x00); // exit self test 618 | 619 | if(selfTest[0] < 575 && selfTest[0] > 243 && selfTest[1] < 575 && selfTest[1] > 243 && selfTest[2] < 575 && selfTest[2] > 243) 620 | { return true; } else { return false;} 621 | } 622 | 623 | 624 | // Configure the motion detection control for low power accelerometer mode 625 | void LowPowerAccelOnlyMPU6050() 626 | { 627 | 628 | // The sensor has a high-pass filter necessary to invoke to allow the sensor motion detection algorithms work properly 629 | // Motion detection occurs on free-fall (acceleration below a threshold for some time for all axes), motion (acceleration 630 | // above a threshold for some time on at least one axis), and zero-motion toggle (acceleration on each axis less than a 631 | // threshold for some time sets this flag, motion above the threshold turns it off). The high-pass filter takes gravity out 632 | // consideration for these threshold evaluations; otherwise, the flags would be set all the time! 633 | 634 | uint8_t c = readByte(MPU6050_ADDRESS, PWR_MGMT_1); 635 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c & ~0x30); // Clear sleep and cycle bits [5:6] 636 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c | 0x30); // Set sleep and cycle bits [5:6] to zero to make sure accelerometer is running 637 | 638 | c = readByte(MPU6050_ADDRESS, PWR_MGMT_2); 639 | writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c & ~0x38); // Clear standby XA, YA, and ZA bits [3:5] 640 | writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c | 0x00); // Set XA, YA, and ZA bits [3:5] to zero to make sure accelerometer is running 641 | 642 | c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG); 643 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x07); // Clear high-pass filter bits [2:0] 644 | // Set high-pass filter to 0) reset (disable), 1) 5 Hz, 2) 2.5 Hz, 3) 1.25 Hz, 4) 0.63 Hz, or 7) Hold 645 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c | 0x00); // Set ACCEL_HPF to 0; reset mode disbaling high-pass filter 646 | 647 | c = readByte(MPU6050_ADDRESS, CONFIG); 648 | writeByte(MPU6050_ADDRESS, CONFIG, c & ~0x07); // Clear low-pass filter bits [2:0] 649 | writeByte(MPU6050_ADDRESS, CONFIG, c | 0x00); // Set DLPD_CFG to 0; 260 Hz bandwidth, 1 kHz rate 650 | 651 | c = readByte(MPU6050_ADDRESS, INT_ENABLE); 652 | writeByte(MPU6050_ADDRESS, INT_ENABLE, c & ~0xFF); // Clear all interrupts 653 | writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x40); // Enable motion threshold (bits 5) interrupt only 654 | 655 | // Motion detection interrupt requires the absolute value of any axis to lie above the detection threshold 656 | // for at least the counter duration 657 | writeByte(MPU6050_ADDRESS, MOT_THR, 0x80); // Set motion detection to 0.256 g; LSB = 2 mg 658 | writeByte(MPU6050_ADDRESS, MOT_DUR, 0x01); // Set motion detect duration to 1 ms; LSB is 1 ms @ 1 kHz rate 659 | 660 | delay (100); // Add delay for accumulation of samples 661 | 662 | c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG); 663 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x07); // Clear high-pass filter bits [2:0] 664 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c | 0x07); // Set ACCEL_HPF to 7; hold the initial accleration value as a referance 665 | 666 | c = readByte(MPU6050_ADDRESS, PWR_MGMT_2); 667 | writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c & ~0xC7); // Clear standby XA, YA, and ZA bits [3:5] and LP_WAKE_CTRL bits [6:7] 668 | writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c | 0x47); // Set wakeup frequency to 5 Hz, and disable XG, YG, and ZG gyros (bits [0:2]) 669 | 670 | c = readByte(MPU6050_ADDRESS, PWR_MGMT_1); 671 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c & ~0x20); // Clear sleep and cycle bit 5 672 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c | 0x20); // Set cycle bit 5 to begin low power accelerometer motion interrupts 673 | 674 | } 675 | 676 | 677 | void initMPU6050() 678 | { 679 | // wake up device 680 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors 681 | delay(100); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt 682 | 683 | // get stable time source 684 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 685 | 686 | // Configure Gyro and Accelerometer 687 | // Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively; 688 | // DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both 689 | // Maximum delay is 4.9 ms which is just over a 200 Hz maximum rate 690 | writeByte(MPU6050_ADDRESS, CONFIG, 0x03); 691 | 692 | // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) 693 | writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; the same rate set in CONFIG above 694 | 695 | // Set gyroscope full scale range 696 | // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 697 | uint8_t c = readByte(MPU6050_ADDRESS, GYRO_CONFIG); 698 | writeByte(MPU6050_ADDRESS, GYRO_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] 699 | writeByte(MPU6050_ADDRESS, GYRO_CONFIG, c & ~0x18); // Clear AFS bits [4:3] 700 | writeByte(MPU6050_ADDRESS, GYRO_CONFIG, c | Gscale << 3); // Set full scale range for the gyro 701 | 702 | // Set accelerometer configuration 703 | c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG); 704 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] 705 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x18); // Clear AFS bits [4:3] 706 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c | Ascale << 3); // Set full scale range for the accelerometer 707 | 708 | // Configure Interrupts and Bypass Enable 709 | // Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips 710 | // can join the I2C bus and all can be controlled by the Arduino as master 711 | writeByte(MPU6050_ADDRESS, INT_PIN_CFG, 0x22); 712 | writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt 713 | } 714 | 715 | // Function which accumulates gyro and accelerometer data after device initialization. It calculates the average 716 | // of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. 717 | void calibrateMPU6050(float * dest1, float * dest2) 718 | { 719 | uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data 720 | uint16_t ii, packet_count, fifo_count; 721 | int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; 722 | 723 | // reset device, reset all registers, clear gyro and accelerometer bias registers 724 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device 725 | delay(100); 726 | 727 | // get stable time source 728 | // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 729 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); 730 | writeByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00); 731 | delay(200); 732 | 733 | // Configure device for bias calculation 734 | writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts 735 | writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO 736 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source 737 | writeByte(MPU6050_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master 738 | writeByte(MPU6050_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes 739 | writeByte(MPU6050_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP 740 | delay(15); 741 | 742 | // Configure MPU6050 gyro and accelerometer for bias calculation 743 | writeByte(MPU6050_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz 744 | writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz 745 | writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity 746 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity 747 | 748 | uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec 749 | uint16_t accelsensitivity = 16384; // = 16384 LSB/g 750 | 751 | // Configure FIFO to capture accelerometer and gyro data for bias calculation 752 | writeByte(MPU6050_ADDRESS, USER_CTRL, 0x40); // Enable FIFO 753 | writeByte(MPU6050_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 1024 bytes in MPU-6050) 754 | delay(80); // accumulate 80 samples in 80 milliseconds = 960 bytes 755 | 756 | // At end of sample accumulation, turn off FIFO sensor read 757 | writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO 758 | readBytes(MPU6050_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count 759 | fifo_count = ((uint16_t)data[0] << 8) | data[1]; 760 | packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging 761 | 762 | for (ii = 0; ii < packet_count; ii++) { 763 | int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; 764 | readBytes(MPU6050_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging 765 | accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO 766 | accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; 767 | accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; 768 | gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; 769 | gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; 770 | gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; 771 | 772 | accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases 773 | accel_bias[1] += (int32_t) accel_temp[1]; 774 | accel_bias[2] += (int32_t) accel_temp[2]; 775 | gyro_bias[0] += (int32_t) gyro_temp[0]; 776 | gyro_bias[1] += (int32_t) gyro_temp[1]; 777 | gyro_bias[2] += (int32_t) gyro_temp[2]; 778 | 779 | } 780 | accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases 781 | accel_bias[1] /= (int32_t) packet_count; 782 | accel_bias[2] /= (int32_t) packet_count; 783 | gyro_bias[0] /= (int32_t) packet_count; 784 | gyro_bias[1] /= (int32_t) packet_count; 785 | gyro_bias[2] /= (int32_t) packet_count; 786 | 787 | if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation 788 | else {accel_bias[2] += (int32_t) accelsensitivity;} 789 | 790 | // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup 791 | 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 792 | data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases 793 | data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; 794 | data[3] = (-gyro_bias[1]/4) & 0xFF; 795 | data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; 796 | data[5] = (-gyro_bias[2]/4) & 0xFF; 797 | 798 | // Push gyro biases to hardware registers 799 | writeByte(MPU6050_ADDRESS, XG_OFFS_USRH, data[0]); 800 | writeByte(MPU6050_ADDRESS, XG_OFFS_USRL, data[1]); 801 | writeByte(MPU6050_ADDRESS, YG_OFFS_USRH, data[2]); 802 | writeByte(MPU6050_ADDRESS, YG_OFFS_USRL, data[3]); 803 | writeByte(MPU6050_ADDRESS, ZG_OFFS_USRH, data[4]); 804 | writeByte(MPU6050_ADDRESS, ZG_OFFS_USRL, data[5]); 805 | 806 | dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction 807 | dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; 808 | dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; 809 | 810 | // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain 811 | // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold 812 | // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature 813 | // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that 814 | // the accelerometer biases calculated above must be divided by 8. 815 | 816 | int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases 817 | readBytes(MPU6050_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values 818 | accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 819 | readBytes(MPU6050_ADDRESS, YA_OFFSET_H, 2, &data[0]); 820 | accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 821 | readBytes(MPU6050_ADDRESS, ZA_OFFSET_H, 2, &data[0]); 822 | accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 823 | 824 | uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers 825 | uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis 826 | 827 | for(ii = 0; ii < 3; ii++) { 828 | if(accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit 829 | } 830 | 831 | // Construct total accelerometer bias, including calculated average accelerometer bias from above 832 | accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) 833 | accel_bias_reg[1] -= (accel_bias[1]/8); 834 | accel_bias_reg[2] -= (accel_bias[2]/8); 835 | 836 | data[0] = (accel_bias_reg[0] >> 8) & 0xFF; 837 | data[1] = (accel_bias_reg[0]) & 0xFF; 838 | data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers 839 | data[2] = (accel_bias_reg[1] >> 8) & 0xFF; 840 | data[3] = (accel_bias_reg[1]) & 0xFF; 841 | data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers 842 | data[4] = (accel_bias_reg[2] >> 8) & 0xFF; 843 | data[5] = (accel_bias_reg[2]) & 0xFF; 844 | data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers 845 | 846 | // Push accelerometer biases to hardware registers 847 | writeByte(MPU6050_ADDRESS, XA_OFFSET_H, data[0]); 848 | writeByte(MPU6050_ADDRESS, XA_OFFSET_L_TC, data[1]); 849 | writeByte(MPU6050_ADDRESS, YA_OFFSET_H, data[2]); 850 | writeByte(MPU6050_ADDRESS, YA_OFFSET_L_TC, data[3]); 851 | writeByte(MPU6050_ADDRESS, ZA_OFFSET_H, data[4]); 852 | writeByte(MPU6050_ADDRESS, ZA_OFFSET_L_TC, data[5]); 853 | 854 | // Output scaled accelerometer biases for manual subtraction in the main program 855 | dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; 856 | dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; 857 | dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; 858 | } 859 | 860 | 861 | 862 | // Accelerometer and gyroscope self test; check calibration wrt factory settings 863 | void MPU6050SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass 864 | { 865 | uint8_t rawData[4]; 866 | uint8_t selfTest[6]; 867 | float factoryTrim[6]; 868 | 869 | // Configure the accelerometer for self-test 870 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0xF0); // Enable self test on all three axes and set accelerometer range to +/- 8 g 871 | writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s 872 | delay(250); // Delay a while to let the device execute the self-test 873 | rawData[0] = readByte(MPU6050_ADDRESS, SELF_TEST_X); // X-axis self-test results 874 | rawData[1] = readByte(MPU6050_ADDRESS, SELF_TEST_Y); // Y-axis self-test results 875 | rawData[2] = readByte(MPU6050_ADDRESS, SELF_TEST_Z); // Z-axis self-test results 876 | rawData[3] = readByte(MPU6050_ADDRESS, SELF_TEST_A); // Mixed-axis self-test results 877 | // Extract the acceleration test results first 878 | selfTest[0] = (rawData[0] >> 3) | (rawData[3] & 0x30) >> 4 ; // XA_TEST result is a five-bit unsigned integer 879 | selfTest[1] = (rawData[1] >> 3) | (rawData[3] & 0x0C) >> 4 ; // YA_TEST result is a five-bit unsigned integer 880 | selfTest[2] = (rawData[2] >> 3) | (rawData[3] & 0x03) >> 4 ; // ZA_TEST result is a five-bit unsigned integer 881 | // Extract the gyration test results first 882 | selfTest[3] = rawData[0] & 0x1F ; // XG_TEST result is a five-bit unsigned integer 883 | selfTest[4] = rawData[1] & 0x1F ; // YG_TEST result is a five-bit unsigned integer 884 | selfTest[5] = rawData[2] & 0x1F ; // ZG_TEST result is a five-bit unsigned integer 885 | // Process results to allow final comparison with factory set values 886 | factoryTrim[0] = (4096.0*0.34)*(pow( (0.92/0.34) , (((float)selfTest[0] - 1.0)/30.0))); // FT[Xa] factory trim calculation 887 | factoryTrim[1] = (4096.0*0.34)*(pow( (0.92/0.34) , (((float)selfTest[1] - 1.0)/30.0))); // FT[Ya] factory trim calculation 888 | factoryTrim[2] = (4096.0*0.34)*(pow( (0.92/0.34) , (((float)selfTest[2] - 1.0)/30.0))); // FT[Za] factory trim calculation 889 | factoryTrim[3] = ( 25.0*131.0)*(pow( 1.046 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation 890 | factoryTrim[4] = (-25.0*131.0)*(pow( 1.046 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation 891 | factoryTrim[5] = ( 25.0*131.0)*(pow( 1.046 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation 892 | 893 | // Output self-test results and factory trim calculation if desired 894 | // Serial.println(selfTest[0]); Serial.println(selfTest[1]); Serial.println(selfTest[2]); 895 | // Serial.println(selfTest[3]); Serial.println(selfTest[4]); Serial.println(selfTest[5]); 896 | // Serial.println(factoryTrim[0]); Serial.println(factoryTrim[1]); Serial.println(factoryTrim[2]); 897 | // Serial.println(factoryTrim[3]); Serial.println(factoryTrim[4]); Serial.println(factoryTrim[5]); 898 | 899 | // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response 900 | // To get to percent, must multiply by 100 and subtract result from 100 901 | for (int i = 0; i < 6; i++) { 902 | destination[i] = 100.0 + 100.0*((float)selfTest[i] - factoryTrim[i])/factoryTrim[i]; // Report percent differences 903 | } 904 | 905 | } 906 | 907 | void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) 908 | { 909 | Wire.beginTransmission(address); // Initialize the Tx buffer 910 | Wire.write(subAddress); // Put slave register address in Tx buffer 911 | Wire.write(data); // Put data in Tx buffer 912 | Wire.endTransmission(); // Send the Tx buffer 913 | } 914 | 915 | uint8_t readByte(uint8_t address, uint8_t subAddress) 916 | { 917 | uint8_t data; // `data` will store the register data 918 | Wire.beginTransmission(address); // Initialize the Tx buffer 919 | Wire.write(subAddress); // Put slave register address in Tx buffer 920 | Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive 921 | Wire.requestFrom(address, (uint8_t) 1); // Read one byte from slave register address 922 | data = Wire.read(); // Fill Rx buffer with result 923 | return data; // Return data read from slave register 924 | } 925 | 926 | void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) 927 | { 928 | Wire.beginTransmission(address); // Initialize the Tx buffer 929 | Wire.write(subAddress); // Put slave register address in Tx buffer 930 | Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive 931 | uint8_t i = 0; 932 | Wire.requestFrom(address, count); // Read bytes from slave register address 933 | while (Wire.available()) { 934 | dest[i++] = Wire.read(); } // Put read results in the Rx buffer 935 | } 936 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | MPU6050HMC5883AHRS 2 | ================== 3 | 4 | 9 DoF sensor fusion with AHRS output for MPU-6050 gyro/accel + HMC5883 magnetometer sensor 5 | 6 | Demonstrate MPU-6050 basic functionality including initialization, accelerometer trimming, sleep mode functionality as well as 7 | parameterizing the register addresses. Added display functions to allow display to on-breadboard monitor. 8 | No DMP use. We just want to get out the accelerations, temperature, and gyro readings. 9 | Added the HMC5883L magnetometer for true 9 DoF sensor fusion using the open-source Madgwick and Mahony quaternion filtering 10 | algorithms. Out put as Yaw, Pitch, and Roll with absolute orientation to fixed true North (magnetic North plus declination) 11 | and Down (gravity). Easy 9 DoF sensor fusion with inexpensive and ubiquitous sensors. 12 | 13 | Runs on 3.3 V 8 MHz Pro Mini at ~150 Hz filter update rate. 14 | 15 | A discussion of the use and limitations of this sensor and sensor fusion in general is found here: https://github.com/kriswiner/MPU-6050/wiki/Affordable-9-DoF-Sensor-Fusion. 16 | -------------------------------------------------------------------------------- /quaternion Filters.ino: -------------------------------------------------------------------------------- 1 | // Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays" 2 | // (see http://www.x-io.co.uk/category/open-source/ for examples and more details) 3 | // which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute 4 | // device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc. 5 | // The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms 6 | // but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz! 7 | void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) 8 | { 9 | float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability 10 | float norm; 11 | float hx, hy, _2bx, _2bz; 12 | float s1, s2, s3, s4; 13 | float qDot1, qDot2, qDot3, qDot4; 14 | 15 | // Auxiliary variables to avoid repeated arithmetic 16 | float _2q1mx; 17 | float _2q1my; 18 | float _2q1mz; 19 | float _2q2mx; 20 | float _4bx; 21 | float _4bz; 22 | float _2q1 = 2.0f * q1; 23 | float _2q2 = 2.0f * q2; 24 | float _2q3 = 2.0f * q3; 25 | float _2q4 = 2.0f * q4; 26 | float _2q1q3 = 2.0f * q1 * q3; 27 | float _2q3q4 = 2.0f * q3 * q4; 28 | float q1q1 = q1 * q1; 29 | float q1q2 = q1 * q2; 30 | float q1q3 = q1 * q3; 31 | float q1q4 = q1 * q4; 32 | float q2q2 = q2 * q2; 33 | float q2q3 = q2 * q3; 34 | float q2q4 = q2 * q4; 35 | float q3q3 = q3 * q3; 36 | float q3q4 = q3 * q4; 37 | float q4q4 = q4 * q4; 38 | 39 | // Normalise accelerometer measurement 40 | norm = sqrt(ax * ax + ay * ay + az * az); 41 | if (norm == 0.0f) return; // handle NaN 42 | norm = 1.0f/norm; 43 | ax *= norm; 44 | ay *= norm; 45 | az *= norm; 46 | 47 | // Normalise magnetometer measurement 48 | norm = sqrt(mx * mx + my * my + mz * mz); 49 | if (norm == 0.0f) return; // handle NaN 50 | norm = 1.0f/norm; 51 | mx *= norm; 52 | my *= norm; 53 | mz *= norm; 54 | 55 | // Reference direction of Earth's magnetic field 56 | _2q1mx = 2.0f * q1 * mx; 57 | _2q1my = 2.0f * q1 * my; 58 | _2q1mz = 2.0f * q1 * mz; 59 | _2q2mx = 2.0f * q2 * mx; 60 | hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; 61 | hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; 62 | _2bx = sqrt(hx * hx + hy * hy); 63 | _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; 64 | _4bx = 2.0f * _2bx; 65 | _4bz = 2.0f * _2bz; 66 | 67 | // Gradient decent algorithm corrective step 68 | 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); 69 | 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); 70 | 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); 71 | 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); 72 | norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude 73 | norm = 1.0f/norm; 74 | s1 *= norm; 75 | s2 *= norm; 76 | s3 *= norm; 77 | s4 *= norm; 78 | 79 | // Compute rate of change of quaternion 80 | qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1; 81 | qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2; 82 | qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3; 83 | qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4; 84 | 85 | // Integrate to yield quaternion 86 | q1 += qDot1 * deltat; 87 | q2 += qDot2 * deltat; 88 | q3 += qDot3 * deltat; 89 | q4 += qDot4 * deltat; 90 | norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion 91 | norm = 1.0f/norm; 92 | q[0] = q1 * norm; 93 | q[1] = q2 * norm; 94 | q[2] = q3 * norm; 95 | q[3] = q4 * norm; 96 | 97 | } 98 | 99 | 100 | 101 | // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and 102 | // measured ones. 103 | void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) 104 | { 105 | float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability 106 | float norm; 107 | float hx, hy, bx, bz; 108 | float vx, vy, vz, wx, wy, wz; 109 | float ex, ey, ez; 110 | float pa, pb, pc; 111 | 112 | // Auxiliary variables to avoid repeated arithmetic 113 | float q1q1 = q1 * q1; 114 | float q1q2 = q1 * q2; 115 | float q1q3 = q1 * q3; 116 | float q1q4 = q1 * q4; 117 | float q2q2 = q2 * q2; 118 | float q2q3 = q2 * q3; 119 | float q2q4 = q2 * q4; 120 | float q3q3 = q3 * q3; 121 | float q3q4 = q3 * q4; 122 | float q4q4 = q4 * q4; 123 | 124 | // Normalise accelerometer measurement 125 | norm = sqrt(ax * ax + ay * ay + az * az); 126 | if (norm == 0.0f) return; // handle NaN 127 | norm = 1.0f / norm; // use reciprocal for division 128 | ax *= norm; 129 | ay *= norm; 130 | az *= norm; 131 | 132 | // Normalise magnetometer measurement 133 | norm = sqrt(mx * mx + my * my + mz * mz); 134 | if (norm == 0.0f) return; // handle NaN 135 | norm = 1.0f / norm; // use reciprocal for division 136 | mx *= norm; 137 | my *= norm; 138 | mz *= norm; 139 | 140 | // Reference direction of Earth's magnetic field 141 | hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3); 142 | hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2); 143 | bx = sqrt((hx * hx) + (hy * hy)); 144 | bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3); 145 | 146 | // Estimated direction of gravity and magnetic field 147 | vx = 2.0f * (q2q4 - q1q3); 148 | vy = 2.0f * (q1q2 + q3q4); 149 | vz = q1q1 - q2q2 - q3q3 + q4q4; 150 | wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3); 151 | wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4); 152 | wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3); 153 | 154 | // Error is cross product between estimated direction and measured direction of gravity 155 | ex = (ay * vz - az * vy) + (my * wz - mz * wy); 156 | ey = (az * vx - ax * vz) + (mz * wx - mx * wz); 157 | ez = (ax * vy - ay * vx) + (mx * wy - my * wx); 158 | if (Ki > 0.0f) 159 | { 160 | eInt[0] += ex; // accumulate integral error 161 | eInt[1] += ey; 162 | eInt[2] += ez; 163 | } 164 | else 165 | { 166 | eInt[0] = 0.0f; // prevent integral wind up 167 | eInt[1] = 0.0f; 168 | eInt[2] = 0.0f; 169 | } 170 | 171 | // Apply feedback terms 172 | gx = gx + Kp * ex + Ki * eInt[0]; 173 | gy = gy + Kp * ey + Ki * eInt[1]; 174 | gz = gz + Kp * ez + Ki * eInt[2]; 175 | 176 | // Integrate rate of change of quaternion 177 | pa = q2; 178 | pb = q3; 179 | pc = q4; 180 | q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat); 181 | q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat); 182 | q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat); 183 | q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat); 184 | 185 | // Normalise quaternion 186 | norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); 187 | norm = 1.0f / norm; 188 | q[0] = q1 * norm; 189 | q[1] = q2 * norm; 190 | q[2] = q3 * norm; 191 | q[3] = q4 * norm; 192 | 193 | } 194 | 195 | --------------------------------------------------------------------------------