From d104fa3ea3b2e9e8deff36b9d0399f06d479cdeb Mon Sep 17 00:00:00 2001 From: Brent Wilkins Date: Wed, 1 Jun 2016 15:51:20 -0600 Subject: [PATCH 01/28] Hacking this into an Arduino library --- .gitattributes | 22 + .gitignore | 204 ++++ MPU9250BasicAHRS.ino | 1034 ----------------- README.md | 58 +- .../MPU9250BasicAHRS/MPU9250BasicAHRS.ino | 371 ++++++ .../MPU9250BasicAHRS_t3.ino | 0 .../MPU9250_LPS25H_BasicAHRS_t3.ino | 0 .../MPU9250_MPL3115A2_BasicAHRS_t3.ino | 0 .../MPU9250_MS5611_BasicAHRS_t3.ino | 0 .../MPU9250_MS5637_AHRS_t3.ino | 0 {STM32F401 => extras/STM32F401}/MPU9250.h | 0 {STM32F401 => extras/STM32F401}/Readme.md | 0 {STM32F401 => extras/STM32F401}/main.cpp | 0 keywords.txt | 57 + library.properties | 9 + quaternionFilters.ino | 194 ---- src/MPU9250.cpp | 460 ++++++++ src/MPU9250.h | 306 +++++ src/quaternionFilters.cpp | 206 ++++ src/quaternionFilters.h | 9 + 20 files changed, 1685 insertions(+), 1245 deletions(-) create mode 100644 .gitattributes create mode 100644 .gitignore delete mode 100644 MPU9250BasicAHRS.ino create mode 100644 examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino rename MPU9250BasicAHRS_t3.ino => extras/MPU9250BasicAHRS_t3.ino (100%) rename MPU9250_LPS25H_BasicAHRS_t3.ino => extras/MPU9250_LPS25H_BasicAHRS_t3.ino (100%) rename MPU9250_MPL3115A2_BasicAHRS_t3.ino => extras/MPU9250_MPL3115A2_BasicAHRS_t3.ino (100%) rename MPU9250_MS5611_BasicAHRS_t3.ino => extras/MPU9250_MS5611_BasicAHRS_t3.ino (100%) rename MPU9250_MS5637_AHRS_t3.ino => extras/MPU9250_MS5637_AHRS_t3.ino (100%) rename {STM32F401 => extras/STM32F401}/MPU9250.h (100%) rename {STM32F401 => extras/STM32F401}/Readme.md (100%) rename {STM32F401 => extras/STM32F401}/main.cpp (100%) create mode 100644 keywords.txt create mode 100644 library.properties delete mode 100644 quaternionFilters.ino create mode 100644 src/MPU9250.cpp create mode 100644 src/MPU9250.h create mode 100644 src/quaternionFilters.cpp create mode 100644 src/quaternionFilters.h diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..412eeda --- /dev/null +++ b/.gitattributes @@ -0,0 +1,22 @@ +# Auto detect text files and perform LF normalization +* text=auto + +# Custom for Visual Studio +*.cs diff=csharp +*.sln merge=union +*.csproj merge=union +*.vbproj merge=union +*.fsproj merge=union +*.dbproj merge=union + +# Standard to msysgit +*.doc diff=astextplain +*.DOC diff=astextplain +*.docx diff=astextplain +*.DOCX diff=astextplain +*.dot diff=astextplain +*.DOT diff=astextplain +*.pdf diff=astextplain +*.PDF diff=astextplain +*.rtf diff=astextplain +*.RTF diff=astextplain diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..4d3e333 --- /dev/null +++ b/.gitignore @@ -0,0 +1,204 @@ +################# +## SparkFun Useful stuff +################# + +## AVR Development +*.eep +*.elf +*.lst +*.lss +*.sym +*.d +*.o +*.srec +*.map + +## Notepad++ backup files +*.bak + +## BOM files +*bom* + +################# +## Eclipse +################# + +*.pydevproject +.project +.metadata +bin/ +tmp/ +*.tmp +*.bak +*.swp +*~.nib +local.properties +.classpath +.settings/ +.loadpath + +# External tool builders +.externalToolBuilders/ + +# Locally stored "Eclipse launch configurations" +*.launch + +# CDT-specific +.cproject + +# PDT-specific +.buildpath + + +############# +## Eagle +############# + +# Ignore the board and schematic backup files +*.b#? +*.s#? + + +################# +## Visual Studio +################# + +## Ignore Visual Studio temporary files, build results, and +## files generated by popular Visual Studio add-ons. + +# User-specific files +*.suo +*.user +*.sln.docstates + +# Build results +[Dd]ebug/ +[Rr]elease/ +*_i.c +*_p.c +*.ilk +*.meta +*.obj +*.pch +*.pdb +*.pgc +*.pgd +*.rsp +*.sbr +*.tlb +*.tli +*.tlh +*.tmp +*.vspscc +.builds +*.dotCover + +## TODO: If you have NuGet Package Restore enabled, uncomment this +#packages/ + +# Visual C++ cache files +ipch/ +*.aps +*.ncb +*.opensdf +*.sdf + +# Visual Studio profiler +*.psess +*.vsp + +# ReSharper is a .NET coding add-in +_ReSharper* + +# Installshield output folder +[Ee]xpress + +# DocProject is a documentation generator add-in +DocProject/buildhelp/ +DocProject/Help/*.HxT +DocProject/Help/*.HxC +DocProject/Help/*.hhc +DocProject/Help/*.hhk +DocProject/Help/*.hhp +DocProject/Help/Html2 +DocProject/Help/html + +# Click-Once directory +publish + +# Others +[Bb]in +[Oo]bj +sql +TestResults +*.Cache +ClientBin +stylecop.* +~$* +*.dbmdl +Generated_Code #added for RIA/Silverlight projects + +# Backup & report files from converting an old project file to a newer +# Visual Studio version. Backup files are not needed, because we have git ;-) +_UpgradeReport_Files/ +Backup*/ +UpgradeLog*.XML + + +############ +## Windows +############ + +# Windows image file caches +Thumbs.db + +# Folder config file +Desktop.ini + + +############# +## Mac OS +############# + +.DS_Store + + +############# +## Linux +############# + +# backup files (*.bak on Win) +*~ + + +############# +## Python +############# + +*.py[co] + +# Packages +*.egg +*.egg-info +dist +build +eggs +parts +bin +var +sdist +develop-eggs +.installed.cfg + +# Installer logs +pip-log.txt + +# Unit test / coverage reports +.coverage +.tox + +#Translations +*.mo + +#Mr Developer +.mr.developer.cfg diff --git a/MPU9250BasicAHRS.ino b/MPU9250BasicAHRS.ino deleted file mode 100644 index 4f594e2..0000000 --- a/MPU9250BasicAHRS.ino +++ /dev/null @@ -1,1034 +0,0 @@ -/* MPU9250 Basic Example Code - by: Kris Winer - date: April 1, 2014 - license: Beerware - Use this code however you'd like. If you - find it useful you can buy me a beer some time. - - Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, - getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to - allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and - Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1. - - SDA and SCL should have external pull-up resistors (to 3.3V). - 10k resistors are on the EMSENSR-9250 breakout board. - - Hardware setup: - MPU9250 Breakout --------- Arduino - VDD ---------------------- 3.3V - VDDI --------------------- 3.3V - SDA ----------------------- A4 - SCL ----------------------- A5 - GND ---------------------- GND - - Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library. - 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. - We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file. - We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file. - */ -#include -#include -#include -#include - -// Using NOKIA 5110 monochrome 84 x 48 pixel display -// pin 9 - Serial clock out (SCLK) -// pin 8 - Serial data out (DIN) -// pin 7 - Data/Command select (D/C) -// pin 5 - LCD chip select (CS) -// pin 6 - LCD reset (RST) -Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); - -// 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 -// above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map -// -//Magnetometer Registers -#define AK8963_ADDRESS 0x0C -#define WHO_AM_I_AK8963 0x00 // should return 0x48 -#define INFO 0x01 -#define AK8963_ST1 0x02 // data ready status bit 0 -#define AK8963_XOUT_L 0x03 // data -#define AK8963_XOUT_H 0x04 -#define AK8963_YOUT_L 0x05 -#define AK8963_YOUT_H 0x06 -#define AK8963_ZOUT_L 0x07 -#define AK8963_ZOUT_H 0x08 -#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 -#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 -#define AK8963_ASTC 0x0C // Self test control -#define AK8963_I2CDIS 0x0F // I2C disable -#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value -#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value -#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value - -#define SELF_TEST_X_GYRO 0x00 -#define SELF_TEST_Y_GYRO 0x01 -#define SELF_TEST_Z_GYRO 0x02 - -/*#define X_FINE_GAIN 0x03 // [7:0] fine gain -#define Y_FINE_GAIN 0x04 -#define Z_FINE_GAIN 0x05 -#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer -#define XA_OFFSET_L_TC 0x07 -#define YA_OFFSET_H 0x08 -#define YA_OFFSET_L_TC 0x09 -#define ZA_OFFSET_H 0x0A -#define ZA_OFFSET_L_TC 0x0B */ - -#define SELF_TEST_X_ACCEL 0x0D -#define SELF_TEST_Y_ACCEL 0x0E -#define SELF_TEST_Z_ACCEL 0x0F - -#define SELF_TEST_A 0x10 - -#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope -#define XG_OFFSET_L 0x14 -#define YG_OFFSET_H 0x15 -#define YG_OFFSET_L 0x16 -#define ZG_OFFSET_H 0x17 -#define ZG_OFFSET_L 0x18 -#define SMPLRT_DIV 0x19 -#define CONFIG 0x1A -#define GYRO_CONFIG 0x1B -#define ACCEL_CONFIG 0x1C -#define ACCEL_CONFIG2 0x1D -#define LP_ACCEL_ODR 0x1E -#define WOM_THR 0x1F - -#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms -#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] -#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms - -#define FIFO_EN 0x23 -#define I2C_MST_CTRL 0x24 -#define I2C_SLV0_ADDR 0x25 -#define I2C_SLV0_REG 0x26 -#define I2C_SLV0_CTRL 0x27 -#define I2C_SLV1_ADDR 0x28 -#define I2C_SLV1_REG 0x29 -#define I2C_SLV1_CTRL 0x2A -#define I2C_SLV2_ADDR 0x2B -#define I2C_SLV2_REG 0x2C -#define I2C_SLV2_CTRL 0x2D -#define I2C_SLV3_ADDR 0x2E -#define I2C_SLV3_REG 0x2F -#define I2C_SLV3_CTRL 0x30 -#define I2C_SLV4_ADDR 0x31 -#define I2C_SLV4_REG 0x32 -#define I2C_SLV4_DO 0x33 -#define I2C_SLV4_CTRL 0x34 -#define I2C_SLV4_DI 0x35 -#define I2C_MST_STATUS 0x36 -#define INT_PIN_CFG 0x37 -#define INT_ENABLE 0x38 -#define DMP_INT_STATUS 0x39 // Check DMP interrupt -#define INT_STATUS 0x3A -#define ACCEL_XOUT_H 0x3B -#define ACCEL_XOUT_L 0x3C -#define ACCEL_YOUT_H 0x3D -#define ACCEL_YOUT_L 0x3E -#define ACCEL_ZOUT_H 0x3F -#define ACCEL_ZOUT_L 0x40 -#define TEMP_OUT_H 0x41 -#define TEMP_OUT_L 0x42 -#define GYRO_XOUT_H 0x43 -#define GYRO_XOUT_L 0x44 -#define GYRO_YOUT_H 0x45 -#define GYRO_YOUT_L 0x46 -#define GYRO_ZOUT_H 0x47 -#define GYRO_ZOUT_L 0x48 -#define EXT_SENS_DATA_00 0x49 -#define EXT_SENS_DATA_01 0x4A -#define EXT_SENS_DATA_02 0x4B -#define EXT_SENS_DATA_03 0x4C -#define EXT_SENS_DATA_04 0x4D -#define EXT_SENS_DATA_05 0x4E -#define EXT_SENS_DATA_06 0x4F -#define EXT_SENS_DATA_07 0x50 -#define EXT_SENS_DATA_08 0x51 -#define EXT_SENS_DATA_09 0x52 -#define EXT_SENS_DATA_10 0x53 -#define EXT_SENS_DATA_11 0x54 -#define EXT_SENS_DATA_12 0x55 -#define EXT_SENS_DATA_13 0x56 -#define EXT_SENS_DATA_14 0x57 -#define EXT_SENS_DATA_15 0x58 -#define EXT_SENS_DATA_16 0x59 -#define EXT_SENS_DATA_17 0x5A -#define EXT_SENS_DATA_18 0x5B -#define EXT_SENS_DATA_19 0x5C -#define EXT_SENS_DATA_20 0x5D -#define EXT_SENS_DATA_21 0x5E -#define EXT_SENS_DATA_22 0x5F -#define EXT_SENS_DATA_23 0x60 -#define MOT_DETECT_STATUS 0x61 -#define I2C_SLV0_DO 0x63 -#define I2C_SLV1_DO 0x64 -#define I2C_SLV2_DO 0x65 -#define I2C_SLV3_DO 0x66 -#define I2C_MST_DELAY_CTRL 0x67 -#define SIGNAL_PATH_RESET 0x68 -#define MOT_DETECT_CTRL 0x69 -#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP -#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode -#define PWR_MGMT_2 0x6C -#define DMP_BANK 0x6D // Activates a specific bank in the DMP -#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank -#define DMP_REG 0x6F // Register in DMP from which to read or to which to write -#define DMP_REG_1 0x70 -#define DMP_REG_2 0x71 -#define FIFO_COUNTH 0x72 -#define FIFO_COUNTL 0x73 -#define FIFO_R_W 0x74 -#define WHO_AM_I_MPU9250 0x75 // Should return 0x71 -#define XA_OFFSET_H 0x77 -#define XA_OFFSET_L 0x78 -#define YA_OFFSET_H 0x7A -#define YA_OFFSET_L 0x7B -#define ZA_OFFSET_H 0x7D -#define ZA_OFFSET_L 0x7E - -// Using the MSENSR-9250 breakout board, ADO is set to 0 -// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 -#define ADO 1 -#if ADO -#define MPU9250_ADDRESS 0x69 // Device address when ADO = 1 -#else -#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0 -#define AK8963_ADDRESS 0x0C // Address of magnetometer -#endif - -#define AHRS true // set to false for basic data read -#define SerialDebug true // set to true to get Serial output for debugging - -// Set initial input parameters -enum Ascale { - AFS_2G = 0, - AFS_4G, - AFS_8G, - AFS_16G -}; - -enum Gscale { - GFS_250DPS = 0, - GFS_500DPS, - GFS_1000DPS, - GFS_2000DPS -}; - -enum Mscale { - MFS_14BITS = 0, // 0.6 mG per LSB - MFS_16BITS // 0.15 mG per LSB -}; - -// Specify sensor full scale -uint8_t Gscale = GFS_250DPS; -uint8_t Ascale = AFS_2G; -uint8_t Mscale = MFS_16BITS; // Choose either 14-bit or 16-bit magnetometer resolution -uint8_t Mmode = 0x02; // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read -float aRes, gRes, mRes; // scale resolutions per LSB for the sensors - -// Pin definitions -int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins -int myLed = 13; // Set up pin 13 led for toggling - -int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output -int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output -int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output -float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0}; // Factory mag calibration and mag bias -float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer -int16_t tempCount; // temperature raw count output -float temperature; // Stores the real internal chip temperature in degrees Celsius -float SelfTest[6]; // holds results of gyro and accelerometer self test - -// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System) -float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s) -float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) -// There is a tradeoff in the beta parameter between accuracy and response speed. -// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. -// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion. -// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car! -// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec -// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense; -// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. -// In any case, this is the free parameter in the Madgwick filtering and fusion scheme. -float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta -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 -#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 -#define Ki 0.0f - -uint32_t delt_t = 0; // used to control display output rate -uint32_t count = 0, sumCount = 0; // used to control display output rate -float pitch, yaw, roll; -float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes -uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval -uint32_t Now = 0; // used to calculate integration interval - -float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values -float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion -float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method - - -void setup() -{ - Wire.begin(); -// TWBR = 12; // 400 kbit/sec I2C speed - Serial.begin(38400); - - // Set up the interrupt pin, its set as active high, push-pull - pinMode(intPin, INPUT); - digitalWrite(intPin, LOW); - pinMode(myLed, OUTPUT); - digitalWrite(myLed, HIGH); - - display.begin(); // Ini8ialize the display - display.setContrast(58); // Set the contrast - -// Start device display with ID of sensor - display.clearDisplay(); - display.setTextSize(2); - display.setCursor(0,0); display.print("MPU9250"); - display.setTextSize(1); - display.setCursor(0, 20); display.print("9-DOF 16-bit"); - display.setCursor(0, 30); display.print("motion sensor"); - display.setCursor(20,40); display.print("60 ug LSB"); - display.display(); - delay(1000); - -// Set up for data display - display.setTextSize(1); // Set text size to normal, 2 is twice normal etc. - display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen - display.clearDisplay(); // clears the screen and buffer - - // Read the WHO_AM_I register, this is a good test of communication - byte c = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 - Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x71, HEX); - display.setCursor(20,0); display.print("MPU9250"); - display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(c, HEX); - display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0x71, HEX); - display.display(); - delay(1000); - - if (c == 0x71) // WHO_AM_I should always be 0x68 - { - Serial.println("MPU9250 is online..."); - - MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values - Serial.print("x-axis self test: acceleration trim within : "); Serial.print(SelfTest[0],1); Serial.println("% of factory value"); - Serial.print("y-axis self test: acceleration trim within : "); Serial.print(SelfTest[1],1); Serial.println("% of factory value"); - Serial.print("z-axis self test: acceleration trim within : "); Serial.print(SelfTest[2],1); Serial.println("% of factory value"); - Serial.print("x-axis self test: gyration trim within : "); Serial.print(SelfTest[3],1); Serial.println("% of factory value"); - Serial.print("y-axis self test: gyration trim within : "); Serial.print(SelfTest[4],1); Serial.println("% of factory value"); - Serial.print("z-axis self test: gyration trim within : "); Serial.print(SelfTest[5],1); Serial.println("% of factory value"); - - calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers - display.clearDisplay(); - - display.setCursor(0, 0); display.print("MPU9250 bias"); - display.setCursor(0, 8); display.print(" x y z "); - - display.setCursor(0, 16); display.print((int)(1000*accelBias[0])); - display.setCursor(24, 16); display.print((int)(1000*accelBias[1])); - display.setCursor(48, 16); display.print((int)(1000*accelBias[2])); - display.setCursor(72, 16); display.print("mg"); - - display.setCursor(0, 24); display.print(gyroBias[0], 1); - display.setCursor(24, 24); display.print(gyroBias[1], 1); - display.setCursor(48, 24); display.print(gyroBias[2], 1); - display.setCursor(66, 24); display.print("o/s"); - - display.display(); - delay(1000); - - initMPU9250(); - Serial.println("MPU9250 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature - - // Read the WHO_AM_I register of the magnetometer, this is a good test of communication - byte d = readByte(AK8963_ADDRESS, WHO_AM_I_AK8963); // Read WHO_AM_I register for AK8963 - Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" I should be "); Serial.println(0x48, HEX); - display.clearDisplay(); - display.setCursor(20,0); display.print("AK8963"); - display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(d, HEX); - display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0x48, HEX); - display.display(); - delay(1000); - - // Get magnetometer calibration from AK8963 ROM - initAK8963(magCalibration); Serial.println("AK8963 initialized for active data mode...."); // Initialize device for active mode read of magnetometer - - if(SerialDebug) { - // Serial.println("Calibration values: "); - Serial.print("X-Axis sensitivity adjustment value "); Serial.println(magCalibration[0], 2); - Serial.print("Y-Axis sensitivity adjustment value "); Serial.println(magCalibration[1], 2); - Serial.print("Z-Axis sensitivity adjustment value "); Serial.println(magCalibration[2], 2); - } - - display.clearDisplay(); - display.setCursor(20,0); display.print("AK8963"); - display.setCursor(0,10); display.print("ASAX "); display.setCursor(50,10); display.print(magCalibration[0], 2); - display.setCursor(0,20); display.print("ASAY "); display.setCursor(50,20); display.print(magCalibration[1], 2); - display.setCursor(0,30); display.print("ASAZ "); display.setCursor(50,30); display.print(magCalibration[2], 2); - display.display(); - delay(1000); - } - else - { - Serial.print("Could not connect to MPU9250: 0x"); - Serial.println(c, HEX); - while(1) ; // Loop forever if communication doesn't happen - } -} - -void loop() -{ - // If intPin goes high, all data registers have new data - if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt - readAccelData(accelCount); // Read the x/y/z adc values - getAres(); - - // Now we'll calculate the accleration value into actual g's - ax = (float)accelCount[0]*aRes; // - accelBias[0]; // get actual g value, this depends on scale being set - ay = (float)accelCount[1]*aRes; // - accelBias[1]; - az = (float)accelCount[2]*aRes; // - accelBias[2]; - - readGyroData(gyroCount); // Read the x/y/z adc values - getGres(); - - // Calculate the gyro value into actual degrees per second - gx = (float)gyroCount[0]*gRes; // get actual gyro value, this depends on scale being set - gy = (float)gyroCount[1]*gRes; - gz = (float)gyroCount[2]*gRes; - - readMagData(magCount); // Read the x/y/z adc values - getMres(); - magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated - magbias[1] = +120.; // User environmental x-axis correction in milliGauss - magbias[2] = +125.; // User environmental x-axis correction in milliGauss - - // Calculate the magnetometer values in milliGauss - // Include factory calibration per data sheet and user environmental corrections - mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set - my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]; - mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; - } - - Now = micros(); - deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update - lastUpdate = Now; - - sum += deltat; // sum for averaging filter update rate - sumCount++; - - // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of the magnetometer; - // the magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro! - // We have to make some allowance for this orientationmismatch in feeding the output to the quaternion filter. - // For the MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like - // in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention. - // This is ok by aircraft orientation standards! - // Pass gyro rate as rad/s -// MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); - MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); - - - if (!AHRS) { - delt_t = millis() - count; - if(delt_t > 500) { - - if(SerialDebug) { - // Print acceleration values in milligs! - Serial.print("X-acceleration: "); Serial.print(1000*ax); Serial.print(" mg "); - Serial.print("Y-acceleration: "); Serial.print(1000*ay); Serial.print(" mg "); - Serial.print("Z-acceleration: "); Serial.print(1000*az); Serial.println(" mg "); - - // Print gyro values in degree/sec - Serial.print("X-gyro rate: "); Serial.print(gx, 3); Serial.print(" degrees/sec "); - Serial.print("Y-gyro rate: "); Serial.print(gy, 3); Serial.print(" degrees/sec "); - Serial.print("Z-gyro rate: "); Serial.print(gz, 3); Serial.println(" degrees/sec"); - - // Print mag values in degree/sec - Serial.print("X-mag field: "); Serial.print(mx); Serial.print(" mG "); - Serial.print("Y-mag field: "); Serial.print(my); Serial.print(" mG "); - Serial.print("Z-mag field: "); Serial.print(mz); Serial.println(" mG"); - - tempCount = readTempData(); // Read the adc values - temperature = ((float) tempCount) / 333.87 + 21.0; // Temperature in degrees Centigrade - // Print temperature in degrees Centigrade - Serial.print("Temperature is "); Serial.print(temperature, 1); Serial.println(" degrees C"); // Print T values to tenths of s degree C - } - - display.clearDisplay(); - display.setCursor(0, 0); display.print("MPU9250/AK8963"); - display.setCursor(0, 8); display.print(" x y z "); - - display.setCursor(0, 16); display.print((int)(1000*ax)); - display.setCursor(24, 16); display.print((int)(1000*ay)); - display.setCursor(48, 16); display.print((int)(1000*az)); - display.setCursor(72, 16); display.print("mg"); - - display.setCursor(0, 24); display.print((int)(gx)); - display.setCursor(24, 24); display.print((int)(gy)); - display.setCursor(48, 24); display.print((int)(gz)); - display.setCursor(66, 24); display.print("o/s"); - - display.setCursor(0, 32); display.print((int)(mx)); - display.setCursor(24, 32); display.print((int)(my)); - display.setCursor(48, 32); display.print((int)(mz)); - display.setCursor(72, 32); display.print("mG"); - - display.setCursor(0, 40); display.print("Gyro T "); - display.setCursor(50, 40); display.print(temperature, 1); display.print(" C"); - display.display(); - - count = millis(); - digitalWrite(myLed, !digitalRead(myLed)); // toggle led - } - } - else { - - // Serial print and/or display at 0.5 s rate independent of data rates - delt_t = millis() - count; - if (delt_t > 500) { // update LCD once per half-second independent of read rate - - if(SerialDebug) { - Serial.print("ax = "); Serial.print((int)1000*ax); - Serial.print(" ay = "); Serial.print((int)1000*ay); - Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg"); - Serial.print("gx = "); Serial.print( gx, 2); - Serial.print(" gy = "); Serial.print( gy, 2); - Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s"); - Serial.print("mx = "); Serial.print( (int)mx ); - Serial.print(" my = "); Serial.print( (int)my ); - Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG"); - - Serial.print("q0 = "); Serial.print(q[0]); - Serial.print(" qx = "); Serial.print(q[1]); - Serial.print(" qy = "); Serial.print(q[2]); - Serial.print(" qz = "); Serial.println(q[3]); - } - - // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. - // In this coordinate system, the positive z-axis is down toward Earth. - // 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. - // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. - // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. - // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. - // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be - // applied in the correct order which for this configuration is yaw, pitch, and then roll. - // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. - 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]); - pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); - 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]); - pitch *= 180.0f / PI; - yaw *= 180.0f / PI; - yaw -= 13.8; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 - roll *= 180.0f / PI; - - if(SerialDebug) { - Serial.print("Yaw, Pitch, Roll: "); - Serial.print(yaw, 2); - Serial.print(", "); - Serial.print(pitch, 2); - Serial.print(", "); - Serial.println(roll, 2); - - Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz"); - } - - display.clearDisplay(); - - display.setCursor(0, 0); display.print(" x y z "); - - display.setCursor(0, 8); display.print((int)(1000*ax)); - display.setCursor(24, 8); display.print((int)(1000*ay)); - display.setCursor(48, 8); display.print((int)(1000*az)); - display.setCursor(72, 8); display.print("mg"); - - display.setCursor(0, 16); display.print((int)(gx)); - display.setCursor(24, 16); display.print((int)(gy)); - display.setCursor(48, 16); display.print((int)(gz)); - display.setCursor(66, 16); display.print("o/s"); - - display.setCursor(0, 24); display.print((int)(mx)); - display.setCursor(24, 24); display.print((int)(my)); - display.setCursor(48, 24); display.print((int)(mz)); - display.setCursor(72, 24); display.print("mG"); - - display.setCursor(0, 32); display.print((int)(yaw)); - display.setCursor(24, 32); display.print((int)(pitch)); - display.setCursor(48, 32); display.print((int)(roll)); - display.setCursor(66, 32); display.print("ypr"); - - // With these settings the filter is updating at a ~145 Hz rate using the Madgwick scheme and - // >200 Hz using the Mahony scheme even though the display refreshes at only 2 Hz. - // The filter update rate is determined mostly by the mathematical steps in the respective algorithms, - // the processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR: - // an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces - // filter update rates of 36 - 145 and ~38 Hz for the Madgwick and Mahony schemes, respectively. - // This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads. - // This filter update rate should be fast enough to maintain accurate platform orientation for - // stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz - // produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors. - // The 3.3 V 8 MHz Pro Mini is doing pretty well! - display.setCursor(0, 40); display.print("rt: "); display.print((float) sumCount / sum, 2); display.print(" Hz"); - display.display(); - - count = millis(); - sumCount = 0; - sum = 0; - } - } - -} - -//=================================================================================================================== -//====== Set of useful function to access acceleration. gyroscope, magnetometer, and temperature data -//=================================================================================================================== - -void getMres() { - switch (Mscale) - { - // Possible magnetometer scales (and their register bit settings) are: - // 14 bit resolution (0) and 16 bit resolution (1) - case MFS_14BITS: - mRes = 10.*4912./8190.; // Proper scale to return milliGauss - break; - case MFS_16BITS: - mRes = 10.*4912./32760.0; // Proper scale to return milliGauss - break; - } -} - -void getGres() { - switch (Gscale) - { - // Possible gyro scales (and their register bit settings) are: - // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: - case GFS_250DPS: - gRes = 250.0/32768.0; - break; - case GFS_500DPS: - gRes = 500.0/32768.0; - break; - case GFS_1000DPS: - gRes = 1000.0/32768.0; - break; - case GFS_2000DPS: - gRes = 2000.0/32768.0; - break; - } -} - -void getAres() { - switch (Ascale) - { - // Possible accelerometer scales (and their register bit settings) are: - // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: - case AFS_2G: - aRes = 2.0/32768.0; - break; - case AFS_4G: - aRes = 4.0/32768.0; - break; - case AFS_8G: - aRes = 8.0/32768.0; - break; - case AFS_16G: - aRes = 16.0/32768.0; - break; - } -} - - -void readAccelData(int16_t * destination) -{ - uint8_t rawData[6]; // x/y/z accel register data stored here - readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array - destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; - destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; -} - - -void readGyroData(int16_t * destination) -{ - uint8_t rawData[6]; // x/y/z gyro register data stored here - readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array - destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; - destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; -} - -void readMagData(int16_t * destination) -{ - uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition - if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set - readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array - uint8_t c = rawData[6]; // End data read by reading ST2 register - if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data - destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian - destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; - } - } -} - -int16_t readTempData() -{ - uint8_t rawData[2]; // x/y/z gyro register data stored here - readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array - return ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a 16-bit value -} - -void initAK8963(float * destination) -{ - // First extract the factory calibration for each magnetometer axis - uint8_t rawData[3]; // x/y/z gyro calibration data stored here - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer - delay(10); - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode - delay(10); - readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values - destination[0] = (float)(rawData[0] - 128)/256. + 1.; // Return x-axis sensitivity adjustment values, etc. - destination[1] = (float)(rawData[1] - 128)/256. + 1.; - destination[2] = (float)(rawData[2] - 128)/256. + 1.; - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer - delay(10); - // Configure the magnetometer for continuous read and highest resolution - // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, - // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates - writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR - delay(10); -} - - -void initMPU9250() -{ - // wake up device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors - delay(100); // Wait for all registers to reset - - // get stable time source - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else - delay(200); - - // Configure Gyro and Thermometer - // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively; - // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot - // be higher than 1 / 0.0059 = 170 Hz - // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both - // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x03); - - // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; a rate consistent with the filter update rate - // determined inset in CONFIG above - - // Set gyroscope full scale range - // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 - uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value - // c = c & ~0xE0; // Clear self-test bits [7:5] - c = c & ~0x02; // Clear Fchoice bits [1:0] - c = c & ~0x18; // Clear AFS bits [4:3] - c = c | Gscale << 3; // Set full scale range for the gyro - // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register - - // Set accelerometer full-scale range configuration - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value - // c = c & ~0xE0; // Clear self-test bits [7:5] - c = c & ~0x18; // Clear AFS bits [4:3] - c = c | Ascale << 3; // Set full scale range for the accelerometer - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value - - // Set accelerometer sample rate configuration - // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for - // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value - c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) - c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value - // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, - // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting - - // Configure Interrupts and Bypass Enable - // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared, - // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips - // can join the I2C bus and all can be controlled by the Arduino as master - writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22); - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt - delay(100); -} - - -// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average -// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. -void calibrateMPU9250(float * dest1, float * dest2) -{ - uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data - uint16_t ii, packet_count, fifo_count; - int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; - - // reset device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device - delay(100); - - // get stable time source; Auto select clock source to be PLL gyroscope reference if ready - // else use the internal oscillator, bits 2:0 = 001 - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); - writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); - delay(200); - -// Configure device for bias calculation - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source - writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP - delay(15); - -// Configure MPU6050 gyro and accelerometer for bias calculation - writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity - - uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec - uint16_t accelsensitivity = 16384; // = 16384 LSB/g - - // Configure FIFO to capture accelerometer and gyro data for bias calculation - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150) - delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes - -// At end of sample accumulation, turn off FIFO sensor read - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO - readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count - fifo_count = ((uint16_t)data[0] << 8) | data[1]; - packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging - - for (ii = 0; ii < packet_count; ii++) { - int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; - readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging - accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO - accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; - accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; - gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; - gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; - gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; - - accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases - accel_bias[1] += (int32_t) accel_temp[1]; - accel_bias[2] += (int32_t) accel_temp[2]; - gyro_bias[0] += (int32_t) gyro_temp[0]; - gyro_bias[1] += (int32_t) gyro_temp[1]; - gyro_bias[2] += (int32_t) gyro_temp[2]; - -} - accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases - accel_bias[1] /= (int32_t) packet_count; - accel_bias[2] /= (int32_t) packet_count; - gyro_bias[0] /= (int32_t) packet_count; - gyro_bias[1] /= (int32_t) packet_count; - gyro_bias[2] /= (int32_t) packet_count; - - if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation - else {accel_bias[2] += (int32_t) accelsensitivity;} - -// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup - 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 - data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases - data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; - data[3] = (-gyro_bias[1]/4) & 0xFF; - data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; - data[5] = (-gyro_bias[2]/4) & 0xFF; - -// Push gyro biases to hardware registers - writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]); - -// Output scaled gyro biases for display in the main program - dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; - dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; - dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; - -// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain -// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold -// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature -// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that -// the accelerometer biases calculated above must be divided by 8. - - int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases - readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values - accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); - accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); - accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - - uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers - uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis - - for(ii = 0; ii < 3; ii++) { - if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit - } - - // Construct total accelerometer bias, including calculated average accelerometer bias from above - accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) - accel_bias_reg[1] -= (accel_bias[1]/8); - accel_bias_reg[2] -= (accel_bias[2]/8); - - data[0] = (accel_bias_reg[0] >> 8) & 0xFF; - data[1] = (accel_bias_reg[0]) & 0xFF; - data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers - data[2] = (accel_bias_reg[1] >> 8) & 0xFF; - data[3] = (accel_bias_reg[1]) & 0xFF; - data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers - data[4] = (accel_bias_reg[2] >> 8) & 0xFF; - data[5] = (accel_bias_reg[2]) & 0xFF; - data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers - -// Apparently this is not working for the acceleration biases in the MPU-9250 -// Are we handling the temperature correction bit properly? -// Push accelerometer biases to hardware registers - writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); - -// Output scaled accelerometer biases for display in the main program - dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; - dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; - dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; -} - - -// Accelerometer and gyroscope self test; check calibration wrt factory settings -void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass -{ - uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; - uint8_t selfTest[6]; - int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3]; - float factoryTrim[6]; - uint8_t FS = 0; - - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1< 500) { + + if(SerialDebug) { + // Print acceleration values in milligs! + Serial.print("X-acceleration: "); Serial.print(1000*ax); Serial.print(" mg "); + Serial.print("Y-acceleration: "); Serial.print(1000*ay); Serial.print(" mg "); + Serial.print("Z-acceleration: "); Serial.print(1000*az); Serial.println(" mg "); + + // Print gyro values in degree/sec + Serial.print("X-gyro rate: "); Serial.print(gx, 3); Serial.print(" degrees/sec "); + Serial.print("Y-gyro rate: "); Serial.print(gy, 3); Serial.print(" degrees/sec "); + Serial.print("Z-gyro rate: "); Serial.print(gz, 3); Serial.println(" degrees/sec"); + + // Print mag values in degree/sec + Serial.print("X-mag field: "); Serial.print(mx); Serial.print(" mG "); + Serial.print("Y-mag field: "); Serial.print(my); Serial.print(" mG "); + Serial.print("Z-mag field: "); Serial.print(mz); Serial.println(" mG"); + + tempCount = readTempData(); // Read the adc values + temperature = ((float) tempCount) / 333.87 + 21.0; // Temperature in degrees Centigrade + // Print temperature in degrees Centigrade + Serial.print("Temperature is "); Serial.print(temperature, 1); Serial.println(" degrees C"); // Print T values to tenths of s degree C + } + +#ifdef LCD + display.clearDisplay(); + display.setCursor(0, 0); display.print("MPU9250/AK8963"); + display.setCursor(0, 8); display.print(" x y z "); + + display.setCursor(0, 16); display.print((int)(1000*ax)); + display.setCursor(24, 16); display.print((int)(1000*ay)); + display.setCursor(48, 16); display.print((int)(1000*az)); + display.setCursor(72, 16); display.print("mg"); + + display.setCursor(0, 24); display.print((int)(gx)); + display.setCursor(24, 24); display.print((int)(gy)); + display.setCursor(48, 24); display.print((int)(gz)); + display.setCursor(66, 24); display.print("o/s"); + + display.setCursor(0, 32); display.print((int)(mx)); + display.setCursor(24, 32); display.print((int)(my)); + display.setCursor(48, 32); display.print((int)(mz)); + display.setCursor(72, 32); display.print("mG"); + + display.setCursor(0, 40); display.print("Gyro T "); + display.setCursor(50, 40); display.print(temperature, 1); display.print(" C"); + display.display(); +#endif // LCD + + count = millis(); + digitalWrite(myLed, !digitalRead(myLed)); // toggle led + } + } + else { + + // Serial print and/or display at 0.5 s rate independent of data rates + delt_t = millis() - count; + if (delt_t > 500) { // update LCD once per half-second independent of read rate + + if(SerialDebug) { + Serial.print("ax = "); Serial.print((int)1000*ax); + Serial.print(" ay = "); Serial.print((int)1000*ay); + Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg"); + Serial.print("gx = "); Serial.print( gx, 2); + Serial.print(" gy = "); Serial.print( gy, 2); + Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s"); + Serial.print("mx = "); Serial.print( (int)mx ); + Serial.print(" my = "); Serial.print( (int)my ); + Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG"); + + Serial.print("q0 = "); Serial.print(q[0]); + Serial.print(" qx = "); Serial.print(q[1]); + Serial.print(" qy = "); Serial.print(q[2]); + Serial.print(" qz = "); Serial.println(q[3]); + } + + // Define output variables from updated quaternion---these are Tait-Bryan + // angles, commonly used in aircraft orientation. In this coordinate system, + // the positive z-axis is down toward Earth. 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. + // Pitch is angle between sensor x-axis and Earth ground plane, toward the + // Earth is positive, up toward the sky is negative. Roll is angle between + // sensor y-axis and Earth ground plane, y-axis up is positive roll. These + // arise from the definition of the homogeneous rotation matrix constructed + // from quaternions. Tait-Bryan angles as well as Euler angles are + // non-commutative; that is, the get the correct orientation the rotations + // must be applied in the correct order which for this configuration is yaw, + // pitch, and then roll. + // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. + 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]); + pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); + 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]); + pitch *= 180.0f / PI; + yaw *= 180.0f / PI; +// TODO: Declination at Danville, California is 13 degrees 48 minutes and 47 +// seconds on 2014-04-04 + yaw -= 13.8; + roll *= 180.0f / PI; + + if(SerialDebug) { + Serial.print("Yaw, Pitch, Roll: "); + Serial.print(yaw, 2); + Serial.print(", "); + Serial.print(pitch, 2); + Serial.print(", "); + Serial.println(roll, 2); + + Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz"); + } + +#ifdef LCD + display.clearDisplay(); + + display.setCursor(0, 0); display.print(" x y z "); + + display.setCursor(0, 8); display.print((int)(1000*ax)); + display.setCursor(24, 8); display.print((int)(1000*ay)); + display.setCursor(48, 8); display.print((int)(1000*az)); + display.setCursor(72, 8); display.print("mg"); + + display.setCursor(0, 16); display.print((int)(gx)); + display.setCursor(24, 16); display.print((int)(gy)); + display.setCursor(48, 16); display.print((int)(gz)); + display.setCursor(66, 16); display.print("o/s"); + + display.setCursor(0, 24); display.print((int)(mx)); + display.setCursor(24, 24); display.print((int)(my)); + display.setCursor(48, 24); display.print((int)(mz)); + display.setCursor(72, 24); display.print("mG"); + + display.setCursor(0, 32); display.print((int)(yaw)); + display.setCursor(24, 32); display.print((int)(pitch)); + display.setCursor(48, 32); display.print((int)(roll)); + display.setCursor(66, 32); display.print("ypr"); + + // With these settings the filter is updating at a ~145 Hz rate using the + // Madgwick scheme and >200 Hz using the Mahony scheme even though the + // display refreshes at only 2 Hz. The filter update rate is determined + // mostly by the mathematical steps in the respective algorithms, the + // processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR: + // an ODR of 10 Hz for the magnetometer produce the above rates, maximum + // magnetometer ODR of 100 Hz produces filter update rates of 36 - 145 and + // ~38 Hz for the Madgwick and Mahony schemes, respectively. This is + // presumably because the magnetometer read takes longer than the gyro or + // accelerometer reads. This filter update rate should be fast enough to + // maintain accurate platform orientation for stabilization control of a + // fast-moving robot or quadcopter. Compare to the update rate of 200 Hz + // produced by the on-board Digital Motion Processor of Invensense's MPU6050 + // 6 DoF and MPU9150 9DoF sensors. The 3.3 V 8 MHz Pro Mini is doing pretty + // well! + display.setCursor(0, 40); display.print("rt: "); + display.print((float) sumCount / sum, 2); display.print(" Hz"); + display.display(); +#endif // LCD + + count = millis(); + sumCount = 0; + sum = 0; + } + } + +} diff --git a/MPU9250BasicAHRS_t3.ino b/extras/MPU9250BasicAHRS_t3.ino similarity index 100% rename from MPU9250BasicAHRS_t3.ino rename to extras/MPU9250BasicAHRS_t3.ino diff --git a/MPU9250_LPS25H_BasicAHRS_t3.ino b/extras/MPU9250_LPS25H_BasicAHRS_t3.ino similarity index 100% rename from MPU9250_LPS25H_BasicAHRS_t3.ino rename to extras/MPU9250_LPS25H_BasicAHRS_t3.ino diff --git a/MPU9250_MPL3115A2_BasicAHRS_t3.ino b/extras/MPU9250_MPL3115A2_BasicAHRS_t3.ino similarity index 100% rename from MPU9250_MPL3115A2_BasicAHRS_t3.ino rename to extras/MPU9250_MPL3115A2_BasicAHRS_t3.ino diff --git a/MPU9250_MS5611_BasicAHRS_t3.ino b/extras/MPU9250_MS5611_BasicAHRS_t3.ino similarity index 100% rename from MPU9250_MS5611_BasicAHRS_t3.ino rename to extras/MPU9250_MS5611_BasicAHRS_t3.ino diff --git a/MPU9250_MS5637_AHRS_t3.ino b/extras/MPU9250_MS5637_AHRS_t3.ino similarity index 100% rename from MPU9250_MS5637_AHRS_t3.ino rename to extras/MPU9250_MS5637_AHRS_t3.ino diff --git a/STM32F401/MPU9250.h b/extras/STM32F401/MPU9250.h similarity index 100% rename from STM32F401/MPU9250.h rename to extras/STM32F401/MPU9250.h diff --git a/STM32F401/Readme.md b/extras/STM32F401/Readme.md similarity index 100% rename from STM32F401/Readme.md rename to extras/STM32F401/Readme.md diff --git a/STM32F401/main.cpp b/extras/STM32F401/main.cpp similarity index 100% rename from STM32F401/main.cpp rename to extras/STM32F401/main.cpp diff --git a/keywords.txt b/keywords.txt new file mode 100644 index 0000000..e40ad03 --- /dev/null +++ b/keywords.txt @@ -0,0 +1,57 @@ +################################################################################ +# Syntax Coloring Map for SparkFun MPU-9250 Breakout # +################################################################################ + +################################################################################ +# Datatypes (KEYWORD1) +################################################################################ + +MAG_REG_t KEYWORD1 +ACC_REG_t KEYWORD1 +MAG_TEMP_EN_t KEYWORD1 +MAG_XYZDA_t KEYWORD1 +I2C_ADDR_t KEYWORD1 +AxesRaw_t KEYWORD1 +InterfaceMode_t KEYWORD1 +CHIP_t KEYWORD1 +AXIS_t KEYWORD1 +MAG_DO_t KEYWORD1 +MAG_FS_t KEYWORD1 +MAG_BDU_t KEYWORD1 +MAG_OMXY_t KEYWORD1 +MAG_OMZ_t KEYWORD1 +MAG_MD_t KEYWORD1 +ACC_FS_t KEYWORD1 +ACC_BDU_t KEYWORD1 +ACC_ODR_t KEYWORD1 +ACC_AXIS_EN_t KEYWORD1 +ACC_STATUS_FLAGS_t KEYWORD1 + +################################################################################ +# Methods and Functions (KEYWORD2) +################################################################################ + +MPU9250 KEYWORD2 +begin KEYWORD2 +readGyroX KEYWORD2 +readGyroY KEYWORD2 +readGyroZ KEYWORD2 +readAccelX KEYWORD2 +readAccelY KEYWORD2 +readAccelZ KEYWORD2 +readMagX KEYWORD2 +readMagY KEYWORD2 +readMagZ KEYWORD2 +readTempF KEYWORD2 +readTempC KEYWORD2 +getStatus KEYWORD2 + +################################################################################ +# Constants (LITERAL1) +################################################################################ + +SENSITIVITY_ACC LITERAL1 +SENSITIVITY_MAG LITERAL1 +DEBUG LITERAL1 +AERROR LITERAL1 +MERROR LITERAL1 diff --git a/library.properties b/library.properties new file mode 100644 index 0000000..d213c83 --- /dev/null +++ b/library.properties @@ -0,0 +1,9 @@ +name=SparkFun MPU-9250 9 DOF IMU Breakout +version=1.0.0 +author=SparkFun Electronics +maintainer=SparkFun Electronics +sentence=Driver for ST's LSM303C 6-DOF IMU (3-axis accelerometer & 3-axis magnetometer) +paragraph=The LSM303C is a system-in-package featuring a 3D digital linear acceleration sensor and a 3D digital magnetic sensor. The LSM303C has linear acceleration full scales of ±2 g / ±4 g / ±8 g and a magnetic field full scale of ±16 gauss. The LSM303C includes an I2C serial bus interface that supports standard and fast mode (100 kHz and 400 kHz) and a half-duplex subset of the SPI serial interface. +category=Sensors +url=https://github.com/sparkfun/MPU-9250_Breakout +architectures=* diff --git a/quaternionFilters.ino b/quaternionFilters.ino deleted file mode 100644 index b347c43..0000000 --- a/quaternionFilters.ino +++ /dev/null @@ -1,194 +0,0 @@ - -// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays" -// (see http://www.x-io.co.uk/category/open-source/ for examples and more details) -// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute -// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc. -// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms -// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz! - void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) - { - float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability - float norm; - float hx, hy, _2bx, _2bz; - float s1, s2, s3, s4; - float qDot1, qDot2, qDot3, qDot4; - - // Auxiliary variables to avoid repeated arithmetic - float _2q1mx; - float _2q1my; - float _2q1mz; - float _2q2mx; - float _4bx; - float _4bz; - float _2q1 = 2.0f * q1; - float _2q2 = 2.0f * q2; - float _2q3 = 2.0f * q3; - float _2q4 = 2.0f * q4; - float _2q1q3 = 2.0f * q1 * q3; - float _2q3q4 = 2.0f * q3 * q4; - float q1q1 = q1 * q1; - float q1q2 = q1 * q2; - float q1q3 = q1 * q3; - float q1q4 = q1 * q4; - float q2q2 = q2 * q2; - float q2q3 = q2 * q3; - float q2q4 = q2 * q4; - float q3q3 = q3 * q3; - float q3q4 = q3 * q4; - float q4q4 = q4 * q4; - - // Normalise accelerometer measurement - norm = sqrt(ax * ax + ay * ay + az * az); - if (norm == 0.0f) return; // handle NaN - norm = 1.0f/norm; - ax *= norm; - ay *= norm; - az *= norm; - - // Normalise magnetometer measurement - norm = sqrt(mx * mx + my * my + mz * mz); - if (norm == 0.0f) return; // handle NaN - norm = 1.0f/norm; - mx *= norm; - my *= norm; - mz *= norm; - - // Reference direction of Earth's magnetic field - _2q1mx = 2.0f * q1 * mx; - _2q1my = 2.0f * q1 * my; - _2q1mz = 2.0f * q1 * mz; - _2q2mx = 2.0f * q2 * mx; - hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; - hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; - _2bx = sqrt(hx * hx + hy * hy); - _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; - _4bx = 2.0f * _2bx; - _4bz = 2.0f * _2bz; - - // Gradient decent algorithm corrective step - 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); - 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); - 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); - 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); - norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude - norm = 1.0f/norm; - s1 *= norm; - s2 *= norm; - s3 *= norm; - s4 *= norm; - - // Compute rate of change of quaternion - qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1; - qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2; - qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3; - qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4; - - // Integrate to yield quaternion - q1 += qDot1 * deltat; - q2 += qDot2 * deltat; - q3 += qDot3 * deltat; - q4 += qDot4 * deltat; - norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion - norm = 1.0f/norm; - q[0] = q1 * norm; - q[1] = q2 * norm; - q[2] = q3 * norm; - q[3] = q4 * norm; - - } - - - - // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and - // measured ones. - void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) - { - float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability - float norm; - float hx, hy, bx, bz; - float vx, vy, vz, wx, wy, wz; - float ex, ey, ez; - float pa, pb, pc; - - // Auxiliary variables to avoid repeated arithmetic - float q1q1 = q1 * q1; - float q1q2 = q1 * q2; - float q1q3 = q1 * q3; - float q1q4 = q1 * q4; - float q2q2 = q2 * q2; - float q2q3 = q2 * q3; - float q2q4 = q2 * q4; - float q3q3 = q3 * q3; - float q3q4 = q3 * q4; - float q4q4 = q4 * q4; - - // Normalise accelerometer measurement - norm = sqrt(ax * ax + ay * ay + az * az); - if (norm == 0.0f) return; // handle NaN - norm = 1.0f / norm; // use reciprocal for division - ax *= norm; - ay *= norm; - az *= norm; - - // Normalise magnetometer measurement - norm = sqrt(mx * mx + my * my + mz * mz); - if (norm == 0.0f) return; // handle NaN - norm = 1.0f / norm; // use reciprocal for division - mx *= norm; - my *= norm; - mz *= norm; - - // Reference direction of Earth's magnetic field - hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3); - hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2); - bx = sqrt((hx * hx) + (hy * hy)); - bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3); - - // Estimated direction of gravity and magnetic field - vx = 2.0f * (q2q4 - q1q3); - vy = 2.0f * (q1q2 + q3q4); - vz = q1q1 - q2q2 - q3q3 + q4q4; - wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3); - wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4); - wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3); - - // Error is cross product between estimated direction and measured direction of gravity - ex = (ay * vz - az * vy) + (my * wz - mz * wy); - ey = (az * vx - ax * vz) + (mz * wx - mx * wz); - ez = (ax * vy - ay * vx) + (mx * wy - my * wx); - if (Ki > 0.0f) - { - eInt[0] += ex; // accumulate integral error - eInt[1] += ey; - eInt[2] += ez; - } - else - { - eInt[0] = 0.0f; // prevent integral wind up - eInt[1] = 0.0f; - eInt[2] = 0.0f; - } - - // Apply feedback terms - gx = gx + Kp * ex + Ki * eInt[0]; - gy = gy + Kp * ey + Ki * eInt[1]; - gz = gz + Kp * ez + Ki * eInt[2]; - - // Integrate rate of change of quaternion - pa = q2; - pb = q3; - pc = q4; - q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat); - q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat); - q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat); - q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat); - - // Normalise quaternion - norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); - norm = 1.0f / norm; - q[0] = q1 * norm; - q[1] = q2 * norm; - q[2] = q3 * norm; - q[3] = q4 * norm; - - } diff --git a/src/MPU9250.cpp b/src/MPU9250.cpp new file mode 100644 index 0000000..6b3e71e --- /dev/null +++ b/src/MPU9250.cpp @@ -0,0 +1,460 @@ +#include "MPU9250.h" + +//============================================================================== +//====== Set of useful function to access acceleration. gyroscope, magnetometer, +//====== and temperature data +//============================================================================== + +void getMres() { + switch (Mscale) + { + // Possible magnetometer scales (and their register bit settings) are: + // 14 bit resolution (0) and 16 bit resolution (1) + case MFS_14BITS: + mRes = 10.*4912./8190.; // Proper scale to return milliGauss + break; + case MFS_16BITS: + mRes = 10.*4912./32760.0; // Proper scale to return milliGauss + break; + } +} + +void getGres() { + switch (Gscale) + { + // Possible gyro scales (and their register bit settings) are: + // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). + // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: + case GFS_250DPS: + gRes = 250.0/32768.0; + break; + case GFS_500DPS: + gRes = 500.0/32768.0; + break; + case GFS_1000DPS: + gRes = 1000.0/32768.0; + break; + case GFS_2000DPS: + gRes = 2000.0/32768.0; + break; + } +} + +void getAres() { + switch (Ascale) + { + // Possible accelerometer scales (and their register bit settings) are: + // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). + // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: + case AFS_2G: + aRes = 2.0/32768.0; + break; + case AFS_4G: + aRes = 4.0/32768.0; + break; + case AFS_8G: + aRes = 8.0/32768.0; + break; + case AFS_16G: + aRes = 16.0/32768.0; + break; + } +} + + +void readAccelData(int16_t * destination) +{ + uint8_t rawData[6]; // x/y/z accel register data stored here + readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array + destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; + destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; +} + + +void readGyroData(int16_t * destination) +{ + uint8_t rawData[6]; // x/y/z gyro register data stored here + readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array + destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; + destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; +} + +void readMagData(int16_t * destination) +{ + // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of + // data acquisition + uint8_t rawData[7]; + // Wait for magnetometer data ready bit to be set + if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) + { + // Read the six raw data and ST2 registers sequentially into data array + readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); + uint8_t c = rawData[6]; // End data read by reading ST2 register + // Check if magnetic sensor overflow set, if not then report data + if(!(c & 0x08)) + { + // Turn the MSB and LSB into a signed 16-bit value + destination[0] = ((int16_t)rawData[1] << 8) | rawData[0]; + // Data stored as little Endian + destination[1] = ((int16_t)rawData[3] << 8) | rawData[2]; + destination[2] = ((int16_t)rawData[5] << 8) | rawData[4]; + } + } +} + +int16_t readTempData() +{ + uint8_t rawData[2]; // x/y/z gyro register data stored here + readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array + return ((int16_t)rawData[0] << 8) | rawData[1]; // Turn the MSB and LSB into a 16-bit value +} + +void initAK8963(float * destination) +{ + // First extract the factory calibration for each magnetometer axis + uint8_t rawData[3]; // x/y/z gyro calibration data stored here + writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer + delay(10); + writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode + delay(10); + readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values + destination[0] = (float)(rawData[0] - 128)/256. + 1.; // Return x-axis sensitivity adjustment values, etc. + destination[1] = (float)(rawData[1] - 128)/256. + 1.; + destination[2] = (float)(rawData[2] - 128)/256. + 1.; + writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer + delay(10); + // Configure the magnetometer for continuous read and highest resolution + // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, + // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates + writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR + delay(10); +} + + +void initMPU9250() +{ + // wake up device + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors + delay(100); // Wait for all registers to reset + + // get stable time source + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else + delay(200); + + // Configure Gyro and Thermometer + // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively; + // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot + // be higher than 1 / 0.0059 = 170 Hz + // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both + // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz + writeByte(MPU9250_ADDRESS, CONFIG, 0x03); + + // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) + writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; a rate consistent with the filter update rate + // determined inset in CONFIG above + + // Set gyroscope full scale range + // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 + uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value + // c = c & ~0xE0; // Clear self-test bits [7:5] + c = c & ~0x02; // Clear Fchoice bits [1:0] + c = c & ~0x18; // Clear AFS bits [4:3] + c = c | Gscale << 3; // Set full scale range for the gyro + // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG + writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register + + // Set accelerometer full-scale range configuration + c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value + // c = c & ~0xE0; // Clear self-test bits [7:5] + c = c & ~0x18; // Clear AFS bits [4:3] + c = c | Ascale << 3; // Set full scale range for the accelerometer + writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value + + // Set accelerometer sample rate configuration + // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for + // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz + c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value + c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) + c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz + writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value + // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, + // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting + + // Configure Interrupts and Bypass Enable + // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared, + // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips + // can join the I2C bus and all can be controlled by the Arduino as master + writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22); + writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt + delay(100); +} + + +// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average +// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. +void calibrateMPU9250(float * dest1, float * dest2) +{ + uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data + uint16_t ii, packet_count, fifo_count; + int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; + + // reset device + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device + delay(100); + + // get stable time source; Auto select clock source to be PLL gyroscope reference if ready + // else use the internal oscillator, bits 2:0 = 001 + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); + writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); + delay(200); + +// Configure device for bias calculation + writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts + writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source + writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master + writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes + writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP + delay(15); + +// Configure MPU6050 gyro and accelerometer for bias calculation + writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz + writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz + writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity + writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity + + uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec + uint16_t accelsensitivity = 16384; // = 16384 LSB/g + + // Configure FIFO to capture accelerometer and gyro data for bias calculation + writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO + writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150) + delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes + +// At end of sample accumulation, turn off FIFO sensor read + writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO + readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count + fifo_count = ((uint16_t)data[0] << 8) | data[1]; + packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging + + for (ii = 0; ii < packet_count; ii++) + { + int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; + readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging + accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ); // Form signed 16-bit integer for each sample in FIFO + accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ); + accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ); + gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ); + gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ); + gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]); + + accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases + accel_bias[1] += (int32_t) accel_temp[1]; + accel_bias[2] += (int32_t) accel_temp[2]; + gyro_bias[0] += (int32_t) gyro_temp[0]; + gyro_bias[1] += (int32_t) gyro_temp[1]; + gyro_bias[2] += (int32_t) gyro_temp[2]; + } + accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases + accel_bias[1] /= (int32_t) packet_count; + accel_bias[2] /= (int32_t) packet_count; + gyro_bias[0] /= (int32_t) packet_count; + gyro_bias[1] /= (int32_t) packet_count; + gyro_bias[2] /= (int32_t) packet_count; + + if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation + else {accel_bias[2] += (int32_t) accelsensitivity;} + +// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup + 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 + data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases + data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; + data[3] = (-gyro_bias[1]/4) & 0xFF; + data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; + data[5] = (-gyro_bias[2]/4) & 0xFF; + +// Push gyro biases to hardware registers + writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]); + writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]); + writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]); + writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]); + writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]); + writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]); + +// Output scaled gyro biases for display in the main program + dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; + dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; + dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; + +// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain +// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold +// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature +// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that +// the accelerometer biases calculated above must be divided by 8. + + int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases + readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values + accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]); + readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); + accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]); + readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); + accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]); + + uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers + uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis + + for(ii = 0; ii < 3; ii++) { + if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit + } + + // Construct total accelerometer bias, including calculated average accelerometer bias from above + accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) + accel_bias_reg[1] -= (accel_bias[1]/8); + accel_bias_reg[2] -= (accel_bias[2]/8); + + data[0] = (accel_bias_reg[0] >> 8) & 0xFF; + data[1] = (accel_bias_reg[0]) & 0xFF; + data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers + data[2] = (accel_bias_reg[1] >> 8) & 0xFF; + data[3] = (accel_bias_reg[1]) & 0xFF; + data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers + data[4] = (accel_bias_reg[2] >> 8) & 0xFF; + data[5] = (accel_bias_reg[2]) & 0xFF; + data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers + +// Apparently this is not working for the acceleration biases in the MPU-9250 +// Are we handling the temperature correction bit properly? +// Push accelerometer biases to hardware registers + writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]); + writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]); + writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]); + writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]); + writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]); + writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); + +// Output scaled accelerometer biases for display in the main program + dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; + dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; + dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; +} + + +// Accelerometer and gyroscope self test; check calibration wrt factory settings +void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass +{ + uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; + uint8_t selfTest[6]; + int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3]; + float factoryTrim[6]; + uint8_t FS = 0; + + writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz + writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz + writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1< +#include + +#ifdef LCD +#include +#include +#endif // LCD + +//#include "quaternionFilters.h" + +#ifdef LCD +// Using NOKIA 5110 monochrome 84 x 48 pixel display +// pin 9 - Serial clock out (SCLK) +// pin 8 - Serial data out (DIN) +// pin 7 - Data/Command select (D/C) +// pin 5 - LCD chip select (CS) +// pin 6 - LCD reset (RST) +Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); +#endif // LCD + +// 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 above +// document; the MPU9250 and MPU9150 are virtually identical but the latter has +// a different register map + +//Magnetometer Registers +#define AK8963_ADDRESS 0x0C +#define WHO_AM_I_AK8963 0x00 // should return 0x48 +#define INFO 0x01 +#define AK8963_ST1 0x02 // data ready status bit 0 +#define AK8963_XOUT_L 0x03 // data +#define AK8963_XOUT_H 0x04 +#define AK8963_YOUT_L 0x05 +#define AK8963_YOUT_H 0x06 +#define AK8963_ZOUT_L 0x07 +#define AK8963_ZOUT_H 0x08 +#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 +#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 +#define AK8963_ASTC 0x0C // Self test control +#define AK8963_I2CDIS 0x0F // I2C disable +#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value +#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value +#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value + +#define SELF_TEST_X_GYRO 0x00 +#define SELF_TEST_Y_GYRO 0x01 +#define SELF_TEST_Z_GYRO 0x02 + +/*#define X_FINE_GAIN 0x03 // [7:0] fine gain +#define Y_FINE_GAIN 0x04 +#define Z_FINE_GAIN 0x05 +#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer +#define XA_OFFSET_L_TC 0x07 +#define YA_OFFSET_H 0x08 +#define YA_OFFSET_L_TC 0x09 +#define ZA_OFFSET_H 0x0A +#define ZA_OFFSET_L_TC 0x0B */ + +#define SELF_TEST_X_ACCEL 0x0D +#define SELF_TEST_Y_ACCEL 0x0E +#define SELF_TEST_Z_ACCEL 0x0F + +#define SELF_TEST_A 0x10 + +#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope +#define XG_OFFSET_L 0x14 +#define YG_OFFSET_H 0x15 +#define YG_OFFSET_L 0x16 +#define ZG_OFFSET_H 0x17 +#define ZG_OFFSET_L 0x18 +#define SMPLRT_DIV 0x19 +#define CONFIG 0x1A +#define GYRO_CONFIG 0x1B +#define ACCEL_CONFIG 0x1C +#define ACCEL_CONFIG2 0x1D +#define LP_ACCEL_ODR 0x1E +#define WOM_THR 0x1F + +#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms +#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] +#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms + +#define FIFO_EN 0x23 +#define I2C_MST_CTRL 0x24 +#define I2C_SLV0_ADDR 0x25 +#define I2C_SLV0_REG 0x26 +#define I2C_SLV0_CTRL 0x27 +#define I2C_SLV1_ADDR 0x28 +#define I2C_SLV1_REG 0x29 +#define I2C_SLV1_CTRL 0x2A +#define I2C_SLV2_ADDR 0x2B +#define I2C_SLV2_REG 0x2C +#define I2C_SLV2_CTRL 0x2D +#define I2C_SLV3_ADDR 0x2E +#define I2C_SLV3_REG 0x2F +#define I2C_SLV3_CTRL 0x30 +#define I2C_SLV4_ADDR 0x31 +#define I2C_SLV4_REG 0x32 +#define I2C_SLV4_DO 0x33 +#define I2C_SLV4_CTRL 0x34 +#define I2C_SLV4_DI 0x35 +#define I2C_MST_STATUS 0x36 +#define INT_PIN_CFG 0x37 +#define INT_ENABLE 0x38 +#define DMP_INT_STATUS 0x39 // Check DMP interrupt +#define INT_STATUS 0x3A +#define ACCEL_XOUT_H 0x3B +#define ACCEL_XOUT_L 0x3C +#define ACCEL_YOUT_H 0x3D +#define ACCEL_YOUT_L 0x3E +#define ACCEL_ZOUT_H 0x3F +#define ACCEL_ZOUT_L 0x40 +#define TEMP_OUT_H 0x41 +#define TEMP_OUT_L 0x42 +#define GYRO_XOUT_H 0x43 +#define GYRO_XOUT_L 0x44 +#define GYRO_YOUT_H 0x45 +#define GYRO_YOUT_L 0x46 +#define GYRO_ZOUT_H 0x47 +#define GYRO_ZOUT_L 0x48 +#define EXT_SENS_DATA_00 0x49 +#define EXT_SENS_DATA_01 0x4A +#define EXT_SENS_DATA_02 0x4B +#define EXT_SENS_DATA_03 0x4C +#define EXT_SENS_DATA_04 0x4D +#define EXT_SENS_DATA_05 0x4E +#define EXT_SENS_DATA_06 0x4F +#define EXT_SENS_DATA_07 0x50 +#define EXT_SENS_DATA_08 0x51 +#define EXT_SENS_DATA_09 0x52 +#define EXT_SENS_DATA_10 0x53 +#define EXT_SENS_DATA_11 0x54 +#define EXT_SENS_DATA_12 0x55 +#define EXT_SENS_DATA_13 0x56 +#define EXT_SENS_DATA_14 0x57 +#define EXT_SENS_DATA_15 0x58 +#define EXT_SENS_DATA_16 0x59 +#define EXT_SENS_DATA_17 0x5A +#define EXT_SENS_DATA_18 0x5B +#define EXT_SENS_DATA_19 0x5C +#define EXT_SENS_DATA_20 0x5D +#define EXT_SENS_DATA_21 0x5E +#define EXT_SENS_DATA_22 0x5F +#define EXT_SENS_DATA_23 0x60 +#define MOT_DETECT_STATUS 0x61 +#define I2C_SLV0_DO 0x63 +#define I2C_SLV1_DO 0x64 +#define I2C_SLV2_DO 0x65 +#define I2C_SLV3_DO 0x66 +#define I2C_MST_DELAY_CTRL 0x67 +#define SIGNAL_PATH_RESET 0x68 +#define MOT_DETECT_CTRL 0x69 +#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP +#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode +#define PWR_MGMT_2 0x6C +#define DMP_BANK 0x6D // Activates a specific bank in the DMP +#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank +#define DMP_REG 0x6F // Register in DMP from which to read or to which to write +#define DMP_REG_1 0x70 +#define DMP_REG_2 0x71 +#define FIFO_COUNTH 0x72 +#define FIFO_COUNTL 0x73 +#define FIFO_R_W 0x74 +#define WHO_AM_I_MPU9250 0x75 // Should return 0x71 +#define XA_OFFSET_H 0x77 +#define XA_OFFSET_L 0x78 +#define YA_OFFSET_H 0x7A +#define YA_OFFSET_L 0x7B +#define ZA_OFFSET_H 0x7D +#define ZA_OFFSET_L 0x7E + +// Using the MPU-9250 breakout board, ADO is set to 0 +// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 +#define ADO 0 +#if ADO +#define MPU9250_ADDRESS 0x69 // Device address when ADO = 1 +#else +#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0 +#define AK8963_ADDRESS 0x0C // Address of magnetometer +#endif // AD0 + +#define AHRS true // Set to false for basic data read +#define SerialDebug true // Set to true to get Serial output for debugging + +// Set initial input parameters +enum Ascale { + AFS_2G = 0, + AFS_4G, + AFS_8G, + AFS_16G +}; + +enum Gscale { + GFS_250DPS = 0, + GFS_500DPS, + GFS_1000DPS, + GFS_2000DPS +}; + +enum Mscale { + MFS_14BITS = 0, // 0.6 mG per LSB + MFS_16BITS // 0.15 mG per LSB +}; + +// Specify sensor full scale +uint8_t Gscale = GFS_250DPS; +uint8_t Ascale = AFS_2G; +// Choose either 14-bit or 16-bit magnetometer resolution +uint8_t Mscale = MFS_16BITS; +// 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read +uint8_t Mmode = 0x02; +// Scale resolutions per LSB for the sensors +float aRes, gRes, mRes; + +// Pin definitions +int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins +int myLed = 13; // Set up pin 13 led for toggling + +int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output +int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output +int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output +// Factory mag calibration and mag bias +float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0}; +// Bias corrections for gyro and accelerometer +float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; +int16_t tempCount; // Temperature raw count output +float temperature; // Stores the real internal chip temperature in Celsius +float SelfTest[6]; // Holds results of gyro and accelerometer self test + +// Global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference +// System) +// gyroscope measurement error in rads/s (start at 40 deg/s) +float GyroMeasError = PI * (40.0f / 180.0f); +// gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) +float GyroMeasDrift = PI * (0.0f / 180.0f); +// There is a tradeoff in the beta parameter between accuracy and response +// speed. In the original Madgwick study, beta of 0.041 (corresponding to +// GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. However, +// with this value, the LSM9SD0 response time is about 10 seconds to a stable +// initial quaternion. Subsequent changes also require a longish lag time to a +// stable output, not fast enough for a quadcopter or robot car! By increasing +// beta (GyroMeasError) by about a factor of fifteen, the response time constant +// is reduced to ~2 sec +// I haven't noticed any reduction in solution accuracy. This is essentially the +// I coefficient in a PID control sense; the bigger the feedback coefficient, +// the faster the solution converges, usually at the expense of accuracy. In any +// case, this is the free parameter in the Madgwick filtering and fusion scheme. +float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // Compute beta +// Compute zeta, the other free parameter in the Madgwick scheme usually set to +// a small or zero value +float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; +// These are the free parameters in the Mahony filter and fusion scheme, Kp for +// proportional feedback, Ki for integral +#define Kp 2.0f * 5.0f +#define Ki 0.0f + +uint32_t delt_t = 0; // Used to control display output rate +uint32_t count = 0, sumCount = 0; // Used to control display output rate +float pitch, yaw, roll; +float deltat = 0.0f, sum = 0.0f; // Integration interval for both filter schemes +// Used to calculate integration interval +uint32_t lastUpdate = 0, firstUpdate = 0; +uint32_t Now = 0; // Used to calculate integration interval + +// Variables to hold latest sensor data values +float ax, ay, az, gx, gy, gz, mx, my, mz; +// Vector to hold quaternion +float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; +// Vector to hold integral error for Mahony method +float eInt[3] = {0.0f, 0.0f, 0.0f}; + +// Forward declarations of below functions +void MPU9250SelfTest(float *); +void calibrateMPU9250(float *, float *); +void getMres(); +void getGres(); +void getAres(); +void readAccelData(int16_t *); +void readGyroData(int16_t *); +void readMagData(int16_t *); +int16_t readTempData(); +void initAK8963(float *); +void initMPU9250(); +void calibrateMPU9250(float *, float *); +void MPU9250SelfTest(float *); +void writeByte(uint8_t, uint8_t, uint8_t); +uint8_t readByte(uint8_t, uint8_t); +void readBytes(uint8_t, uint8_t, uint8_t, uint8_t *); + +/* +uint8_t readByte(uint8_t address, uint8_t subAddress); +void MPU9250SelfTest(float * destination); +void calibrateMPU9250(float * dest1, float * dest2); +*/ + +#endif // _MPU9250_H_ diff --git a/src/quaternionFilters.cpp b/src/quaternionFilters.cpp new file mode 100644 index 0000000..d07d2f1 --- /dev/null +++ b/src/quaternionFilters.cpp @@ -0,0 +1,206 @@ + +// Implementation of Sebastian Madgwick's "...efficient orientation filter +// for... inertial/magnetic sensor arrays" +// (see http://www.x-io.co.uk/category/open-source/ for examples & more details) +// which fuses acceleration, rotation rate, and magnetic moments to produce a +// quaternion-based estimate of absolute device orientation -- which can be +// converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc. +// The performance of the orientation filter is at least as good as conventional +// Kalman-based filtering algorithms but is much less computationally +// intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz! + +//#ifndef _QUATERNIONfILTERS_H_ +//#define _QUATERNIONfILTERS_H_ + +#include "MPU9250.h" + +void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) +{ + // short name local variable for readability + float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; + float norm; + float hx, hy, _2bx, _2bz; + float s1, s2, s3, s4; + float qDot1, qDot2, qDot3, qDot4; + + // Auxiliary variables to avoid repeated arithmetic + float _2q1mx; + float _2q1my; + float _2q1mz; + float _2q2mx; + float _4bx; + float _4bz; + float _2q1 = 2.0f * q1; + float _2q2 = 2.0f * q2; + float _2q3 = 2.0f * q3; + float _2q4 = 2.0f * q4; + float _2q1q3 = 2.0f * q1 * q3; + float _2q3q4 = 2.0f * q3 * q4; + float q1q1 = q1 * q1; + float q1q2 = q1 * q2; + float q1q3 = q1 * q3; + float q1q4 = q1 * q4; + float q2q2 = q2 * q2; + float q2q3 = q2 * q3; + float q2q4 = q2 * q4; + float q3q3 = q3 * q3; + float q3q4 = q3 * q4; + float q4q4 = q4 * q4; + + // Normalise accelerometer measurement + norm = sqrt(ax * ax + ay * ay + az * az); + if (norm == 0.0f) return; // handle NaN + norm = 1.0f/norm; + ax *= norm; + ay *= norm; + az *= norm; + + // Normalise magnetometer measurement + norm = sqrt(mx * mx + my * my + mz * mz); + if (norm == 0.0f) return; // handle NaN + norm = 1.0f/norm; + mx *= norm; + my *= norm; + mz *= norm; + + // Reference direction of Earth's magnetic field + _2q1mx = 2.0f * q1 * mx; + _2q1my = 2.0f * q1 * my; + _2q1mz = 2.0f * q1 * mz; + _2q2mx = 2.0f * q2 * mx; + hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; + hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; + _2bx = sqrt(hx * hx + hy * hy); + _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; + _4bx = 2.0f * _2bx; + _4bz = 2.0f * _2bz; + + // Gradient decent algorithm corrective step + 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); + 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); + 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); + 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); + norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude + norm = 1.0f/norm; + s1 *= norm; + s2 *= norm; + s3 *= norm; + s4 *= norm; + + // Compute rate of change of quaternion + qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1; + qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2; + qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3; + qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4; + + // Integrate to yield quaternion + q1 += qDot1 * deltat; + q2 += qDot2 * deltat; + q3 += qDot3 * deltat; + q4 += qDot4 * deltat; + norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion + norm = 1.0f/norm; + q[0] = q1 * norm; + q[1] = q2 * norm; + q[2] = q3 * norm; + q[3] = q4 * norm; +} + + + +// Similar to Madgwick scheme but uses proportional and integral filtering on +// the error between estimated reference vectors and measured ones. +void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) +{ + // short name local variable for readability + float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; + float norm; + float hx, hy, bx, bz; + float vx, vy, vz, wx, wy, wz; + float ex, ey, ez; + float pa, pb, pc; + + // Auxiliary variables to avoid repeated arithmetic + float q1q1 = q1 * q1; + float q1q2 = q1 * q2; + float q1q3 = q1 * q3; + float q1q4 = q1 * q4; + float q2q2 = q2 * q2; + float q2q3 = q2 * q3; + float q2q4 = q2 * q4; + float q3q3 = q3 * q3; + float q3q4 = q3 * q4; + float q4q4 = q4 * q4; + + // Normalise accelerometer measurement + norm = sqrt(ax * ax + ay * ay + az * az); + if (norm == 0.0f) return; // handle NaN + norm = 1.0f / norm; // use reciprocal for division + ax *= norm; + ay *= norm; + az *= norm; + + // Normalise magnetometer measurement + norm = sqrt(mx * mx + my * my + mz * mz); + if (norm == 0.0f) return; // handle NaN + norm = 1.0f / norm; // use reciprocal for division + mx *= norm; + my *= norm; + mz *= norm; + + // Reference direction of Earth's magnetic field + hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3); + hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2); + bx = sqrt((hx * hx) + (hy * hy)); + bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3); + + // Estimated direction of gravity and magnetic field + vx = 2.0f * (q2q4 - q1q3); + vy = 2.0f * (q1q2 + q3q4); + vz = q1q1 - q2q2 - q3q3 + q4q4; + wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3); + wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4); + wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3); + + // Error is cross product between estimated direction and measured direction of gravity + ex = (ay * vz - az * vy) + (my * wz - mz * wy); + ey = (az * vx - ax * vz) + (mz * wx - mx * wz); + ez = (ax * vy - ay * vx) + (mx * wy - my * wx); + if (Ki > 0.0f) + { + eInt[0] += ex; // accumulate integral error + eInt[1] += ey; + eInt[2] += ez; + } + else + { + eInt[0] = 0.0f; // prevent integral wind up + eInt[1] = 0.0f; + eInt[2] = 0.0f; + } + + // Apply feedback terms + gx = gx + Kp * ex + Ki * eInt[0]; + gy = gy + Kp * ey + Ki * eInt[1]; + gz = gz + Kp * ez + Ki * eInt[2]; + + // Integrate rate of change of quaternion + pa = q2; + pb = q3; + pc = q4; + q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat); + q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat); + q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat); + q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat); + + // Normalise quaternion + norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); + norm = 1.0f / norm; + q[0] = q1 * norm; + q[1] = q2 * norm; + q[2] = q3 * norm; + q[3] = q4 * norm; +} + +//#endif // _QUATERNIONfILTERS_H_ diff --git a/src/quaternionFilters.h b/src/quaternionFilters.h new file mode 100644 index 0000000..1f5ef43 --- /dev/null +++ b/src/quaternionFilters.h @@ -0,0 +1,9 @@ +#ifndef _QUATERNIONFILTERS_H_ +#define _QUATERNIONFILTERS_H_ + +void MadgwickQuaternionUpdate(float, float, float, float, float, float, float, + float, float); +void MahonyQuaternionUpdate(float, float, float, float, float, float, float, + float, float); + +#endif // _QUATERNIONFILTERS_H_ From 1b85b4a2ed482855f3da3cd698a4f9e35cad9b24 Mon Sep 17 00:00:00 2001 From: Brent Wilkins Date: Thu, 2 Jun 2016 15:35:39 -0600 Subject: [PATCH 02/28] Mostly functional, but gross --- .../MPU9250BasicAHRS/MPU9250BasicAHRS.ino | 529 ++++++++++-------- src/MPU9250.cpp | 58 +- src/MPU9250.h | 431 +++++++------- src/quaternionFilters.cpp | 26 +- src/quaternionFilters.h | 42 +- 5 files changed, 578 insertions(+), 508 deletions(-) diff --git a/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino b/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino index d7d3065..9cd66cd 100644 --- a/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino +++ b/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino @@ -1,20 +1,20 @@ /* MPU9250 Basic Example Code by: Kris Winer date: April 1, 2014 - license: Beerware - Use this code however you'd like. If you + license: Beerware - Use this code however you'd like. If you find it useful you can buy me a beer some time. Modified by Brent Wilkins May 31, 2016 - + Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1. - + SDA and SCL should have external pull-up resistors (to 3.3V). 10k resistors are on the EMSENSR-9250 breakout board. - + Hardware setup: MPU9250 Breakout --------- Arduino VDD ---------------------- 3.3V @@ -27,23 +27,42 @@ #include "quaternionFilters.h" #include "MPU9250.h" +#ifdef LCD +#include +#include + +// Using NOKIA 5110 monochrome 84 x 48 pixel display +// pin 9 - Serial clock out (SCLK) +// pin 8 - Serial data out (DIN) +// pin 7 - Data/Command select (D/C) +// pin 5 - LCD chip select (CS) +// pin 6 - LCD reset (RST) +Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); +#endif // LCD + +// Pin definitions +int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins +int myLed = 13; // Set up pin 13 led for toggling + +MPU9250 myIMU; + void setup() { Wire.begin(); -// TWBR = 12; // 400 kbit/sec I2C speed + // TWBR = 12; // 400 kbit/sec I2C speed Serial.begin(38400); - + // Set up the interrupt pin, its set as active high, push-pull pinMode(intPin, INPUT); digitalWrite(intPin, LOW); pinMode(myLed, OUTPUT); digitalWrite(myLed, HIGH); - + #ifdef LCD display.begin(); // Ini8ialize the display display.setContrast(58); // Set the contrast - -// Start device display with ID of sensor + + // Start device display with ID of sensor display.clearDisplay(); display.setTextSize(2); display.setCursor(0,0); display.print("MPU9250"); @@ -54,96 +73,119 @@ void setup() display.display(); delay(1000); -// Set up for data display + // Set up for data display display.setTextSize(1); // Set text size to normal, 2 is twice normal etc. display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen display.clearDisplay(); // clears the screen and buffer #endif // LCD // Read the WHO_AM_I register, this is a good test of communication - byte c = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 - Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x71, HEX); + byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); + Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); + Serial.print(" I should be "); Serial.println(0x71, HEX); + #ifdef LCD display.setCursor(20,0); display.print("MPU9250"); display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(c, HEX); + display.setCursor(0,20); display.print(c, HEX); display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0x71, HEX); + display.setCursor(0,40); display.print(0x71, HEX); display.display(); + delay(1000); #endif // LCD - delay(1000); if (c == 0x71) // WHO_AM_I should always be 0x68 - { + { Serial.println("MPU9250 is online..."); - + // Start by performing self test and reporting values - MPU9250SelfTest(SelfTest); - Serial.print("x-axis self test: acceleration trim within : "); Serial.print(SelfTest[0],1); Serial.println("% of factory value"); - Serial.print("y-axis self test: acceleration trim within : "); Serial.print(SelfTest[1],1); Serial.println("% of factory value"); - Serial.print("z-axis self test: acceleration trim within : "); Serial.print(SelfTest[2],1); Serial.println("% of factory value"); - Serial.print("x-axis self test: gyration trim within : "); Serial.print(SelfTest[3],1); Serial.println("% of factory value"); - Serial.print("y-axis self test: gyration trim within : "); Serial.print(SelfTest[4],1); Serial.println("% of factory value"); - Serial.print("z-axis self test: gyration trim within : "); Serial.print(SelfTest[5],1); Serial.println("% of factory value"); - - calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers + myIMU.MPU9250SelfTest(myIMU.SelfTest); + Serial.print("x-axis self test: acceleration trim within : "); + Serial.print(myIMU.SelfTest[0],1); Serial.println("% of factory value"); + Serial.print("y-axis self test: acceleration trim within : "); + Serial.print(myIMU.SelfTest[1],1); Serial.println("% of factory value"); + Serial.print("z-axis self test: acceleration trim within : "); + Serial.print(myIMU.SelfTest[2],1); Serial.println("% of factory value"); + Serial.print("x-axis self test: gyration trim within : "); + Serial.print(myIMU.SelfTest[3],1); Serial.println("% of factory value"); + Serial.print("y-axis self test: gyration trim within : "); + Serial.print(myIMU.SelfTest[4],1); Serial.println("% of factory value"); + Serial.print("z-axis self test: gyration trim within : "); + Serial.print(myIMU.SelfTest[5],1); Serial.println("% of factory value"); + + // Calibrate gyro and accelerometers, load biases in bias registers + myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias); + #ifdef LCD display.clearDisplay(); - + display.setCursor(0, 0); display.print("MPU9250 bias"); display.setCursor(0, 8); display.print(" x y z "); - display.setCursor(0, 16); display.print((int)(1000*accelBias[0])); - display.setCursor(24, 16); display.print((int)(1000*accelBias[1])); - display.setCursor(48, 16); display.print((int)(1000*accelBias[2])); + display.setCursor(0, 16); display.print((int)(1000*accelBias[0])); + display.setCursor(24, 16); display.print((int)(1000*accelBias[1])); + display.setCursor(48, 16); display.print((int)(1000*accelBias[2])); display.setCursor(72, 16); display.print("mg"); - - display.setCursor(0, 24); display.print(gyroBias[0], 1); - display.setCursor(24, 24); display.print(gyroBias[1], 1); - display.setCursor(48, 24); display.print(gyroBias[2], 1); - display.setCursor(66, 24); display.print("o/s"); - + + display.setCursor(0, 24); display.print(myIMU.gyroBias[0], 1); + display.setCursor(24, 24); display.print(myIMU.gyroBias[1], 1); + display.setCursor(48, 24); display.print(myIMU.gyroBias[2], 1); + display.setCursor(66, 24); display.print("o/s"); + display.display(); + delay(1000); #endif // LCD - delay(1000); - - initMPU9250(); - Serial.println("MPU9250 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature - - // Read the WHO_AM_I register of the magnetometer, this is a good test of communication - byte d = readByte(AK8963_ADDRESS, WHO_AM_I_AK8963); // Read WHO_AM_I register for AK8963 - Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" I should be "); Serial.println(0x48, HEX); + + myIMU.initMPU9250(); + // Initialize device for active mode read of acclerometer, gyroscope, and + // temperature + Serial.println("MPU9250 initialized for active data mode...."); + + // Read the WHO_AM_I register of the magnetometer, this is a good test of + // communication + byte d = myIMU.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963); + Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX); + Serial.print(" I should be "); Serial.println(0x48, HEX); + #ifdef LCD display.clearDisplay(); display.setCursor(20,0); display.print("AK8963"); display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(d, HEX); + display.setCursor(0,20); display.print(d, HEX); display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0x48, HEX); + display.setCursor(0,40); display.print(0x48, HEX); display.display(); + delay(1000); #endif // LCD - delay(1000); - + // Get magnetometer calibration from AK8963 ROM - initAK8963(magCalibration); Serial.println("AK8963 initialized for active data mode...."); // Initialize device for active mode read of magnetometer - - if(SerialDebug) { - // Serial.println("Calibration values: "); - Serial.print("X-Axis sensitivity adjustment value "); Serial.println(magCalibration[0], 2); - Serial.print("Y-Axis sensitivity adjustment value "); Serial.println(magCalibration[1], 2); - Serial.print("Z-Axis sensitivity adjustment value "); Serial.println(magCalibration[2], 2); - } + myIMU.initAK8963(myIMU.magCalibration); + // Initialize device for active mode read of magnetometer + Serial.println("AK8963 initialized for active data mode...."); + if (SerialDebug) + { + // Serial.println("Calibration values: "); + Serial.print("X-Axis sensitivity adjustment value "); + Serial.println(myIMU.magCalibration[0], 2); + Serial.print("Y-Axis sensitivity adjustment value "); + Serial.println(myIMU.magCalibration[1], 2); + Serial.print("Z-Axis sensitivity adjustment value "); + Serial.println(myIMU.magCalibration[2], 2); + } #ifdef LCD display.clearDisplay(); display.setCursor(20,0); display.print("AK8963"); - display.setCursor(0,10); display.print("ASAX "); display.setCursor(50,10); display.print(magCalibration[0], 2); - display.setCursor(0,20); display.print("ASAY "); display.setCursor(50,20); display.print(magCalibration[1], 2); - display.setCursor(0,30); display.print("ASAZ "); display.setCursor(50,30); display.print(magCalibration[2], 2); + display.setCursor(0,10); display.print("ASAX "); display.setCursor(50,10); + display.print(myIMU.magCalibration[0], 2); + display.setCursor(0,20); display.print("ASAY "); display.setCursor(50,20); + display.print(myIMU.magCalibration[1], 2); + display.setCursor(0,30); display.print("ASAZ "); display.setCursor(50,30); + display.print(myIMU.magCalibration[2], 2); display.display(); + delay(1000); #endif // LCD - delay(1000); - } + } // if (c == 0x71) else { Serial.print("Could not connect to MPU9250: 0x"); @@ -153,45 +195,57 @@ void setup() } void loop() -{ +{ // If intPin goes high, all data registers have new data - if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt - readAccelData(accelCount); // Read the x/y/z adc values - getAres(); - + // On interrupt, check if data ready interrupt + if (myIMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) + { + myIMU.readAccelData(myIMU.accelCount); // Read the x/y/z adc values + myIMU.getAres(); + // Now we'll calculate the accleration value into actual g's - ax = (float)accelCount[0]*aRes; // - accelBias[0]; // get actual g value, this depends on scale being set - ay = (float)accelCount[1]*aRes; // - accelBias[1]; - az = (float)accelCount[2]*aRes; // - accelBias[2]; - - readGyroData(gyroCount); // Read the x/y/z adc values - getGres(); - + // This depends on scale being set + myIMU.ax = (float)myIMU.accelCount[0]*myIMU.aRes; // - accelBias[0]; + myIMU.ay = (float)myIMU.accelCount[1]*myIMU.aRes; // - accelBias[1]; + myIMU.az = (float)myIMU.accelCount[2]*myIMU.aRes; // - accelBias[2]; + + myIMU.readGyroData(myIMU.gyroCount); // Read the x/y/z adc values + myIMU.getGres(); + // Calculate the gyro value into actual degrees per second - gx = (float)gyroCount[0]*gRes; // get actual gyro value, this depends on scale being set - gy = (float)gyroCount[1]*gRes; - gz = (float)gyroCount[2]*gRes; - - readMagData(magCount); // Read the x/y/z adc values - getMres(); - magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated - magbias[1] = +120.; // User environmental x-axis correction in milliGauss - magbias[2] = +125.; // User environmental x-axis correction in milliGauss - + // This depends on scale being set + myIMU.gx = (float)myIMU.gyroCount[0]*myIMU.gRes; + myIMU.gy = (float)myIMU.gyroCount[1]*myIMU.gRes; + myIMU.gz = (float)myIMU.gyroCount[2]*myIMU.gRes; + + myIMU.readMagData(myIMU.magCount); // Read the x/y/z adc values + myIMU.getMres(); + // User environmental x-axis correction in milliGauss, should be + // automatically calculated + myIMU.magbias[0] = +470.; + myIMU.magbias[1] = +120.; // User environmental x-axis correction in milliGauss + myIMU.magbias[2] = +125.; // User environmental x-axis correction in milliGauss + // Calculate the magnetometer values in milliGauss - // Include factory calibration per data sheet and user environmental corrections - mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set - my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]; - mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; - } - - Now = micros(); - deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update - lastUpdate = Now; - - sum += deltat; // sum for averaging filter update rate - sumCount++; - + // Include factory calibration per data sheet and user environmental + // corrections + // Get actual magnetometer value, this depends on scale being set + myIMU.mx = (float)myIMU.magCount[0]*myIMU.mRes*myIMU.magCalibration[0] - + myIMU.magbias[0]; + myIMU.my = (float)myIMU.magCount[1]*myIMU.mRes*myIMU.magCalibration[1] - + myIMU.magbias[1]; + myIMU.mz = (float)myIMU.magCount[2]*myIMU.mRes*myIMU.magCalibration[2] - + myIMU.magbias[2]; + } // if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) + + myIMU.Now = micros(); + // Set integration time by time elapsed since last filter update + deltat = ((myIMU.Now - myIMU.lastUpdate)/1000000.0f); + myIMU.lastUpdate = myIMU.Now; + + myIMU.sum += deltat; // sum for averaging filter update rate + myIMU.sumCount++; + // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of // the magnetometer; the magnetometer z-axis (+ down) is opposite to z-axis // (+ up) of accelerometer and gyro! We have to make some allowance for this @@ -201,87 +255,109 @@ void loop() // modified to allow any convenient orientation convention. This is ok by // aircraft orientation standards! Pass gyro rate as rad/s // MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); - MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); - - - if (!AHRS) { - delt_t = millis() - count; - if(delt_t > 500) { - - if(SerialDebug) { - // Print acceleration values in milligs! - Serial.print("X-acceleration: "); Serial.print(1000*ax); Serial.print(" mg "); - Serial.print("Y-acceleration: "); Serial.print(1000*ay); Serial.print(" mg "); - Serial.print("Z-acceleration: "); Serial.print(1000*az); Serial.println(" mg "); - - // Print gyro values in degree/sec - Serial.print("X-gyro rate: "); Serial.print(gx, 3); Serial.print(" degrees/sec "); - Serial.print("Y-gyro rate: "); Serial.print(gy, 3); Serial.print(" degrees/sec "); - Serial.print("Z-gyro rate: "); Serial.print(gz, 3); Serial.println(" degrees/sec"); - - // Print mag values in degree/sec - Serial.print("X-mag field: "); Serial.print(mx); Serial.print(" mG "); - Serial.print("Y-mag field: "); Serial.print(my); Serial.print(" mG "); - Serial.print("Z-mag field: "); Serial.print(mz); Serial.println(" mG"); - - tempCount = readTempData(); // Read the adc values - temperature = ((float) tempCount) / 333.87 + 21.0; // Temperature in degrees Centigrade - // Print temperature in degrees Centigrade - Serial.print("Temperature is "); Serial.print(temperature, 1); Serial.println(" degrees C"); // Print T values to tenths of s degree C - } - + MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx*PI/180.0f, + myIMU.gy*PI/180.0f, myIMU.gz*PI/180.0f, myIMU.my, + myIMU.mx, myIMU.mz, q); + + + if (!AHRS) + { + myIMU.delt_t = millis() - myIMU.count; + if (myIMU.delt_t > 500) + { + if(SerialDebug) + { + // Print acceleration values in milligs! + Serial.print("X-acceleration: "); Serial.print(1000*myIMU.ax); + Serial.print(" mg "); + Serial.print("Y-acceleration: "); Serial.print(1000*myIMU.ay); + Serial.print(" mg "); + Serial.print("Z-acceleration: "); Serial.print(1000*myIMU.az); + Serial.println(" mg "); + + // Print gyro values in degree/sec + Serial.print("X-gyro rate: "); Serial.print(myIMU.gx, 3); + Serial.print(" degrees/sec "); + Serial.print("Y-gyro rate: "); Serial.print(myIMU.gy, 3); + Serial.print(" degrees/sec "); + Serial.print("Z-gyro rate: "); Serial.print(myIMU.gz, 3); + Serial.println(" degrees/sec"); + + // Print mag values in degree/sec + Serial.print("X-mag field: "); Serial.print(myIMU.mx); Serial.print(" mG "); + Serial.print("Y-mag field: "); Serial.print(myIMU.my); Serial.print(" mG "); + Serial.print("Z-mag field: "); Serial.print(myIMU.mz); + Serial.println(" mG"); + + myIMU.tempCount = myIMU.readTempData(); // Read the adc values + // Temperature in degrees Centigrade + myIMU.temperature = ((float) myIMU.tempCount) / 333.87 + 21.0; + // Print temperature in degrees Centigrade + Serial.print("Temperature is "); Serial.print(myIMU.temperature, 1); + Serial.println(" degrees C"); + } + #ifdef LCD - display.clearDisplay(); - display.setCursor(0, 0); display.print("MPU9250/AK8963"); - display.setCursor(0, 8); display.print(" x y z "); + display.clearDisplay(); + display.setCursor(0, 0); display.print("MPU9250/AK8963"); + display.setCursor(0, 8); display.print(" x y z "); - display.setCursor(0, 16); display.print((int)(1000*ax)); - display.setCursor(24, 16); display.print((int)(1000*ay)); - display.setCursor(48, 16); display.print((int)(1000*az)); - display.setCursor(72, 16); display.print("mg"); - - display.setCursor(0, 24); display.print((int)(gx)); - display.setCursor(24, 24); display.print((int)(gy)); - display.setCursor(48, 24); display.print((int)(gz)); - display.setCursor(66, 24); display.print("o/s"); - - display.setCursor(0, 32); display.print((int)(mx)); - display.setCursor(24, 32); display.print((int)(my)); - display.setCursor(48, 32); display.print((int)(mz)); - display.setCursor(72, 32); display.print("mG"); - - display.setCursor(0, 40); display.print("Gyro T "); - display.setCursor(50, 40); display.print(temperature, 1); display.print(" C"); - display.display(); + display.setCursor(0, 16); display.print((int)(1000*myIMU.ax)); + display.setCursor(24, 16); display.print((int)(1000*myIMU.ay)); + display.setCursor(48, 16); display.print((int)(1000*myIMU.az)); + display.setCursor(72, 16); display.print("mg"); + + display.setCursor(0, 24); display.print((int)(myIMU.gx)); + display.setCursor(24, 24); display.print((int)(myIMU.gy)); + display.setCursor(48, 24); display.print((int)(myIMU.gz)); + display.setCursor(66, 24); display.print("o/s"); + + display.setCursor(0, 32); display.print((int)(myIMU.mx)); + display.setCursor(24, 32); display.print((int)(myIMU.my)); + display.setCursor(48, 32); display.print((int)(myIMU.mz)); + display.setCursor(72, 32); display.print("mG"); + + display.setCursor(0, 40); display.print("Gyro T "); + display.setCursor(50, 40); display.print(myIMU.temperature, 1); + display.print(" C"); + display.display(); #endif // LCD - - count = millis(); - digitalWrite(myLed, !digitalRead(myLed)); // toggle led - } - } - else { - + + myIMU.count = millis(); + digitalWrite(myLed, !digitalRead(myLed)); // toggle led + } // if (myIMU.delt_t > 500) + } // if (!AHRS) + else + { // Serial print and/or display at 0.5 s rate independent of data rates - delt_t = millis() - count; - if (delt_t > 500) { // update LCD once per half-second independent of read rate - - if(SerialDebug) { - Serial.print("ax = "); Serial.print((int)1000*ax); - Serial.print(" ay = "); Serial.print((int)1000*ay); - Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg"); - Serial.print("gx = "); Serial.print( gx, 2); - Serial.print(" gy = "); Serial.print( gy, 2); - Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s"); - Serial.print("mx = "); Serial.print( (int)mx ); - Serial.print(" my = "); Serial.print( (int)my ); - Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG"); - - Serial.print("q0 = "); Serial.print(q[0]); - Serial.print(" qx = "); Serial.print(q[1]); - Serial.print(" qy = "); Serial.print(q[2]); - Serial.print(" qz = "); Serial.println(q[3]); - } - + myIMU.delt_t = millis() - myIMU.count; + + // update LCD once per half-second independent of read rate + if (myIMU.delt_t > 500) + { + if(SerialDebug) + { + Serial.print("ax = "); Serial.print((int)1000*myIMU.ax); + Serial.print(" ay = "); Serial.print((int)1000*myIMU.ay); + Serial.print(" az = "); Serial.print((int)1000*myIMU.az); + Serial.println(" mg"); + + Serial.print("gx = "); Serial.print( myIMU.gx, 2); + Serial.print(" gy = "); Serial.print( myIMU.gy, 2); + Serial.print(" gz = "); Serial.print( myIMU.gz, 2); + Serial.println(" deg/s"); + + Serial.print("mx = "); Serial.print( (int)myIMU.mx ); + Serial.print(" my = "); Serial.print( (int)myIMU.my ); + Serial.print(" mz = "); Serial.print( (int)myIMU.mz ); + Serial.println(" mG"); + + Serial.print("q0 = "); Serial.print(q[0]); + Serial.print(" qx = "); Serial.print(q[1]); + Serial.print(" qy = "); Serial.print(q[2]); + Serial.print(" qz = "); Serial.println(q[3]); + } + // Define output variables from updated quaternion---these are Tait-Bryan // angles, commonly used in aircraft orientation. In this coordinate system, // the positive z-axis is down toward Earth. Yaw is the angle between Sensor @@ -296,52 +372,56 @@ void loop() // must be applied in the correct order which for this configuration is yaw, // pitch, and then roll. // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. - 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]); - pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); - 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]); - pitch *= 180.0f / PI; - yaw *= 180.0f / PI; + myIMU.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]); + myIMU.pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); + myIMU.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]); + myIMU.pitch *= 180.0f / PI; + myIMU.yaw *= 180.0f / PI; // TODO: Declination at Danville, California is 13 degrees 48 minutes and 47 // seconds on 2014-04-04 - yaw -= 13.8; - roll *= 180.0f / PI; - - if(SerialDebug) { - Serial.print("Yaw, Pitch, Roll: "); - Serial.print(yaw, 2); - Serial.print(", "); - Serial.print(pitch, 2); - Serial.print(", "); - Serial.println(roll, 2); - - Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz"); - } - + myIMU.yaw -= 13.8; + myIMU.roll *= 180.0f / PI; + + if(SerialDebug) + { + Serial.print("Yaw, Pitch, Roll: "); + Serial.print(myIMU.yaw, 2); + Serial.print(", "); + Serial.print(myIMU.pitch, 2); + Serial.print(", "); + Serial.println(myIMU.roll, 2); + + Serial.print("rate = "); Serial.print((float)myIMU.sumCount/myIMU.sum, 2); + Serial.println(" Hz"); + } + #ifdef LCD - display.clearDisplay(); - - display.setCursor(0, 0); display.print(" x y z "); - - display.setCursor(0, 8); display.print((int)(1000*ax)); - display.setCursor(24, 8); display.print((int)(1000*ay)); - display.setCursor(48, 8); display.print((int)(1000*az)); - display.setCursor(72, 8); display.print("mg"); - - display.setCursor(0, 16); display.print((int)(gx)); - display.setCursor(24, 16); display.print((int)(gy)); - display.setCursor(48, 16); display.print((int)(gz)); - display.setCursor(66, 16); display.print("o/s"); - - display.setCursor(0, 24); display.print((int)(mx)); - display.setCursor(24, 24); display.print((int)(my)); - display.setCursor(48, 24); display.print((int)(mz)); - display.setCursor(72, 24); display.print("mG"); - - display.setCursor(0, 32); display.print((int)(yaw)); - display.setCursor(24, 32); display.print((int)(pitch)); - display.setCursor(48, 32); display.print((int)(roll)); - display.setCursor(66, 32); display.print("ypr"); - + display.clearDisplay(); + + display.setCursor(0, 0); display.print(" x y z "); + + display.setCursor(0, 8); display.print((int)(1000*myIMU.ax)); + display.setCursor(24, 8); display.print((int)(1000*myIMU.ay)); + display.setCursor(48, 8); display.print((int)(1000*myIMU.az)); + display.setCursor(72, 8); display.print("mg"); + + display.setCursor(0, 16); display.print((int)(myIMU.gx)); + display.setCursor(24, 16); display.print((int)(myIMU.gy)); + display.setCursor(48, 16); display.print((int)(myIMU.gz)); + display.setCursor(66, 16); display.print("o/s"); + + display.setCursor(0, 24); display.print((int)(myIMU.mx)); + display.setCursor(24, 24); display.print((int)(myIMU.my)); + display.setCursor(48, 24); display.print((int)(myIMU.mz)); + display.setCursor(72, 24); display.print("mG"); + + display.setCursor(0, 32); display.print((int)(myIMU.yaw)); + display.setCursor(24, 32); display.print((int)(myIMU.pitch)); + display.setCursor(48, 32); display.print((int)(myIMU.roll)); + display.setCursor(66, 32); display.print("ypr"); + // With these settings the filter is updating at a ~145 Hz rate using the // Madgwick scheme and >200 Hz using the Mahony scheme even though the // display refreshes at only 2 Hz. The filter update rate is determined @@ -357,15 +437,14 @@ void loop() // produced by the on-board Digital Motion Processor of Invensense's MPU6050 // 6 DoF and MPU9150 9DoF sensors. The 3.3 V 8 MHz Pro Mini is doing pretty // well! - display.setCursor(0, 40); display.print("rt: "); - display.print((float) sumCount / sum, 2); display.print(" Hz"); - display.display(); + display.setCursor(0, 40); display.print("rt: "); + display.print((float) myIMU.sumCount / myIMU.sum, 2); display.print(" Hz"); + display.display(); #endif // LCD - count = millis(); - sumCount = 0; - sum = 0; - } - } - + myIMU.count = millis(); + myIMU.sumCount = 0; + myIMU.sum = 0; + } // if (myIMU.delt_t > 500) + } // if (AHRS) } diff --git a/src/MPU9250.cpp b/src/MPU9250.cpp index 6b3e71e..0c64584 100644 --- a/src/MPU9250.cpp +++ b/src/MPU9250.cpp @@ -5,7 +5,7 @@ //====== and temperature data //============================================================================== -void getMres() { +void MPU9250::getMres() { switch (Mscale) { // Possible magnetometer scales (and their register bit settings) are: @@ -19,7 +19,7 @@ void getMres() { } } -void getGres() { +void MPU9250::getGres() { switch (Gscale) { // Possible gyro scales (and their register bit settings) are: @@ -40,7 +40,7 @@ void getGres() { } } -void getAres() { +void MPU9250::getAres() { switch (Ascale) { // Possible accelerometer scales (and their register bit settings) are: @@ -62,7 +62,7 @@ void getAres() { } -void readAccelData(int16_t * destination) +void MPU9250::readAccelData(int16_t * destination) { uint8_t rawData[6]; // x/y/z accel register data stored here readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array @@ -72,7 +72,7 @@ void readAccelData(int16_t * destination) } -void readGyroData(int16_t * destination) +void MPU9250::readGyroData(int16_t * destination) { uint8_t rawData[6]; // x/y/z gyro register data stored here readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array @@ -81,7 +81,7 @@ void readGyroData(int16_t * destination) destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; } -void readMagData(int16_t * destination) +void MPU9250::readMagData(int16_t * destination) { // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of // data acquisition @@ -104,14 +104,14 @@ void readMagData(int16_t * destination) } } -int16_t readTempData() +int16_t MPU9250::readTempData() { uint8_t rawData[2]; // x/y/z gyro register data stored here readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array return ((int16_t)rawData[0] << 8) | rawData[1]; // Turn the MSB and LSB into a 16-bit value } -void initAK8963(float * destination) +void MPU9250::initAK8963(float * destination) { // First extract the factory calibration for each magnetometer axis uint8_t rawData[3]; // x/y/z gyro calibration data stored here @@ -133,7 +133,7 @@ void initAK8963(float * destination) } -void initMPU9250() +void MPU9250::initMPU9250() { // wake up device writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors @@ -192,25 +192,27 @@ void initMPU9250() } -// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average -// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. -void calibrateMPU9250(float * dest1, float * dest2) +// Function which accumulates gyro and accelerometer data after device +// initialization. It calculates the average of the at-rest readings and then +// loads the resulting offsets into accelerometer and gyro bias registers. +void MPU9250::calibrateMPU9250(float * gyroBias, float * accelBias) { uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data uint16_t ii, packet_count, fifo_count; int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; - // reset device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device + // reset device + // Write a one to bit 7 reset bit; toggle reset device + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); delay(100); - // get stable time source; Auto select clock source to be PLL gyroscope reference if ready - // else use the internal oscillator, bits 2:0 = 001 + // get stable time source; Auto select clock source to be PLL gyroscope + // reference if ready else use the internal oscillator, bits 2:0 = 001 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); delay(200); -// Configure device for bias calculation + // Configure device for bias calculation writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source @@ -284,9 +286,9 @@ void calibrateMPU9250(float * dest1, float * dest2) writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]); // Output scaled gyro biases for display in the main program - dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; - dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; - dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; + gyroBias[0] = (float) gyro_bias[0]/(float) gyrosensitivity; + gyroBias[1] = (float) gyro_bias[1]/(float) gyrosensitivity; + gyroBias[2] = (float) gyro_bias[2]/(float) gyrosensitivity; // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold @@ -335,14 +337,14 @@ void calibrateMPU9250(float * dest1, float * dest2) writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); // Output scaled accelerometer biases for display in the main program - dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; - dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; - dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; + accelBias[0] = (float)accel_bias[0]/(float)accelsensitivity; + accelBias[1] = (float)accel_bias[1]/(float)accelsensitivity; + accelBias[2] = (float)accel_bias[2]/(float)accelsensitivity; } // Accelerometer and gyroscope self test; check calibration wrt factory settings -void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass +void MPU9250::MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass { uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; uint8_t selfTest[6]; @@ -428,7 +430,7 @@ void MPU9250SelfTest(float * destination) // Should return percent deviation fro // Wire.h read and write protocols -void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) +void MPU9250::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) { Wire.beginTransmission(address); // Initialize the Tx buffer Wire.write(subAddress); // Put slave register address in Tx buffer @@ -436,7 +438,7 @@ void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) Wire.endTransmission(); // Send the Tx buffer } -uint8_t readByte(uint8_t address, uint8_t subAddress) +uint8_t MPU9250::readByte(uint8_t address, uint8_t subAddress) { uint8_t data; // `data` will store the register data Wire.beginTransmission(address); // Initialize the Tx buffer @@ -447,8 +449,8 @@ uint8_t readByte(uint8_t address, uint8_t subAddress) return data; // Return data read from slave register } -void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * - dest) +void MPU9250::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, + uint8_t * dest) { Wire.beginTransmission(address); // Initialize the Tx buffer Wire.write(subAddress); // Put slave register address in Tx buffer diff --git a/src/MPU9250.h b/src/MPU9250.h index fbb299c..aa65642 100644 --- a/src/MPU9250.h +++ b/src/MPU9250.h @@ -1,5 +1,5 @@ /* - Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library. + Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library. 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. We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file. We are also using the 400 kHz fast @@ -11,23 +11,6 @@ #include #include -#ifdef LCD -#include -#include -#endif // LCD - -//#include "quaternionFilters.h" - -#ifdef LCD -// Using NOKIA 5110 monochrome 84 x 48 pixel display -// pin 9 - Serial clock out (SCLK) -// pin 8 - Serial data out (DIN) -// pin 7 - Data/Command select (D/C) -// pin 5 - LCD chip select (CS) -// pin 6 - LCD reset (RST) -Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); -#endif // LCD - // 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 above // document; the MPU9250 and MPU9150 are virtually identical but the latter has @@ -38,12 +21,12 @@ Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); #define WHO_AM_I_AK8963 0x00 // should return 0x48 #define INFO 0x01 #define AK8963_ST1 0x02 // data ready status bit 0 -#define AK8963_XOUT_L 0x03 // data -#define AK8963_XOUT_H 0x04 -#define AK8963_YOUT_L 0x05 -#define AK8963_YOUT_H 0x06 -#define AK8963_ZOUT_L 0x07 -#define AK8963_ZOUT_H 0x08 +#define AK8963_XOUT_L 0x03 // data +#define AK8963_XOUT_H 0x04 +#define AK8963_YOUT_L 0x05 +#define AK8963_YOUT_H 0x06 +#define AK8963_ZOUT_L 0x07 +#define AK8963_ZOUT_H 0x08 #define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 #define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 #define AK8963_ASTC 0x0C // Self test control @@ -52,8 +35,8 @@ Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); #define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value #define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value -#define SELF_TEST_X_GYRO 0x00 -#define SELF_TEST_Y_GYRO 0x01 +#define SELF_TEST_X_GYRO 0x00 +#define SELF_TEST_Y_GYRO 0x01 #define SELF_TEST_Z_GYRO 0x02 /*#define X_FINE_GAIN 0x03 // [7:0] fine gain @@ -67,240 +50,212 @@ Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); #define ZA_OFFSET_L_TC 0x0B */ #define SELF_TEST_X_ACCEL 0x0D -#define SELF_TEST_Y_ACCEL 0x0E +#define SELF_TEST_Y_ACCEL 0x0E #define SELF_TEST_Z_ACCEL 0x0F -#define SELF_TEST_A 0x10 +#define SELF_TEST_A 0x10 -#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope -#define XG_OFFSET_L 0x14 -#define YG_OFFSET_H 0x15 -#define YG_OFFSET_L 0x16 -#define ZG_OFFSET_H 0x17 -#define ZG_OFFSET_L 0x18 -#define SMPLRT_DIV 0x19 -#define CONFIG 0x1A -#define GYRO_CONFIG 0x1B -#define ACCEL_CONFIG 0x1C -#define ACCEL_CONFIG2 0x1D -#define LP_ACCEL_ODR 0x1E -#define WOM_THR 0x1F +#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope +#define XG_OFFSET_L 0x14 +#define YG_OFFSET_H 0x15 +#define YG_OFFSET_L 0x16 +#define ZG_OFFSET_H 0x17 +#define ZG_OFFSET_L 0x18 +#define SMPLRT_DIV 0x19 +#define CONFIG 0x1A +#define GYRO_CONFIG 0x1B +#define ACCEL_CONFIG 0x1C +#define ACCEL_CONFIG2 0x1D +#define LP_ACCEL_ODR 0x1E +#define WOM_THR 0x1F -#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms -#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] -#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms +// Duration counter threshold for motion interrupt generation, 1 kHz rate, +// LSB = 1 ms +#define MOT_DUR 0x20 +// Zero-motion detection threshold bits [7:0] +#define ZMOT_THR 0x21 +// Duration counter threshold for zero motion interrupt generation, 16 Hz rate, +// LSB = 64 ms +#define ZRMOT_DUR 0x22 -#define FIFO_EN 0x23 -#define I2C_MST_CTRL 0x24 -#define I2C_SLV0_ADDR 0x25 -#define I2C_SLV0_REG 0x26 -#define I2C_SLV0_CTRL 0x27 -#define I2C_SLV1_ADDR 0x28 -#define I2C_SLV1_REG 0x29 -#define I2C_SLV1_CTRL 0x2A -#define I2C_SLV2_ADDR 0x2B -#define I2C_SLV2_REG 0x2C -#define I2C_SLV2_CTRL 0x2D -#define I2C_SLV3_ADDR 0x2E -#define I2C_SLV3_REG 0x2F -#define I2C_SLV3_CTRL 0x30 -#define I2C_SLV4_ADDR 0x31 -#define I2C_SLV4_REG 0x32 -#define I2C_SLV4_DO 0x33 -#define I2C_SLV4_CTRL 0x34 -#define I2C_SLV4_DI 0x35 -#define I2C_MST_STATUS 0x36 -#define INT_PIN_CFG 0x37 -#define INT_ENABLE 0x38 -#define DMP_INT_STATUS 0x39 // Check DMP interrupt -#define INT_STATUS 0x3A -#define ACCEL_XOUT_H 0x3B -#define ACCEL_XOUT_L 0x3C -#define ACCEL_YOUT_H 0x3D -#define ACCEL_YOUT_L 0x3E -#define ACCEL_ZOUT_H 0x3F -#define ACCEL_ZOUT_L 0x40 -#define TEMP_OUT_H 0x41 -#define TEMP_OUT_L 0x42 -#define GYRO_XOUT_H 0x43 -#define GYRO_XOUT_L 0x44 -#define GYRO_YOUT_H 0x45 -#define GYRO_YOUT_L 0x46 -#define GYRO_ZOUT_H 0x47 -#define GYRO_ZOUT_L 0x48 -#define EXT_SENS_DATA_00 0x49 -#define EXT_SENS_DATA_01 0x4A -#define EXT_SENS_DATA_02 0x4B -#define EXT_SENS_DATA_03 0x4C -#define EXT_SENS_DATA_04 0x4D -#define EXT_SENS_DATA_05 0x4E -#define EXT_SENS_DATA_06 0x4F -#define EXT_SENS_DATA_07 0x50 -#define EXT_SENS_DATA_08 0x51 -#define EXT_SENS_DATA_09 0x52 -#define EXT_SENS_DATA_10 0x53 -#define EXT_SENS_DATA_11 0x54 -#define EXT_SENS_DATA_12 0x55 -#define EXT_SENS_DATA_13 0x56 -#define EXT_SENS_DATA_14 0x57 -#define EXT_SENS_DATA_15 0x58 -#define EXT_SENS_DATA_16 0x59 -#define EXT_SENS_DATA_17 0x5A -#define EXT_SENS_DATA_18 0x5B -#define EXT_SENS_DATA_19 0x5C -#define EXT_SENS_DATA_20 0x5D -#define EXT_SENS_DATA_21 0x5E -#define EXT_SENS_DATA_22 0x5F -#define EXT_SENS_DATA_23 0x60 -#define MOT_DETECT_STATUS 0x61 -#define I2C_SLV0_DO 0x63 -#define I2C_SLV1_DO 0x64 -#define I2C_SLV2_DO 0x65 -#define I2C_SLV3_DO 0x66 +#define FIFO_EN 0x23 +#define I2C_MST_CTRL 0x24 +#define I2C_SLV0_ADDR 0x25 +#define I2C_SLV0_REG 0x26 +#define I2C_SLV0_CTRL 0x27 +#define I2C_SLV1_ADDR 0x28 +#define I2C_SLV1_REG 0x29 +#define I2C_SLV1_CTRL 0x2A +#define I2C_SLV2_ADDR 0x2B +#define I2C_SLV2_REG 0x2C +#define I2C_SLV2_CTRL 0x2D +#define I2C_SLV3_ADDR 0x2E +#define I2C_SLV3_REG 0x2F +#define I2C_SLV3_CTRL 0x30 +#define I2C_SLV4_ADDR 0x31 +#define I2C_SLV4_REG 0x32 +#define I2C_SLV4_DO 0x33 +#define I2C_SLV4_CTRL 0x34 +#define I2C_SLV4_DI 0x35 +#define I2C_MST_STATUS 0x36 +#define INT_PIN_CFG 0x37 +#define INT_ENABLE 0x38 +#define DMP_INT_STATUS 0x39 // Check DMP interrupt +#define INT_STATUS 0x3A +#define ACCEL_XOUT_H 0x3B +#define ACCEL_XOUT_L 0x3C +#define ACCEL_YOUT_H 0x3D +#define ACCEL_YOUT_L 0x3E +#define ACCEL_ZOUT_H 0x3F +#define ACCEL_ZOUT_L 0x40 +#define TEMP_OUT_H 0x41 +#define TEMP_OUT_L 0x42 +#define GYRO_XOUT_H 0x43 +#define GYRO_XOUT_L 0x44 +#define GYRO_YOUT_H 0x45 +#define GYRO_YOUT_L 0x46 +#define GYRO_ZOUT_H 0x47 +#define GYRO_ZOUT_L 0x48 +#define EXT_SENS_DATA_00 0x49 +#define EXT_SENS_DATA_01 0x4A +#define EXT_SENS_DATA_02 0x4B +#define EXT_SENS_DATA_03 0x4C +#define EXT_SENS_DATA_04 0x4D +#define EXT_SENS_DATA_05 0x4E +#define EXT_SENS_DATA_06 0x4F +#define EXT_SENS_DATA_07 0x50 +#define EXT_SENS_DATA_08 0x51 +#define EXT_SENS_DATA_09 0x52 +#define EXT_SENS_DATA_10 0x53 +#define EXT_SENS_DATA_11 0x54 +#define EXT_SENS_DATA_12 0x55 +#define EXT_SENS_DATA_13 0x56 +#define EXT_SENS_DATA_14 0x57 +#define EXT_SENS_DATA_15 0x58 +#define EXT_SENS_DATA_16 0x59 +#define EXT_SENS_DATA_17 0x5A +#define EXT_SENS_DATA_18 0x5B +#define EXT_SENS_DATA_19 0x5C +#define EXT_SENS_DATA_20 0x5D +#define EXT_SENS_DATA_21 0x5E +#define EXT_SENS_DATA_22 0x5F +#define EXT_SENS_DATA_23 0x60 +#define MOT_DETECT_STATUS 0x61 +#define I2C_SLV0_DO 0x63 +#define I2C_SLV1_DO 0x64 +#define I2C_SLV2_DO 0x65 +#define I2C_SLV3_DO 0x66 #define I2C_MST_DELAY_CTRL 0x67 #define SIGNAL_PATH_RESET 0x68 -#define MOT_DETECT_CTRL 0x69 -#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP -#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode -#define PWR_MGMT_2 0x6C -#define DMP_BANK 0x6D // Activates a specific bank in the DMP -#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank -#define DMP_REG 0x6F // Register in DMP from which to read or to which to write -#define DMP_REG_1 0x70 -#define DMP_REG_2 0x71 -#define FIFO_COUNTH 0x72 -#define FIFO_COUNTL 0x73 -#define FIFO_R_W 0x74 -#define WHO_AM_I_MPU9250 0x75 // Should return 0x71 -#define XA_OFFSET_H 0x77 -#define XA_OFFSET_L 0x78 -#define YA_OFFSET_H 0x7A -#define YA_OFFSET_L 0x7B -#define ZA_OFFSET_H 0x7D -#define ZA_OFFSET_L 0x7E +#define MOT_DETECT_CTRL 0x69 +#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP +#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode +#define PWR_MGMT_2 0x6C +#define DMP_BANK 0x6D // Activates a specific bank in the DMP +#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank +#define DMP_REG 0x6F // Register in DMP from which to read or to which to write +#define DMP_REG_1 0x70 +#define DMP_REG_2 0x71 +#define FIFO_COUNTH 0x72 +#define FIFO_COUNTL 0x73 +#define FIFO_R_W 0x74 +#define WHO_AM_I_MPU9250 0x75 // Should return 0x71 +#define XA_OFFSET_H 0x77 +#define XA_OFFSET_L 0x78 +#define YA_OFFSET_H 0x7A +#define YA_OFFSET_L 0x7B +#define ZA_OFFSET_H 0x7D +#define ZA_OFFSET_L 0x7E -// Using the MPU-9250 breakout board, ADO is set to 0 +// Using the MPU-9250 breakout board, ADO is set to 0 // Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 #define ADO 0 #if ADO #define MPU9250_ADDRESS 0x69 // Device address when ADO = 1 #else #define MPU9250_ADDRESS 0x68 // Device address when ADO = 0 -#define AK8963_ADDRESS 0x0C // Address of magnetometer -#endif // AD0 +#define AK8963_ADDRESS 0x0C // Address of magnetometer +#endif // AD0 #define AHRS true // Set to false for basic data read #define SerialDebug true // Set to true to get Serial output for debugging -// Set initial input parameters -enum Ascale { - AFS_2G = 0, - AFS_4G, - AFS_8G, - AFS_16G -}; - -enum Gscale { - GFS_250DPS = 0, - GFS_500DPS, - GFS_1000DPS, - GFS_2000DPS -}; +class MPU9250 +{ + protected: + // Set initial input parameters + enum Ascale { + AFS_2G = 0, + AFS_4G, + AFS_8G, + AFS_16G + }; -enum Mscale { - MFS_14BITS = 0, // 0.6 mG per LSB - MFS_16BITS // 0.15 mG per LSB -}; + enum Gscale { + GFS_250DPS = 0, + GFS_500DPS, + GFS_1000DPS, + GFS_2000DPS + }; -// Specify sensor full scale -uint8_t Gscale = GFS_250DPS; -uint8_t Ascale = AFS_2G; -// Choose either 14-bit or 16-bit magnetometer resolution -uint8_t Mscale = MFS_16BITS; -// 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read -uint8_t Mmode = 0x02; -// Scale resolutions per LSB for the sensors -float aRes, gRes, mRes; - -// Pin definitions -int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins -int myLed = 13; // Set up pin 13 led for toggling + enum Mscale { + MFS_14BITS = 0, // 0.6 mG per LSB + MFS_16BITS // 0.15 mG per LSB + }; -int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output -int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output -int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output -// Factory mag calibration and mag bias -float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0}; -// Bias corrections for gyro and accelerometer -float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; -int16_t tempCount; // Temperature raw count output -float temperature; // Stores the real internal chip temperature in Celsius -float SelfTest[6]; // Holds results of gyro and accelerometer self test + // Specify sensor full scale + uint8_t Gscale = GFS_250DPS; + uint8_t Ascale = AFS_2G; + // Choose either 14-bit or 16-bit magnetometer resolution + uint8_t Mscale = MFS_16BITS; + // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read + uint8_t Mmode = 0x02; -// Global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference -// System) -// gyroscope measurement error in rads/s (start at 40 deg/s) -float GyroMeasError = PI * (40.0f / 180.0f); -// gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) -float GyroMeasDrift = PI * (0.0f / 180.0f); -// There is a tradeoff in the beta parameter between accuracy and response -// speed. In the original Madgwick study, beta of 0.041 (corresponding to -// GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. However, -// with this value, the LSM9SD0 response time is about 10 seconds to a stable -// initial quaternion. Subsequent changes also require a longish lag time to a -// stable output, not fast enough for a quadcopter or robot car! By increasing -// beta (GyroMeasError) by about a factor of fifteen, the response time constant -// is reduced to ~2 sec -// I haven't noticed any reduction in solution accuracy. This is essentially the -// I coefficient in a PID control sense; the bigger the feedback coefficient, -// the faster the solution converges, usually at the expense of accuracy. In any -// case, this is the free parameter in the Madgwick filtering and fusion scheme. -float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // Compute beta -// Compute zeta, the other free parameter in the Madgwick scheme usually set to -// a small or zero value -float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; -// These are the free parameters in the Mahony filter and fusion scheme, Kp for -// proportional feedback, Ki for integral -#define Kp 2.0f * 5.0f -#define Ki 0.0f + public: + float pitch, yaw, roll; + float temperature; // Stores the real internal chip temperature in Celsius + int16_t tempCount; // Temperature raw count output + uint32_t delt_t = 0; // Used to control display output rate + uint32_t count = 0, sumCount = 0; // Used to control display output rate + // Used to calculate integration interval + uint32_t lastUpdate = 0, firstUpdate = 0; + // Integration interval for both filter schemes + float sum = 0.0f; + uint32_t Now = 0; // Used to calculate integration interval + int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output + int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output + // Scale resolutions per LSB for the sensors + float aRes, gRes, mRes; + // Variables to hold latest sensor data values + float ax, ay, az, gx, gy, gz, mx, my, mz; + // Factory mag calibration and mag bias + float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0}; + // Bias corrections for gyro and accelerometer + float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; + float SelfTest[6]; + // Stores the 16-bit signed accelerometer sensor output + int16_t accelCount[3]; + // Constants for 9 DoF fusion and AHRS (Attitude and Heading + // Reference System) + // gyroscope measurement error in rads/s (start at 40 deg/s) + + protected: -uint32_t delt_t = 0; // Used to control display output rate -uint32_t count = 0, sumCount = 0; // Used to control display output rate -float pitch, yaw, roll; -float deltat = 0.0f, sum = 0.0f; // Integration interval for both filter schemes -// Used to calculate integration interval -uint32_t lastUpdate = 0, firstUpdate = 0; -uint32_t Now = 0; // Used to calculate integration interval - -// Variables to hold latest sensor data values -float ax, ay, az, gx, gy, gz, mx, my, mz; -// Vector to hold quaternion -float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; -// Vector to hold integral error for Mahony method -float eInt[3] = {0.0f, 0.0f, 0.0f}; - -// Forward declarations of below functions -void MPU9250SelfTest(float *); -void calibrateMPU9250(float *, float *); -void getMres(); -void getGres(); -void getAres(); -void readAccelData(int16_t *); -void readGyroData(int16_t *); -void readMagData(int16_t *); -int16_t readTempData(); -void initAK8963(float *); -void initMPU9250(); -void calibrateMPU9250(float *, float *); -void MPU9250SelfTest(float *); -void writeByte(uint8_t, uint8_t, uint8_t); -uint8_t readByte(uint8_t, uint8_t); -void readBytes(uint8_t, uint8_t, uint8_t, uint8_t *); - -/* -uint8_t readByte(uint8_t address, uint8_t subAddress); -void MPU9250SelfTest(float * destination); -void calibrateMPU9250(float * dest1, float * dest2); -*/ + public: + void getMres(); + void getGres(); + void getAres(); + void readAccelData(int16_t *); + void readGyroData(int16_t *); + void readMagData(int16_t *); + int16_t readTempData(); + void initAK8963(float *); + void initMPU9250(); + void calibrateMPU9250(float * gyroBias, float * accelBias); + void MPU9250SelfTest(float * destination); + void writeByte(uint8_t, uint8_t, uint8_t); + uint8_t readByte(uint8_t, uint8_t); + void readBytes(uint8_t, uint8_t, uint8_t, uint8_t *); +}; // class MPU9250 #endif // _MPU9250_H_ diff --git a/src/quaternionFilters.cpp b/src/quaternionFilters.cpp index d07d2f1..eef5dbd 100644 --- a/src/quaternionFilters.cpp +++ b/src/quaternionFilters.cpp @@ -12,9 +12,9 @@ //#ifndef _QUATERNIONfILTERS_H_ //#define _QUATERNIONfILTERS_H_ -#include "MPU9250.h" +#include "quaternionFilters.h" -void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) +void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float * q) { // short name local variable for readability float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; @@ -106,12 +106,12 @@ void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, q[2] = q3 * norm; q[3] = q4 * norm; } - - - + + + // Similar to Madgwick scheme but uses proportional and integral filtering on -// the error between estimated reference vectors and measured ones. -void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) +// the error between estimated reference vectors and measured ones. +void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float * q) { // short name local variable for readability float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; @@ -131,20 +131,20 @@ void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, fl float q2q4 = q2 * q4; float q3q3 = q3 * q3; float q3q4 = q3 * q4; - float q4q4 = q4 * q4; + float q4q4 = q4 * q4; // Normalise accelerometer measurement norm = sqrt(ax * ax + ay * ay + az * az); - if (norm == 0.0f) return; // handle NaN - norm = 1.0f / norm; // use reciprocal for division + if (norm == 0.0f) return; // Handle NaN + norm = 1.0f / norm; // Use reciprocal for division ax *= norm; ay *= norm; az *= norm; // Normalise magnetometer measurement norm = sqrt(mx * mx + my * my + mz * mz); - if (norm == 0.0f) return; // handle NaN - norm = 1.0f / norm; // use reciprocal for division + if (norm == 0.0f) return; // Handle NaN + norm = 1.0f / norm; // Use reciprocal for division mx *= norm; my *= norm; mz *= norm; @@ -161,7 +161,7 @@ void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, fl vz = q1q1 - q2q2 - q3q3 + q4q4; wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3); wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4); - wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3); + wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3); // Error is cross product between estimated direction and measured direction of gravity ex = (ay * vz - az * vy) + (my * wz - mz * wy); diff --git a/src/quaternionFilters.h b/src/quaternionFilters.h index 1f5ef43..68a1422 100644 --- a/src/quaternionFilters.h +++ b/src/quaternionFilters.h @@ -1,9 +1,43 @@ #ifndef _QUATERNIONFILTERS_H_ #define _QUATERNIONFILTERS_H_ -void MadgwickQuaternionUpdate(float, float, float, float, float, float, float, - float, float); -void MahonyQuaternionUpdate(float, float, float, float, float, float, float, - float, float); +#include + +// These are the free parameters in the Mahony filter and fusion scheme, Kp +// for proportional feedback, Ki for integral +#define Kp 2.0f * 5.0f +#define Ki 0.0f + +// Vector to hold integral error for Mahony method +static float eInt[3] = {0.0f, 0.0f, 0.0f}; +// Vector to hold quaternion +static float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; + +static float GyroMeasError = PI * (40.0f / 180.0f); +// gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) +static float GyroMeasDrift = PI * (0.0f / 180.0f); +// There is a tradeoff in the beta parameter between accuracy and response +// speed. In the original Madgwick study, beta of 0.041 (corresponding to +// GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. +// However, with this value, the LSM9SD0 response time is about 10 seconds +// to a stable initial quaternion. Subsequent changes also require a +// longish lag time to a stable output, not fast enough for a quadcopter or +// robot car! By increasing beta (GyroMeasError) by about a factor of +// fifteen, the response time constant is reduced to ~2 sec. I haven't +// noticed any reduction in solution accuracy. This is essentially the I +// coefficient in a PID control sense; the bigger the feedback coefficient, +// the faster the solution converges, usually at the expense of accuracy. +// In any case, this is the free parameter in the Madgwick filtering and +// fusion scheme. +static float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // Compute beta +// Compute zeta, the other free parameter in the Madgwick scheme usually +// set to a small or zero value +static float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; +static float deltat = 0.0f; + +void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, + float gz, float mx, float my, float mz, float *); +void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, + float gz, float mx, float my, float mz, float *); #endif // _QUATERNIONFILTERS_H_ From 1598a6dd6fc2b3c84630b37ea73f16dbe29e7f74 Mon Sep 17 00:00:00 2001 From: Brent Wilkins Date: Tue, 19 Jul 2016 13:06:30 -0600 Subject: [PATCH 03/28] Got things separated and working --- .../MPU9250BasicAHRS/MPU9250BasicAHRS.ino | 100 ++++++++++-------- src/MPU9250.cpp | 16 ++- src/MPU9250.h | 17 ++- src/quaternionFilters.cpp | 42 ++++++-- src/quaternionFilters.h | 39 +------ 5 files changed, 118 insertions(+), 96 deletions(-) diff --git a/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino b/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino index 9cd66cd..26e17ec 100644 --- a/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino +++ b/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino @@ -40,6 +40,9 @@ Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); #endif // LCD +#define AHRS true // Set to false for basic data read +#define SerialDebug true // Set to true to get Serial output for debugging + // Pin definitions int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins int myLed = 13; // Set up pin 13 led for toggling @@ -223,8 +226,10 @@ void loop() // User environmental x-axis correction in milliGauss, should be // automatically calculated myIMU.magbias[0] = +470.; - myIMU.magbias[1] = +120.; // User environmental x-axis correction in milliGauss - myIMU.magbias[2] = +125.; // User environmental x-axis correction in milliGauss + // User environmental x-axis correction in milliGauss TODO axis?? + myIMU.magbias[1] = +120.; + // User environmental x-axis correction in milliGauss + myIMU.magbias[2] = +125.; // Calculate the magnetometer values in milliGauss // Include factory calibration per data sheet and user environmental @@ -238,13 +243,8 @@ void loop() myIMU.magbias[2]; } // if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) - myIMU.Now = micros(); - // Set integration time by time elapsed since last filter update - deltat = ((myIMU.Now - myIMU.lastUpdate)/1000000.0f); - myIMU.lastUpdate = myIMU.Now; - - myIMU.sum += deltat; // sum for averaging filter update rate - myIMU.sumCount++; + // Must be called before updating quaternions! + myIMU.updateTime(); // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of // the magnetometer; the magnetometer z-axis (+ down) is opposite to z-axis @@ -255,10 +255,9 @@ void loop() // modified to allow any convenient orientation convention. This is ok by // aircraft orientation standards! Pass gyro rate as rad/s // MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); - MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx*PI/180.0f, - myIMU.gy*PI/180.0f, myIMU.gz*PI/180.0f, myIMU.my, - myIMU.mx, myIMU.mz, q); - + MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx*DEG_TO_RAD, + myIMU.gy*DEG_TO_RAD, myIMU.gz*DEG_TO_RAD, myIMU.my, + myIMU.mx, myIMU.mz, myIMU.deltat); if (!AHRS) { @@ -284,8 +283,10 @@ void loop() Serial.println(" degrees/sec"); // Print mag values in degree/sec - Serial.print("X-mag field: "); Serial.print(myIMU.mx); Serial.print(" mG "); - Serial.print("Y-mag field: "); Serial.print(myIMU.my); Serial.print(" mG "); + Serial.print("X-mag field: "); Serial.print(myIMU.mx); + Serial.print(" mG "); + Serial.print("Y-mag field: "); Serial.print(myIMU.my); + Serial.print(" mG "); Serial.print("Z-mag field: "); Serial.print(myIMU.mz); Serial.println(" mG"); @@ -352,37 +353,46 @@ void loop() Serial.print(" mz = "); Serial.print( (int)myIMU.mz ); Serial.println(" mG"); - Serial.print("q0 = "); Serial.print(q[0]); - Serial.print(" qx = "); Serial.print(q[1]); - Serial.print(" qy = "); Serial.print(q[2]); - Serial.print(" qz = "); Serial.println(q[3]); + Serial.print("q0 = "); Serial.print(*getQ()); + Serial.print(" qx = "); Serial.print(*(getQ() + 1)); + Serial.print(" qy = "); Serial.print(*(getQ() + 2)); + Serial.print(" qz = "); Serial.println(*(getQ() + 3)); } - // Define output variables from updated quaternion---these are Tait-Bryan - // angles, commonly used in aircraft orientation. In this coordinate system, - // the positive z-axis is down toward Earth. 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. - // Pitch is angle between sensor x-axis and Earth ground plane, toward the - // Earth is positive, up toward the sky is negative. Roll is angle between - // sensor y-axis and Earth ground plane, y-axis up is positive roll. These - // arise from the definition of the homogeneous rotation matrix constructed - // from quaternions. Tait-Bryan angles as well as Euler angles are - // non-commutative; that is, the get the correct orientation the rotations - // must be applied in the correct order which for this configuration is yaw, - // pitch, and then roll. - // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. - myIMU.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]); - myIMU.pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); - myIMU.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]); - myIMU.pitch *= 180.0f / PI; - myIMU.yaw *= 180.0f / PI; +// Define output variables from updated quaternion---these are Tait-Bryan +// angles, commonly used in aircraft orientation. In this coordinate system, +// the positive z-axis is down toward Earth. 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. +// Pitch is angle between sensor x-axis and Earth ground plane, toward the +// Earth is positive, up toward the sky is negative. Roll is angle between +// sensor y-axis and Earth ground plane, y-axis up is positive roll. These +// arise from the definition of the homogeneous rotation matrix constructed +// from quaternions. Tait-Bryan angles as well as Euler angles are +// non-commutative; that is, the get the correct orientation the rotations +// must be applied in the correct order which for this configuration is yaw, +// pitch, and then roll. +// For more see +// http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles +// which has additional links. + //myIMU.yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + + myIMU.yaw = atan2(2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ() * + *(getQ()+3)), *getQ() * *getQ() + *(getQ()+1) * *(getQ()+1) + - *(getQ()+2) * *(getQ()+2) - *(getQ()+3) * *(getQ()+3)); + myIMU.pitch = -asin(2.0f * (*(getQ()+1) * *(getQ()+3) - *getQ() * + *(getQ()+2))); + myIMU.roll = atan2(2.0f * (*getQ() * *(getQ()+1) + *(getQ()+2) * + *(getQ()+3)), *getQ() * *getQ() - *(getQ()+1) * *(getQ()+1) + - *(getQ()+2) * *(getQ()+2) + *(getQ()+3) * *(getQ()+3)); + myIMU.pitch *= RAD_TO_DEG; + myIMU.yaw *= RAD_TO_DEG; // TODO: Declination at Danville, California is 13 degrees 48 minutes and 47 // seconds on 2014-04-04 - myIMU.yaw -= 13.8; - myIMU.roll *= 180.0f / PI; + // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is + // 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19 + // - http://www.ngdc.noaa.gov/geomag-web/#declination + myIMU.yaw -= 8.5; + myIMU.roll *= RAD_TO_DEG; if(SerialDebug) { @@ -393,7 +403,8 @@ void loop() Serial.print(", "); Serial.println(myIMU.roll, 2); - Serial.print("rate = "); Serial.print((float)myIMU.sumCount/myIMU.sum, 2); + Serial.print("rate = "); + Serial.print((float)myIMU.sumCount/myIMU.sum, 2); Serial.println(" Hz"); } @@ -438,7 +449,8 @@ void loop() // 6 DoF and MPU9150 9DoF sensors. The 3.3 V 8 MHz Pro Mini is doing pretty // well! display.setCursor(0, 40); display.print("rt: "); - display.print((float) myIMU.sumCount / myIMU.sum, 2); display.print(" Hz"); + display.print((float) myIMU.sumCount / myIMU.sum, 2); + display.print(" Hz"); display.display(); #endif // LCD diff --git a/src/MPU9250.cpp b/src/MPU9250.cpp index 0c64584..c57fd57 100644 --- a/src/MPU9250.cpp +++ b/src/MPU9250.cpp @@ -110,7 +110,20 @@ int16_t MPU9250::readTempData() readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array return ((int16_t)rawData[0] << 8) | rawData[1]; // Turn the MSB and LSB into a 16-bit value } - + +// Calculate the time the last update took for use in the quaternion filters +void MPU9250::updateTime() +{ + Now = micros(); + + // Set integration time by time elapsed since last filter update + deltat = ((Now - lastUpdate) / 1000000.0f); + lastUpdate = Now; + + sum += deltat; // sum for averaging filter update rate + sumCount++; +} + void MPU9250::initAK8963(float * destination) { // First extract the factory calibration for each magnetometer axis @@ -132,7 +145,6 @@ void MPU9250::initAK8963(float * destination) delay(10); } - void MPU9250::initMPU9250() { // wake up device diff --git a/src/MPU9250.h b/src/MPU9250.h index aa65642..90657e1 100644 --- a/src/MPU9250.h +++ b/src/MPU9250.h @@ -177,9 +177,6 @@ #define AK8963_ADDRESS 0x0C // Address of magnetometer #endif // AD0 -#define AHRS true // Set to false for basic data read -#define SerialDebug true // Set to true to get Serial output for debugging - class MPU9250 { protected: @@ -216,12 +213,13 @@ class MPU9250 float temperature; // Stores the real internal chip temperature in Celsius int16_t tempCount; // Temperature raw count output uint32_t delt_t = 0; // Used to control display output rate - uint32_t count = 0, sumCount = 0; // Used to control display output rate - // Used to calculate integration interval - uint32_t lastUpdate = 0, firstUpdate = 0; - // Integration interval for both filter schemes - float sum = 0.0f; - uint32_t Now = 0; // Used to calculate integration interval + + uint32_t count = 0, sumCount = 0; // used to control display output rate + // Static so other stuff can use it: MPU9250::deltat + float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes + uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval + uint32_t Now = 0; // used to calculate integration interval + int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output // Scale resolutions per LSB for the sensors @@ -249,6 +247,7 @@ class MPU9250 void readGyroData(int16_t *); void readMagData(int16_t *); int16_t readTempData(); + void updateTime(); void initAK8963(float *); void initMPU9250(); void calibrateMPU9250(float * gyroBias, float * accelBias); diff --git a/src/quaternionFilters.cpp b/src/quaternionFilters.cpp index eef5dbd..87094fe 100644 --- a/src/quaternionFilters.cpp +++ b/src/quaternionFilters.cpp @@ -9,12 +9,40 @@ // Kalman-based filtering algorithms but is much less computationally // intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz! -//#ifndef _QUATERNIONfILTERS_H_ -//#define _QUATERNIONfILTERS_H_ - #include "quaternionFilters.h" -void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float * q) +// These are the free parameters in the Mahony filter and fusion scheme, Kp +// for proportional feedback, Ki for integral +#define Kp 2.0f * 5.0f +#define Ki 0.0f + +static float GyroMeasError = PI * (40.0f / 180.0f); +// gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) +static float GyroMeasDrift = PI * (0.0f / 180.0f); +// There is a tradeoff in the beta parameter between accuracy and response +// speed. In the original Madgwick study, beta of 0.041 (corresponding to +// GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. +// However, with this value, the LSM9SD0 response time is about 10 seconds +// to a stable initial quaternion. Subsequent changes also require a +// longish lag time to a stable output, not fast enough for a quadcopter or +// robot car! By increasing beta (GyroMeasError) by about a factor of +// fifteen, the response time constant is reduced to ~2 sec. I haven't +// noticed any reduction in solution accuracy. This is essentially the I +// coefficient in a PID control sense; the bigger the feedback coefficient, +// the faster the solution converges, usually at the expense of accuracy. +// In any case, this is the free parameter in the Madgwick filtering and +// fusion scheme. +static float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // Compute beta +// Compute zeta, the other free parameter in the Madgwick scheme usually +// set to a small or zero value +static float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; + +// Vector to hold integral error for Mahony method +static float eInt[3] = {0.0f, 0.0f, 0.0f}; +// Vector to hold quaternion +static float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; + +void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat) { // short name local variable for readability float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; @@ -111,7 +139,7 @@ void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, // Similar to Madgwick scheme but uses proportional and integral filtering on // the error between estimated reference vectors and measured ones. -void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float * q) +void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat) { // short name local variable for readability float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; @@ -184,7 +212,7 @@ void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, fl gx = gx + Kp * ex + Ki * eInt[0]; gy = gy + Kp * ey + Ki * eInt[1]; gz = gz + Kp * ez + Ki * eInt[2]; - + // Integrate rate of change of quaternion pa = q2; pb = q3; @@ -203,4 +231,4 @@ void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, fl q[3] = q4 * norm; } -//#endif // _QUATERNIONfILTERS_H_ +const float * getQ () { return q; } diff --git a/src/quaternionFilters.h b/src/quaternionFilters.h index 68a1422..2b18a0f 100644 --- a/src/quaternionFilters.h +++ b/src/quaternionFilters.h @@ -3,41 +3,12 @@ #include -// These are the free parameters in the Mahony filter and fusion scheme, Kp -// for proportional feedback, Ki for integral -#define Kp 2.0f * 5.0f -#define Ki 0.0f - -// Vector to hold integral error for Mahony method -static float eInt[3] = {0.0f, 0.0f, 0.0f}; -// Vector to hold quaternion -static float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; - -static float GyroMeasError = PI * (40.0f / 180.0f); -// gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) -static float GyroMeasDrift = PI * (0.0f / 180.0f); -// There is a tradeoff in the beta parameter between accuracy and response -// speed. In the original Madgwick study, beta of 0.041 (corresponding to -// GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. -// However, with this value, the LSM9SD0 response time is about 10 seconds -// to a stable initial quaternion. Subsequent changes also require a -// longish lag time to a stable output, not fast enough for a quadcopter or -// robot car! By increasing beta (GyroMeasError) by about a factor of -// fifteen, the response time constant is reduced to ~2 sec. I haven't -// noticed any reduction in solution accuracy. This is essentially the I -// coefficient in a PID control sense; the bigger the feedback coefficient, -// the faster the solution converges, usually at the expense of accuracy. -// In any case, this is the free parameter in the Madgwick filtering and -// fusion scheme. -static float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // Compute beta -// Compute zeta, the other free parameter in the Madgwick scheme usually -// set to a small or zero value -static float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; -static float deltat = 0.0f; - void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, - float gz, float mx, float my, float mz, float *); + float gz, float mx, float my, float mz, + float deltat); void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, - float gz, float mx, float my, float mz, float *); + float gz, float mx, float my, float mz, + float deltat); +const float * getQ(); #endif // _QUATERNIONFILTERS_H_ From 069cc0a4c723c9535d223cb975355b8b70c23271 Mon Sep 17 00:00:00 2001 From: Brent Wilkins Date: Tue, 19 Jul 2016 14:08:15 -0600 Subject: [PATCH 04/28] Lil cleaner --- examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino b/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino index 26e17ec..0d12c6e 100644 --- a/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino +++ b/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino @@ -3,7 +3,7 @@ date: April 1, 2014 license: Beerware - Use this code however you'd like. If you find it useful you can buy me a beer some time. - Modified by Brent Wilkins May 31, 2016 + Modified by Brent Wilkins July 19, 2016 Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, getting properly scaled accelerometer, @@ -386,8 +386,6 @@ void loop() - *(getQ()+2) * *(getQ()+2) + *(getQ()+3) * *(getQ()+3)); myIMU.pitch *= RAD_TO_DEG; myIMU.yaw *= RAD_TO_DEG; -// TODO: Declination at Danville, California is 13 degrees 48 minutes and 47 -// seconds on 2014-04-04 // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is // 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19 // - http://www.ngdc.noaa.gov/geomag-web/#declination From 4cc172f0acad67b17d776dec6fa38b4ce2ff6b8f Mon Sep 17 00:00:00 2001 From: Brent Wilkins Date: Thu, 21 Jul 2016 11:24:13 -0600 Subject: [PATCH 05/28] removed unnecessary commented out code --- examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino b/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino index 0d12c6e..dd4e643 100644 --- a/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino +++ b/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino @@ -375,7 +375,6 @@ void loop() // For more see // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles // which has additional links. - //myIMU.yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + myIMU.yaw = atan2(2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ() * *(getQ()+3)), *getQ() * *getQ() + *(getQ()+1) * *(getQ()+1) - *(getQ()+2) * *(getQ()+2) - *(getQ()+3) * *(getQ()+3)); From b9efe8f2cf716668ada80e60d1bf182b413ed6ac Mon Sep 17 00:00:00 2001 From: Brent Wilkins Date: Tue, 26 Jul 2016 14:23:42 -0600 Subject: [PATCH 06/28] Removed old comments --- src/MPU9250.h | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/MPU9250.h b/src/MPU9250.h index 90657e1..579e315 100644 --- a/src/MPU9250.h +++ b/src/MPU9250.h @@ -215,8 +215,7 @@ class MPU9250 uint32_t delt_t = 0; // Used to control display output rate uint32_t count = 0, sumCount = 0; // used to control display output rate - // Static so other stuff can use it: MPU9250::deltat - float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes + float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval uint32_t Now = 0; // used to calculate integration interval @@ -233,12 +232,7 @@ class MPU9250 float SelfTest[6]; // Stores the 16-bit signed accelerometer sensor output int16_t accelCount[3]; - // Constants for 9 DoF fusion and AHRS (Attitude and Heading - // Reference System) - // gyroscope measurement error in rads/s (start at 40 deg/s) - protected: - public: void getMres(); void getGres(); From e1c388cd1bfd8d1449ae6f033b38b031f7b372f1 Mon Sep 17 00:00:00 2001 From: Brent Wilkins Date: Tue, 26 Jul 2016 14:24:04 -0600 Subject: [PATCH 07/28] Updated to match code --- keywords.txt | 191 ++++++++++++++++++++++++++++++++++++++++----------- 1 file changed, 151 insertions(+), 40 deletions(-) diff --git a/keywords.txt b/keywords.txt index e40ad03..6c390e6 100644 --- a/keywords.txt +++ b/keywords.txt @@ -6,52 +6,163 @@ # Datatypes (KEYWORD1) ################################################################################ -MAG_REG_t KEYWORD1 -ACC_REG_t KEYWORD1 -MAG_TEMP_EN_t KEYWORD1 -MAG_XYZDA_t KEYWORD1 -I2C_ADDR_t KEYWORD1 -AxesRaw_t KEYWORD1 -InterfaceMode_t KEYWORD1 -CHIP_t KEYWORD1 -AXIS_t KEYWORD1 -MAG_DO_t KEYWORD1 -MAG_FS_t KEYWORD1 -MAG_BDU_t KEYWORD1 -MAG_OMXY_t KEYWORD1 -MAG_OMZ_t KEYWORD1 -MAG_MD_t KEYWORD1 -ACC_FS_t KEYWORD1 -ACC_BDU_t KEYWORD1 -ACC_ODR_t KEYWORD1 -ACC_AXIS_EN_t KEYWORD1 -ACC_STATUS_FLAGS_t KEYWORD1 +MPU9250 KEYWORD1 ################################################################################ # Methods and Functions (KEYWORD2) ################################################################################ -MPU9250 KEYWORD2 -begin KEYWORD2 -readGyroX KEYWORD2 -readGyroY KEYWORD2 -readGyroZ KEYWORD2 -readAccelX KEYWORD2 -readAccelY KEYWORD2 -readAccelZ KEYWORD2 -readMagX KEYWORD2 -readMagY KEYWORD2 -readMagZ KEYWORD2 -readTempF KEYWORD2 -readTempC KEYWORD2 -getStatus KEYWORD2 +getMres KEYWORD2 +getGres KEYWORD2 +getAres KEYWORD2 +readAccelData KEYWORD2 +readGyroData KEYWORD2 +readMagData KEYWORD2 +readTempData KEYWORD2 +updateTime KEYWORD2 +initAK8963 KEYWORD2 +initMPU9250 KEYWORD2 +calibrateMPU9250 KEYWORD2 +MPU9250SelfTest KEYWORD2 +writeByte KEYWORD2 +readByte KEYWORD2 +readBytes KEYWORD2 + +MadgwickQuaternionUpdate KEYWORD2 +MahonyQuaternionUpdate KEYWORD2 +getQ KEYWORD2 ################################################################################ # Constants (LITERAL1) ################################################################################ - -SENSITIVITY_ACC LITERAL1 -SENSITIVITY_MAG LITERAL1 -DEBUG LITERAL1 -AERROR LITERAL1 -MERROR LITERAL1 +AK8963_ADDRESS LITERAL1 +WHO_AM_I_AK8963 LITERAL1 +INFO LITERAL1 +AK8963_ST1 LITERAL1 +AK8963_XOUT_L LITERAL1 +AK8963_XOUT_H LITERAL1 +AK8963_YOUT_L LITERAL1 +AK8963_YOUT_H LITERAL1 +AK8963_ZOUT_L LITERAL1 +AK8963_ZOUT_H LITERAL1 +AK8963_ST2 LITERAL1 +AK8963_CNTL LITERAL1 +AK8963_ASTC LITERAL1 +AK8963_I2CDIS LITERAL1 +AK8963_ASAX LITERAL1 +AK8963_ASAY LITERAL1 +AK8963_ASAZ LITERAL1 +SELF_TEST_X_GYRO LITERAL1 +SELF_TEST_Y_GYRO LITERAL1 +SELF_TEST_Z_GYRO LITERAL1 +SELF_TEST_X_ACCEL LITERAL1 +SELF_TEST_Y_ACCEL LITERAL1 +SELF_TEST_Z_ACCEL LITERAL1 +SELF_TEST_A LITERAL1 +XG_OFFSET_H LITERAL1 +XG_OFFSET_L LITERAL1 +YG_OFFSET_H LITERAL1 +YG_OFFSET_L LITERAL1 +ZG_OFFSET_H LITERAL1 +ZG_OFFSET_L LITERAL1 +SMPLRT_DIV LITERAL1 +CONFIG LITERAL1 +GYRO_CONFIG LITERAL1 +ACCEL_CONFIG LITERAL1 +ACCEL_CONFIG2 LITERAL1 +LP_ACCEL_ODR LITERAL1 +WOM_THR LITERAL1 +MOT_DUR LITERAL1 +ZMOT_THR LITERAL1 +ZRMOT_DUR LITERAL1 +FIFO_EN LITERAL1 +I2C_MST_CTRL LITERAL1 +I2C_SLV0_ADDR LITERAL1 +I2C_SLV0_REG LITERAL1 +I2C_SLV0_CTRL LITERAL1 +I2C_SLV1_ADDR LITERAL1 +I2C_SLV1_REG LITERAL1 +I2C_SLV1_CTRL LITERAL1 +I2C_SLV2_ADDR LITERAL1 +I2C_SLV2_REG LITERAL1 +I2C_SLV2_CTRL LITERAL1 +I2C_SLV3_ADDR LITERAL1 +I2C_SLV3_REG LITERAL1 +I2C_SLV3_CTRL LITERAL1 +I2C_SLV4_ADDR LITERAL1 +I2C_SLV4_REG LITERAL1 +I2C_SLV4_DO LITERAL1 +I2C_SLV4_CTRL LITERAL1 +I2C_SLV4_DI LITERAL1 +I2C_MST_STATUS LITERAL1 +INT_PIN_CFG LITERAL1 +INT_ENABLE LITERAL1 +DMP_INT_STATUS LITERAL1 +INT_STATUS LITERAL1 +ACCEL_XOUT_H LITERAL1 +ACCEL_XOUT_L LITERAL1 +ACCEL_YOUT_H LITERAL1 +ACCEL_YOUT_L LITERAL1 +ACCEL_ZOUT_H LITERAL1 +ACCEL_ZOUT_L LITERAL1 +TEMP_OUT_H LITERAL1 +TEMP_OUT_L LITERAL1 +GYRO_XOUT_H LITERAL1 +GYRO_XOUT_L LITERAL1 +GYRO_YOUT_H LITERAL1 +GYRO_YOUT_L LITERAL1 +GYRO_ZOUT_H LITERAL1 +GYRO_ZOUT_L LITERAL1 +EXT_SENS_DATA_00 LITERAL1 +EXT_SENS_DATA_01 LITERAL1 +EXT_SENS_DATA_02 LITERAL1 +EXT_SENS_DATA_03 LITERAL1 +EXT_SENS_DATA_04 LITERAL1 +EXT_SENS_DATA_05 LITERAL1 +EXT_SENS_DATA_06 LITERAL1 +EXT_SENS_DATA_07 LITERAL1 +EXT_SENS_DATA_08 LITERAL1 +EXT_SENS_DATA_09 LITERAL1 +EXT_SENS_DATA_10 LITERAL1 +EXT_SENS_DATA_11 LITERAL1 +EXT_SENS_DATA_12 LITERAL1 +EXT_SENS_DATA_13 LITERAL1 +EXT_SENS_DATA_14 LITERAL1 +EXT_SENS_DATA_15 LITERAL1 +EXT_SENS_DATA_16 LITERAL1 +EXT_SENS_DATA_17 LITERAL1 +EXT_SENS_DATA_18 LITERAL1 +EXT_SENS_DATA_19 LITERAL1 +EXT_SENS_DATA_20 LITERAL1 +EXT_SENS_DATA_21 LITERAL1 +EXT_SENS_DATA_22 LITERAL1 +EXT_SENS_DATA_23 LITERAL1 +MOT_DETECT_STATUS LITERAL1 +I2C_SLV0_DO LITERAL1 +I2C_SLV1_DO LITERAL1 +I2C_SLV2_DO LITERAL1 +I2C_SLV3_DO LITERAL1 +I2C_MST_DELAY_CTRL LITERAL1 +SIGNAL_PATH_RESET LITERAL1 +MOT_DETECT_CTRL LITERAL1 +USER_CTRL LITERAL1 +PWR_MGMT_1 LITERAL1 +PWR_MGMT_2 LITERAL1 +DMP_BANK LITERAL1 +DMP_RW_PNT LITERAL1 +DMP_REG LITERAL1 +DMP_REG_1 LITERAL1 +DMP_REG_2 LITERAL1 +FIFO_COUNTH LITERAL1 +FIFO_COUNTL LITERAL1 +FIFO_R_W LITERAL1 +WHO_AM_I_MPU9250 LITERAL1 +XA_OFFSET_H LITERAL1 +XA_OFFSET_L LITERAL1 +YA_OFFSET_H LITERAL1 +YA_OFFSET_L LITERAL1 +ZA_OFFSET_H LITERAL1 +ZA_OFFSET_L LITERAL1 +ADO LITERAL1 +MPU9250_ADDRESS LITERAL1 +AK8963_ADDRESS LITERAL1 From d6eadedc596e1b79ab33fc2002b29f06ec92465b Mon Sep 17 00:00:00 2001 From: Brent Wilkins Date: Tue, 26 Jul 2016 16:03:43 -0600 Subject: [PATCH 08/28] -m --- extras/MPU9250BasicAHRS_t3.ino | 1045 -------------- extras/MPU9250_LPS25H_BasicAHRS_t3.ino | 1167 ---------------- extras/MPU9250_MPL3115A2_BasicAHRS_t3.ino | 1493 --------------------- extras/MPU9250_MS5611_BasicAHRS_t3.ino | 1214 ----------------- extras/MPU9250_MS5637_AHRS_t3.ino | 1297 ------------------ extras/STM32F401/MPU9250.h | 879 ------------ extras/STM32F401/Readme.md | 3 - extras/STM32F401/main.cpp | 228 ---- library.properties | 4 +- 9 files changed, 2 insertions(+), 7328 deletions(-) delete mode 100644 extras/MPU9250BasicAHRS_t3.ino delete mode 100644 extras/MPU9250_LPS25H_BasicAHRS_t3.ino delete mode 100644 extras/MPU9250_MPL3115A2_BasicAHRS_t3.ino delete mode 100644 extras/MPU9250_MS5611_BasicAHRS_t3.ino delete mode 100644 extras/MPU9250_MS5637_AHRS_t3.ino delete mode 100644 extras/STM32F401/MPU9250.h delete mode 100644 extras/STM32F401/Readme.md delete mode 100644 extras/STM32F401/main.cpp diff --git a/extras/MPU9250BasicAHRS_t3.ino b/extras/MPU9250BasicAHRS_t3.ino deleted file mode 100644 index b07a780..0000000 --- a/extras/MPU9250BasicAHRS_t3.ino +++ /dev/null @@ -1,1045 +0,0 @@ -/* MPU9250 Basic Example Code - by: Kris Winer - date: April 1, 2014 - license: Beerware - Use this code however you'd like. If you - find it useful you can buy me a beer some time. - - Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, - getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to - allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and - Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1. - - SDA and SCL should have external pull-up resistors (to 3.3V). - 10k resistors are on the EMSENSR-9250 breakout board. - - Hardware setup: - MPU9250 Breakout --------- Arduino - VDD ---------------------- 3.3V - VDDI --------------------- 3.3V - SDA ----------------------- A4 - SCL ----------------------- A5 - GND ---------------------- GND - - Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library. - 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. - We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file. - We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file. - */ -//#include "Wire.h" -#include -#include -#include -#include - -// Using NOKIA 5110 monochrome 84 x 48 pixel display -// pin 9 - Serial clock out (SCLK) -// pin 8 - Serial data out (DIN) -// pin 7 - Data/Command select (D/C) -// pin 5 - LCD chip select (CS) -// pin 6 - LCD reset (RST) -Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); - -// 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 -// above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map -// -//Magnetometer Registers -#define AK8963_ADDRESS 0x0C -#define WHO_AM_I_AK8963 0x00 // should return 0x48 -#define INFO 0x01 -#define AK8963_ST1 0x02 // data ready status bit 0 -#define AK8963_XOUT_L 0x03 // data -#define AK8963_XOUT_H 0x04 -#define AK8963_YOUT_L 0x05 -#define AK8963_YOUT_H 0x06 -#define AK8963_ZOUT_L 0x07 -#define AK8963_ZOUT_H 0x08 -#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 -#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 -#define AK8963_ASTC 0x0C // Self test control -#define AK8963_I2CDIS 0x0F // I2C disable -#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value -#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value -#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value - -#define SELF_TEST_X_GYRO 0x00 -#define SELF_TEST_Y_GYRO 0x01 -#define SELF_TEST_Z_GYRO 0x02 - -/*#define X_FINE_GAIN 0x03 // [7:0] fine gain -#define Y_FINE_GAIN 0x04 -#define Z_FINE_GAIN 0x05 -#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer -#define XA_OFFSET_L_TC 0x07 -#define YA_OFFSET_H 0x08 -#define YA_OFFSET_L_TC 0x09 -#define ZA_OFFSET_H 0x0A -#define ZA_OFFSET_L_TC 0x0B */ - -#define SELF_TEST_X_ACCEL 0x0D -#define SELF_TEST_Y_ACCEL 0x0E -#define SELF_TEST_Z_ACCEL 0x0F - -#define SELF_TEST_A 0x10 - -#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope -#define XG_OFFSET_L 0x14 -#define YG_OFFSET_H 0x15 -#define YG_OFFSET_L 0x16 -#define ZG_OFFSET_H 0x17 -#define ZG_OFFSET_L 0x18 -#define SMPLRT_DIV 0x19 -#define CONFIG 0x1A -#define GYRO_CONFIG 0x1B -#define ACCEL_CONFIG 0x1C -#define ACCEL_CONFIG2 0x1D -#define LP_ACCEL_ODR 0x1E -#define WOM_THR 0x1F - -#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms -#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] -#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms - -#define FIFO_EN 0x23 -#define I2C_MST_CTRL 0x24 -#define I2C_SLV0_ADDR 0x25 -#define I2C_SLV0_REG 0x26 -#define I2C_SLV0_CTRL 0x27 -#define I2C_SLV1_ADDR 0x28 -#define I2C_SLV1_REG 0x29 -#define I2C_SLV1_CTRL 0x2A -#define I2C_SLV2_ADDR 0x2B -#define I2C_SLV2_REG 0x2C -#define I2C_SLV2_CTRL 0x2D -#define I2C_SLV3_ADDR 0x2E -#define I2C_SLV3_REG 0x2F -#define I2C_SLV3_CTRL 0x30 -#define I2C_SLV4_ADDR 0x31 -#define I2C_SLV4_REG 0x32 -#define I2C_SLV4_DO 0x33 -#define I2C_SLV4_CTRL 0x34 -#define I2C_SLV4_DI 0x35 -#define I2C_MST_STATUS 0x36 -#define INT_PIN_CFG 0x37 -#define INT_ENABLE 0x38 -#define DMP_INT_STATUS 0x39 // Check DMP interrupt -#define INT_STATUS 0x3A -#define ACCEL_XOUT_H 0x3B -#define ACCEL_XOUT_L 0x3C -#define ACCEL_YOUT_H 0x3D -#define ACCEL_YOUT_L 0x3E -#define ACCEL_ZOUT_H 0x3F -#define ACCEL_ZOUT_L 0x40 -#define TEMP_OUT_H 0x41 -#define TEMP_OUT_L 0x42 -#define GYRO_XOUT_H 0x43 -#define GYRO_XOUT_L 0x44 -#define GYRO_YOUT_H 0x45 -#define GYRO_YOUT_L 0x46 -#define GYRO_ZOUT_H 0x47 -#define GYRO_ZOUT_L 0x48 -#define EXT_SENS_DATA_00 0x49 -#define EXT_SENS_DATA_01 0x4A -#define EXT_SENS_DATA_02 0x4B -#define EXT_SENS_DATA_03 0x4C -#define EXT_SENS_DATA_04 0x4D -#define EXT_SENS_DATA_05 0x4E -#define EXT_SENS_DATA_06 0x4F -#define EXT_SENS_DATA_07 0x50 -#define EXT_SENS_DATA_08 0x51 -#define EXT_SENS_DATA_09 0x52 -#define EXT_SENS_DATA_10 0x53 -#define EXT_SENS_DATA_11 0x54 -#define EXT_SENS_DATA_12 0x55 -#define EXT_SENS_DATA_13 0x56 -#define EXT_SENS_DATA_14 0x57 -#define EXT_SENS_DATA_15 0x58 -#define EXT_SENS_DATA_16 0x59 -#define EXT_SENS_DATA_17 0x5A -#define EXT_SENS_DATA_18 0x5B -#define EXT_SENS_DATA_19 0x5C -#define EXT_SENS_DATA_20 0x5D -#define EXT_SENS_DATA_21 0x5E -#define EXT_SENS_DATA_22 0x5F -#define EXT_SENS_DATA_23 0x60 -#define MOT_DETECT_STATUS 0x61 -#define I2C_SLV0_DO 0x63 -#define I2C_SLV1_DO 0x64 -#define I2C_SLV2_DO 0x65 -#define I2C_SLV3_DO 0x66 -#define I2C_MST_DELAY_CTRL 0x67 -#define SIGNAL_PATH_RESET 0x68 -#define MOT_DETECT_CTRL 0x69 -#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP -#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode -#define PWR_MGMT_2 0x6C -#define DMP_BANK 0x6D // Activates a specific bank in the DMP -#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank -#define DMP_REG 0x6F // Register in DMP from which to read or to which to write -#define DMP_REG_1 0x70 -#define DMP_REG_2 0x71 -#define FIFO_COUNTH 0x72 -#define FIFO_COUNTL 0x73 -#define FIFO_R_W 0x74 -#define WHO_AM_I_MPU9250 0x75 // Should return 0x71 -#define XA_OFFSET_H 0x77 -#define XA_OFFSET_L 0x78 -#define YA_OFFSET_H 0x7A -#define YA_OFFSET_L 0x7B -#define ZA_OFFSET_H 0x7D -#define ZA_OFFSET_L 0x7E - -// Using the MSENSR-9250 breakout board, ADO is set to 0 -// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 -//#define ADO 0 -#if ADO -#define MPU9250_ADDRESS 0x69 // Device address when ADO = 1 -#else -#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0 -#define AK8963_ADDRESS 0x0C // Address of magnetometer -#endif - -#define AHRS true // set to false for basic data read -#define SerialDebug true // set to true to get Serial output for debugging - -// Set initial input parameters -enum Ascale { - AFS_2G = 0, - AFS_4G, - AFS_8G, - AFS_16G -}; - -enum Gscale { - GFS_250DPS = 0, - GFS_500DPS, - GFS_1000DPS, - GFS_2000DPS -}; - -enum Mscale { - MFS_14BITS = 0, // 0.6 mG per LSB - MFS_16BITS // 0.15 mG per LSB -}; - -// Specify sensor full scale -uint8_t Gscale = GFS_250DPS; -uint8_t Ascale = AFS_2G; -uint8_t Mscale = MFS_16BITS; // Choose either 14-bit or 16-bit magnetometer resolution -uint8_t Mmode = 0x02; // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read -float aRes, gRes, mRes; // scale resolutions per LSB for the sensors - -// Pin definitions -int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins -int adoPin = 8; -int myLed = 13; - -int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output -int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output -int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output -float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0}; // Factory mag calibration and mag bias -float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer -int16_t tempCount; // temperature raw count output -float temperature; // Stores the real internal chip temperature in degrees Celsius -float SelfTest[6]; // holds results of gyro and accelerometer self test - -// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System) -float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s) -float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) -// There is a tradeoff in the beta parameter between accuracy and response speed. -// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. -// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion. -// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car! -// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec -// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense; -// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. -// In any case, this is the free parameter in the Madgwick filtering and fusion scheme. -float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta -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 -#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 -#define Ki 0.0f - -uint32_t delt_t = 0; // used to control display output rate -uint32_t count = 0, sumCount = 0; // used to control display output rate -float pitch, yaw, roll; -float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes -uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval -uint32_t Now = 0; // used to calculate integration interval - -float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values -float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion -float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method - - -void setup() -{ -// Wire.begin(); -// TWBR = 12; // 400 kbit/sec I2C speed - // Setup for Master mode, pins 18/19, external pullups, 400kHz - Wire.begin(I2C_MASTER, 0x00, I2C_PINS_16_17, I2C_PULLUP_EXT, I2C_RATE_100); - Serial.begin(38400); - - // Set up the interrupt pin, its set as active high, push-pull - pinMode(intPin, INPUT); - digitalWrite(intPin, LOW); - pinMode(adoPin, OUTPUT); - digitalWrite(adoPin, HIGH); - pinMode(myLed, OUTPUT); - digitalWrite(myLed, HIGH); - - display.begin(); // Initialize the display - display.setContrast(58); // Set the contrast - -// Start device display with ID of sensor - display.clearDisplay(); - display.setTextSize(2); - display.setCursor(0,0); display.print("MPU9250"); - display.setTextSize(1); - display.setCursor(0, 20); display.print("9-DOF 16-bit"); - display.setCursor(0, 30); display.print("motion sensor"); - display.setCursor(20,40); display.print("60 ug LSB"); - display.display(); - delay(1000); - -// Set up for data display - display.setTextSize(1); // Set text size to normal, 2 is twice normal etc. - display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen - display.clearDisplay(); // clears the screen and buffer - - // Read the WHO_AM_I register, this is a good test of communication - byte c = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 - Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x71, HEX); - display.setCursor(20,0); display.print("MPU9250"); - display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(c, HEX); - display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0x71, HEX); - display.display(); - delay(5000); - - if (c == 0x71) // WHO_AM_I should always be 0x68 - { - Serial.println("MPU9250 is online..."); - - MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values - Serial.print("x-axis self test: acceleration trim within : "); Serial.print(SelfTest[0],1); Serial.println("% of factory value"); - Serial.print("y-axis self test: acceleration trim within : "); Serial.print(SelfTest[1],1); Serial.println("% of factory value"); - Serial.print("z-axis self test: acceleration trim within : "); Serial.print(SelfTest[2],1); Serial.println("% of factory value"); - Serial.print("x-axis self test: gyration trim within : "); Serial.print(SelfTest[3],1); Serial.println("% of factory value"); - Serial.print("y-axis self test: gyration trim within : "); Serial.print(SelfTest[4],1); Serial.println("% of factory value"); - Serial.print("z-axis self test: gyration trim within : "); Serial.print(SelfTest[5],1); Serial.println("% of factory value"); - delay(5000); - - calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers - display.clearDisplay(); - - display.setCursor(0, 0); display.print("MPU9250 bias"); - display.setCursor(0, 8); display.print(" x y z "); - - display.setCursor(0, 16); display.print((int)(1000*accelBias[0])); - display.setCursor(24, 16); display.print((int)(1000*accelBias[1])); - display.setCursor(48, 16); display.print((int)(1000*accelBias[2])); - display.setCursor(72, 16); display.print("mg"); - - display.setCursor(0, 24); display.print(gyroBias[0], 1); - display.setCursor(24, 24); display.print(gyroBias[1], 1); - display.setCursor(48, 24); display.print(gyroBias[2], 1); - display.setCursor(66, 24); display.print("o/s"); - - display.display(); - delay(1000); - - initMPU9250(); - Serial.println("MPU9250 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature - - // Read the WHO_AM_I register of the magnetometer, this is a good test of communication - byte d = readByte(AK8963_ADDRESS, WHO_AM_I_AK8963); // Read WHO_AM_I register for AK8963 - Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" I should be "); Serial.println(0x48, HEX); - display.clearDisplay(); - display.setCursor(20,0); display.print("AK8963"); - display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(d, HEX); - display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0x48, HEX); - display.display(); - delay(1000); - - // Get magnetometer calibration from AK8963 ROM - initAK8963(magCalibration); Serial.println("AK8963 initialized for active data mode...."); // Initialize device for active mode read of magnetometer - - if(SerialDebug) { -// Serial.println("Calibration values: "); - Serial.print("X-Axis sensitivity adjustment value "); Serial.println(magCalibration[0], 2); - Serial.print("Y-Axis sensitivity adjustment value "); Serial.println(magCalibration[1], 2); - Serial.print("Z-Axis sensitivity adjustment value "); Serial.println(magCalibration[2], 2); - } - - display.clearDisplay(); - display.setCursor(20,0); display.print("AK8963"); - display.setCursor(0,10); display.print("ASAX "); display.setCursor(50,10); display.print(magCalibration[0], 2); - display.setCursor(0,20); display.print("ASAY "); display.setCursor(50,20); display.print(magCalibration[1], 2); - display.setCursor(0,30); display.print("ASAZ "); display.setCursor(50,30); display.print(magCalibration[2], 2); - display.display(); - delay(1000); - } - else - { - Serial.print("Could not connect to MPU9250: 0x"); - Serial.println(c, HEX); - while(1) ; // Loop forever if communication doesn't happen - } -} - -void loop() -{ - // If intPin goes high, all data registers have new data - if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt - readAccelData(accelCount); // Read the x/y/z adc values - getAres(); - - // Now we'll calculate the accleration value into actual g's - ax = (float)accelCount[0]*aRes; // - accelBias[0]; // get actual g value, this depends on scale being set - ay = (float)accelCount[1]*aRes; // - accelBias[1]; - az = (float)accelCount[2]*aRes; // - accelBias[2]; - - readGyroData(gyroCount); // Read the x/y/z adc values - getGres(); - - // Calculate the gyro value into actual degrees per second - gx = (float)gyroCount[0]*gRes; // get actual gyro value, this depends on scale being set - gy = (float)gyroCount[1]*gRes; - gz = (float)gyroCount[2]*gRes; - - readMagData(magCount); // Read the x/y/z adc values - getMres(); - magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated - magbias[1] = +120.; // User environmental x-axis correction in milliGauss - magbias[2] = +125.; // User environmental x-axis correction in milliGauss - - // Calculate the magnetometer values in milliGauss - // Include factory calibration per data sheet and user environmental corrections - mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set - my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]; - mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; - } - - Now = micros(); - deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update - lastUpdate = Now; - - sum += deltat; // sum for averaging filter update rate - sumCount++; - - // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of the magnetometer; - // the magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro! - // We have to make some allowance for this orientationmismatch in feeding the output to the quaternion filter. - // For the MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like - // in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention. - // This is ok by aircraft orientation standards! - // Pass gyro rate as rad/s - MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); -// MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); - - - if (!AHRS) { - delt_t = millis() - count; - if(delt_t > 500) { - - if(SerialDebug) { - // Print acceleration values in milligs! - Serial.print("X-acceleration: "); Serial.print(1000*ax); Serial.print(" mg "); - Serial.print("Y-acceleration: "); Serial.print(1000*ay); Serial.print(" mg "); - Serial.print("Z-acceleration: "); Serial.print(1000*az); Serial.println(" mg "); - - // Print gyro values in degree/sec - Serial.print("X-gyro rate: "); Serial.print(gx, 3); Serial.print(" degrees/sec "); - Serial.print("Y-gyro rate: "); Serial.print(gy, 3); Serial.print(" degrees/sec "); - Serial.print("Z-gyro rate: "); Serial.print(gz, 3); Serial.println(" degrees/sec"); - - // Print mag values in degree/sec - Serial.print("X-mag field: "); Serial.print(mx); Serial.print(" mG "); - Serial.print("Y-mag field: "); Serial.print(my); Serial.print(" mG "); - Serial.print("Z-mag field: "); Serial.print(mz); Serial.println(" mG"); - - tempCount = readTempData(); // Read the adc values - temperature = ((float) tempCount) / 333.87 + 21.0; // Temperature in degrees Centigrade - // Print temperature in degrees Centigrade - Serial.print("Temperature is "); Serial.print(temperature, 1); Serial.println(" degrees C"); // Print T values to tenths of s degree C - } - - display.clearDisplay(); - display.setCursor(0, 0); display.print("MPU9250/AK8963"); - display.setCursor(0, 8); display.print(" x y z "); - - display.setCursor(0, 16); display.print((int)(1000*ax)); - display.setCursor(24, 16); display.print((int)(1000*ay)); - display.setCursor(48, 16); display.print((int)(1000*az)); - display.setCursor(72, 16); display.print("mg"); - - display.setCursor(0, 24); display.print((int)(gx)); - display.setCursor(24, 24); display.print((int)(gy)); - display.setCursor(48, 24); display.print((int)(gz)); - display.setCursor(66, 24); display.print("o/s"); - - display.setCursor(0, 32); display.print((int)(mx)); - display.setCursor(24, 32); display.print((int)(my)); - display.setCursor(48, 32); display.print((int)(mz)); - display.setCursor(72, 32); display.print("mG"); - - display.setCursor(0, 40); display.print("Gyro T "); - display.setCursor(50, 40); display.print(temperature, 1); display.print(" C"); - display.display(); - - count = millis(); - } - } - else { - - // Serial print and/or display at 0.5 s rate independent of data rates - delt_t = millis() - count; - if (delt_t > 500) { // update LCD once per half-second independent of read rate - - if(SerialDebug) { - Serial.print("ax = "); Serial.print((int)1000*ax); - Serial.print(" ay = "); Serial.print((int)1000*ay); - Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg"); - Serial.print("gx = "); Serial.print( gx, 2); - Serial.print(" gy = "); Serial.print( gy, 2); - Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s"); - Serial.print("mx = "); Serial.print( (int)mx ); - Serial.print(" my = "); Serial.print( (int)my ); - Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG"); - - Serial.print("q0 = "); Serial.print(q[0]); - Serial.print(" qx = "); Serial.print(q[1]); - Serial.print(" qy = "); Serial.print(q[2]); - Serial.print(" qz = "); Serial.println(q[3]); - } - - // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. - // In this coordinate system, the positive z-axis is down toward Earth. - // 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. - // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. - // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. - // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. - // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be - // applied in the correct order which for this configuration is yaw, pitch, and then roll. - // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. - 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]); - pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); - 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]); - pitch *= 180.0f / PI; - yaw *= 180.0f / PI; - yaw -= 13.8; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 - roll *= 180.0f / PI; - - if(SerialDebug) { - Serial.print("Yaw, Pitch, Roll: "); - Serial.print(yaw, 2); - Serial.print(", "); - Serial.print(pitch, 2); - Serial.print(", "); - Serial.println(roll, 2); - - Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz"); - } - - display.clearDisplay(); - - display.setCursor(0, 0); display.print(" x y z "); - - display.setCursor(0, 8); display.print((int)(1000*ax)); - display.setCursor(24, 8); display.print((int)(1000*ay)); - display.setCursor(48, 8); display.print((int)(1000*az)); - display.setCursor(72, 8); display.print("mg"); - - display.setCursor(0, 16); display.print((int)(gx)); - display.setCursor(24, 16); display.print((int)(gy)); - display.setCursor(48, 16); display.print((int)(gz)); - display.setCursor(66, 16); display.print("o/s"); - - display.setCursor(0, 24); display.print((int)(mx)); - display.setCursor(24, 24); display.print((int)(my)); - display.setCursor(48, 24); display.print((int)(mz)); - display.setCursor(72, 24); display.print("mG"); - - display.setCursor(0, 32); display.print((int)(yaw)); - display.setCursor(24, 32); display.print((int)(pitch)); - display.setCursor(48, 32); display.print((int)(roll)); - display.setCursor(66, 32); display.print("ypr"); - - // With these settings the filter is updating at a ~145 Hz rate using the Madgwick scheme and - // >200 Hz using the Mahony scheme even though the display refreshes at only 2 Hz. - // The filter update rate is determined mostly by the mathematical steps in the respective algorithms, - // the processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR: - // an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces - // filter update rates of 36 - 145 and ~38 Hz for the Madgwick and Mahony schemes, respectively. - // This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads. - // This filter update rate should be fast enough to maintain accurate platform orientation for - // stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz - // produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors. - // The 3.3 V 8 MHz Pro Mini is doing pretty well! - display.setCursor(0, 40); display.print("rt: "); display.print((float) sumCount / sum, 2); display.print(" Hz"); - display.display(); - - digitalWrite(myLed, !digitalRead(myLed)); - count = millis(); - sumCount = 0; - sum = 0; - } - } - -} - -//=================================================================================================================== -//====== Set of useful function to access acceleration. gyroscope, magnetometer, and temperature data -//=================================================================================================================== - -void getMres() { - switch (Mscale) - { - // Possible magnetometer scales (and their register bit settings) are: - // 14 bit resolution (0) and 16 bit resolution (1) - case MFS_14BITS: - mRes = 10.*4912./8190.; // Proper scale to return milliGauss - break; - case MFS_16BITS: - mRes = 10.*4912./32760.0; // Proper scale to return milliGauss - break; - } -} - -void getGres() { - switch (Gscale) - { - // Possible gyro scales (and their register bit settings) are: - // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: - case GFS_250DPS: - gRes = 250.0/32768.0; - break; - case GFS_500DPS: - gRes = 500.0/32768.0; - break; - case GFS_1000DPS: - gRes = 1000.0/32768.0; - break; - case GFS_2000DPS: - gRes = 2000.0/32768.0; - break; - } -} - -void getAres() { - switch (Ascale) - { - // Possible accelerometer scales (and their register bit settings) are: - // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: - case AFS_2G: - aRes = 2.0/32768.0; - break; - case AFS_4G: - aRes = 4.0/32768.0; - break; - case AFS_8G: - aRes = 8.0/32768.0; - break; - case AFS_16G: - aRes = 16.0/32768.0; - break; - } -} - - -void readAccelData(int16_t * destination) -{ - uint8_t rawData[6]; // x/y/z accel register data stored here - readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array - destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; - destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; -} - - -void readGyroData(int16_t * destination) -{ - uint8_t rawData[6]; // x/y/z gyro register data stored here - readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array - destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; - destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; -} - -void readMagData(int16_t * destination) -{ - uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition - if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set - readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array - uint8_t c = rawData[6]; // End data read by reading ST2 register - if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data - destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian - destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; - } - } -} - -int16_t readTempData() -{ - uint8_t rawData[2]; // x/y/z gyro register data stored here - readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array - return ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a 16-bit value -} - -void initAK8963(float * destination) -{ - // First extract the factory calibration for each magnetometer axis - uint8_t rawData[3]; // x/y/z gyro calibration data stored here - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer - delay(10); - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode - delay(10); - readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values - destination[0] = (float)(rawData[0] - 128)/256. + 1.; // Return x-axis sensitivity adjustment values, etc. - destination[1] = (float)(rawData[1] - 128)/256. + 1.; - destination[2] = (float)(rawData[2] - 128)/256. + 1.; - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer - delay(10); - // Configure the magnetometer for continuous read and highest resolution - // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, - // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates - writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR - delay(10); -} - - -void initMPU9250() -{ - // wake up device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors - delay(100); // Wait for all registers to reset - - // get stable time source - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else - delay(200); - - // Configure Gyro and Thermometer - // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively; - // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot - // be higher than 1 / 0.0059 = 170 Hz - // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both - // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x03); - - // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; a rate consistent with the filter update rate - // determined inset in CONFIG above - -// Set gyroscope full scale range - // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 - uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value - // c = c & ~0xE0; // Clear self-test bits [7:5] - c = c & ~0x02; // Clear Fchoice bits [1:0] - c = c & ~0x18; // Clear AFS bits [4:3] - c = c | Gscale << 3; // Set full scale range for the gyro - // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register - - // Set accelerometer full-scale range configuration - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value - // c = c & ~0xE0; // Clear self-test bits [7:5] - c = c & ~0x18; // Clear AFS bits [4:3] - c = c | Ascale << 3; // Set full scale range for the accelerometer - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value - - // Set accelerometer sample rate configuration - // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for - // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value - c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) - c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value - - // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, - // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting - - // Configure Interrupts and Bypass Enable - // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared, - // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips - // can join the I2C bus and all can be controlled by the Arduino as master - writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22); - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt - delay(100); -} - - -// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average -// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. -void calibrateMPU9250(float * dest1, float * dest2) -{ - uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data - uint16_t ii, packet_count, fifo_count; - int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; - - // reset device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device - delay(100); - - // get stable time source; Auto select clock source to be PLL gyroscope reference if ready - // else use the internal oscillator, bits 2:0 = 001 - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); - writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); - delay(200); - -// Configure device for bias calculation - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source - writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP - delay(15); - -// Configure MPU6050 gyro and accelerometer for bias calculation - writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity - - uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec - uint16_t accelsensitivity = 16384; // = 16384 LSB/g - - // Configure FIFO to capture accelerometer and gyro data for bias calculation - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150) - delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes - -// At end of sample accumulation, turn off FIFO sensor read - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO - readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count - fifo_count = ((uint16_t)data[0] << 8) | data[1]; - packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging - - for (ii = 0; ii < packet_count; ii++) { - int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; - readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging - accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO - accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; - accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; - gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; - gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; - gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; - - accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases - accel_bias[1] += (int32_t) accel_temp[1]; - accel_bias[2] += (int32_t) accel_temp[2]; - gyro_bias[0] += (int32_t) gyro_temp[0]; - gyro_bias[1] += (int32_t) gyro_temp[1]; - gyro_bias[2] += (int32_t) gyro_temp[2]; - -} - accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases - accel_bias[1] /= (int32_t) packet_count; - accel_bias[2] /= (int32_t) packet_count; - gyro_bias[0] /= (int32_t) packet_count; - gyro_bias[1] /= (int32_t) packet_count; - gyro_bias[2] /= (int32_t) packet_count; - - if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation - else {accel_bias[2] += (int32_t) accelsensitivity;} - -// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup - 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 - data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases - data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; - data[3] = (-gyro_bias[1]/4) & 0xFF; - data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; - data[5] = (-gyro_bias[2]/4) & 0xFF; - -// Push gyro biases to hardware registers - writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]); - -// Output scaled gyro biases for display in the main program - dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; - dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; - dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; - -// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain -// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold -// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature -// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that -// the accelerometer biases calculated above must be divided by 8. - - int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases - readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values - accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); - accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); - accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - - uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers - uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis - - for(ii = 0; ii < 3; ii++) { - if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit - } - - // Construct total accelerometer bias, including calculated average accelerometer bias from above - accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) - accel_bias_reg[1] -= (accel_bias[1]/8); - accel_bias_reg[2] -= (accel_bias[2]/8); - - data[0] = (accel_bias_reg[0] >> 8) & 0xFF; - data[1] = (accel_bias_reg[0]) & 0xFF; - data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers - data[2] = (accel_bias_reg[1] >> 8) & 0xFF; - data[3] = (accel_bias_reg[1]) & 0xFF; - data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers - data[4] = (accel_bias_reg[2] >> 8) & 0xFF; - data[5] = (accel_bias_reg[2]) & 0xFF; - data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers - -// Apparently this is not working for the acceleration biases in the MPU-9250 -// Are we handling the temperature correction bit properly? -// Push accelerometer biases to hardware registers - writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); - -// Output scaled accelerometer biases for display in the main program - dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; - dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; - dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; -} - -// Accelerometer and gyroscope self test; check calibration wrt factory settings -void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass -{ - uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; - uint8_t selfTest[6]; - int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3]; - float factoryTrim[6]; - uint8_t FS = 0; - - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1< -#include -#include -#include - -// Using NOKIA 5110 monochrome 84 x 48 pixel display -// pin 7 - Serial clock out (SCLK) -// pin 6 - Serial data out (DIN) -// pin 5 - Data/Command select (D/C) -// pin 3 - LCD chip select (SCE) -// pin 4 - LCD reset (RST) -Adafruit_PCD8544 display = Adafruit_PCD8544(7, 6, 5, 3, 4); - -// See LPS25H "MEMS pressure sensor: 260-1260 hPa absolute digital output barometer" Data Sheet -#define LPS25H_REF_P_XL 0x08 -#define LPS25H_REF_P_L 0x09 -#define LPS25H_REF_P_H 0x0A -#define LPS25H_WHOAMI 0x0F // should return 0xBD -#define LPS25H_RES_CONF 0x10 -#define LPS25H_CTRL_REG1 0x20 -#define LPS25H_CTRL_REG2 0x21 -#define LPS25H_CTRL_REG3 0x22 -#define LPS25H_CTRL_REG4 0x23 -#define LPS25H_INT_CFG 0x24 -#define LPS25H_INT_SOURCE 0x25 -#define LPS25H_STATUS_REG 0x27 -#define LPS25H_PRESS_OUT_XL 0x28 -#define LPS25H_PRESS_OUT_L 0x29 -#define LPS25H_PRESS_OUT_H 0x2A -#define LPS25H_TEMP_OUT_L 0x2B -#define LPS25H_TEMP_OUT_H 0x2C -#define LPS25H_FIFO_CTRL 0x2E -#define LPS25H_FIFO_STATUS 0x2F -#define LPS25H_THS_P_L 0x30 -#define LPS25H_THS_P_H 0x31 -#define LPS25H_RPDS_L 0x39 -#define LPS25H_RPDS_H 0x3A - -// 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 -// above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map -// -//Magnetometer Registers -#define AK8963_ADDRESS 0x0C -#define WHO_AM_I_AK8963 0x00 // should return 0x48 -#define INFO 0x01 -#define AK8963_ST1 0x02 // data ready status bit 0 -#define AK8963_XOUT_L 0x03 // data -#define AK8963_XOUT_H 0x04 -#define AK8963_YOUT_L 0x05 -#define AK8963_YOUT_H 0x06 -#define AK8963_ZOUT_L 0x07 -#define AK8963_ZOUT_H 0x08 -#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 -#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 -#define AK8963_ASTC 0x0C // Self test control -#define AK8963_I2CDIS 0x0F // I2C disable -#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value -#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value -#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value - -#define SELF_TEST_X_GYRO 0x00 -#define SELF_TEST_Y_GYRO 0x01 -#define SELF_TEST_Z_GYRO 0x02 - -/*#define X_FINE_GAIN 0x03 // [7:0] fine gain -#define Y_FINE_GAIN 0x04 -#define Z_FINE_GAIN 0x05 -#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer -#define XA_OFFSET_L_TC 0x07 -#define YA_OFFSET_H 0x08 -#define YA_OFFSET_L_TC 0x09 -#define ZA_OFFSET_H 0x0A -#define ZA_OFFSET_L_TC 0x0B */ - -#define SELF_TEST_X_ACCEL 0x0D -#define SELF_TEST_Y_ACCEL 0x0E -#define SELF_TEST_Z_ACCEL 0x0F - -#define SELF_TEST_A 0x10 - -#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope -#define XG_OFFSET_L 0x14 -#define YG_OFFSET_H 0x15 -#define YG_OFFSET_L 0x16 -#define ZG_OFFSET_H 0x17 -#define ZG_OFFSET_L 0x18 -#define SMPLRT_DIV 0x19 -#define CONFIG 0x1A -#define GYRO_CONFIG 0x1B -#define ACCEL_CONFIG 0x1C -#define ACCEL_CONFIG2 0x1D -#define LP_ACCEL_ODR 0x1E -#define WOM_THR 0x1F - -#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms -#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] -#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms - -#define FIFO_EN 0x23 -#define I2C_MST_CTRL 0x24 -#define I2C_SLV0_ADDR 0x25 -#define I2C_SLV0_REG 0x26 -#define I2C_SLV0_CTRL 0x27 -#define I2C_SLV1_ADDR 0x28 -#define I2C_SLV1_REG 0x29 -#define I2C_SLV1_CTRL 0x2A -#define I2C_SLV2_ADDR 0x2B -#define I2C_SLV2_REG 0x2C -#define I2C_SLV2_CTRL 0x2D -#define I2C_SLV3_ADDR 0x2E -#define I2C_SLV3_REG 0x2F -#define I2C_SLV3_CTRL 0x30 -#define I2C_SLV4_ADDR 0x31 -#define I2C_SLV4_REG 0x32 -#define I2C_SLV4_DO 0x33 -#define I2C_SLV4_CTRL 0x34 -#define I2C_SLV4_DI 0x35 -#define I2C_MST_STATUS 0x36 -#define INT_PIN_CFG 0x37 -#define INT_ENABLE 0x38 -#define DMP_INT_STATUS 0x39 // Check DMP interrupt -#define INT_STATUS 0x3A -#define ACCEL_XOUT_H 0x3B -#define ACCEL_XOUT_L 0x3C -#define ACCEL_YOUT_H 0x3D -#define ACCEL_YOUT_L 0x3E -#define ACCEL_ZOUT_H 0x3F -#define ACCEL_ZOUT_L 0x40 -#define TEMP_OUT_H 0x41 -#define TEMP_OUT_L 0x42 -#define GYRO_XOUT_H 0x43 -#define GYRO_XOUT_L 0x44 -#define GYRO_YOUT_H 0x45 -#define GYRO_YOUT_L 0x46 -#define GYRO_ZOUT_H 0x47 -#define GYRO_ZOUT_L 0x48 -#define EXT_SENS_DATA_00 0x49 -#define EXT_SENS_DATA_01 0x4A -#define EXT_SENS_DATA_02 0x4B -#define EXT_SENS_DATA_03 0x4C -#define EXT_SENS_DATA_04 0x4D -#define EXT_SENS_DATA_05 0x4E -#define EXT_SENS_DATA_06 0x4F -#define EXT_SENS_DATA_07 0x50 -#define EXT_SENS_DATA_08 0x51 -#define EXT_SENS_DATA_09 0x52 -#define EXT_SENS_DATA_10 0x53 -#define EXT_SENS_DATA_11 0x54 -#define EXT_SENS_DATA_12 0x55 -#define EXT_SENS_DATA_13 0x56 -#define EXT_SENS_DATA_14 0x57 -#define EXT_SENS_DATA_15 0x58 -#define EXT_SENS_DATA_16 0x59 -#define EXT_SENS_DATA_17 0x5A -#define EXT_SENS_DATA_18 0x5B -#define EXT_SENS_DATA_19 0x5C -#define EXT_SENS_DATA_20 0x5D -#define EXT_SENS_DATA_21 0x5E -#define EXT_SENS_DATA_22 0x5F -#define EXT_SENS_DATA_23 0x60 -#define MOT_DETECT_STATUS 0x61 -#define I2C_SLV0_DO 0x63 -#define I2C_SLV1_DO 0x64 -#define I2C_SLV2_DO 0x65 -#define I2C_SLV3_DO 0x66 -#define I2C_MST_DELAY_CTRL 0x67 -#define SIGNAL_PATH_RESET 0x68 -#define MOT_DETECT_CTRL 0x69 -#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP -#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode -#define PWR_MGMT_2 0x6C -#define DMP_BANK 0x6D // Activates a specific bank in the DMP -#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank -#define DMP_REG 0x6F // Register in DMP from which to read or to which to write -#define DMP_REG_1 0x70 -#define DMP_REG_2 0x71 -#define FIFO_COUNTH 0x72 -#define FIFO_COUNTL 0x73 -#define FIFO_R_W 0x74 -#define WHO_AM_I_MPU9250 0x75 // Should return 0x71 -#define XA_OFFSET_H 0x77 -#define XA_OFFSET_L 0x78 -#define YA_OFFSET_H 0x7A -#define YA_OFFSET_L 0x7B -#define ZA_OFFSET_H 0x7D -#define ZA_OFFSET_L 0x7E - -// Using the MPU9250+LPS25HMiniTeensy 3.1 Add-On shield, ADO is set to 0 -#define ADO 0 -#if ADO -#define MPU9250_ADDRESS 0x69 // Device address when ADO = 1 -#define AK8963_ADDRESS 0x0C // Address of magnetometer -#define LPS25H_ADDRESS 0x5D // Address of altimeter -#else -#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0 -#define AK8963_ADDRESS 0x0C // Address of magnetometer -#define LPS25H_ADDRESS 0x5C // Address of altimeter -#endif - -#define SerialDebug true // set to true to get Serial output for debugging - -// Set initial input parameters -enum Ascale { - AFS_2G = 0, - AFS_4G, - AFS_8G, - AFS_16G -}; - -enum Gscale { - GFS_250DPS = 0, - GFS_500DPS, - GFS_1000DPS, - GFS_2000DPS -}; - -enum PODR { // Altimeter output data rate - P_1shot = 0, - P_1Hz, - P_7Hz, - P_12Hz, // 12.5 Hz output data rate - P_25Hz -}; - -enum Pavg { // Altimeter pressure internal data averaging - P_avg_8 = 0, // average pressure data 8 times - P_avg_32, // average pressure data 32 times - P_avg_128, // average pressure data 128 times - P_avg_512 // average pressure data 512 times -}; - -enum Tavg { // Altimeter temperature internal data averaging - T_avg_8 = 0, // average temperature data 8 times - T_avg_16, // average temperature data 16 times - T_avg_32, // average temperature data 32 times - T_avg_64 // average temperature data 64 times -}; - -enum Mscale { - MFS_14BITS = 0, // 0.6 mG per LSB - MFS_16BITS // 0.15 mG per LSB -}; - - -// Specify sensor full scale -uint8_t PODR = P_12Hz, Pavg = P_avg_512, Tavg = T_avg_64; // set pressure amd temperature output data rate -uint8_t Gscale = GFS_250DPS; -uint8_t Ascale = AFS_2G; -uint8_t Mscale = MFS_16BITS; // Choose either 14-bit or 16-bit magnetometer resolution -uint8_t Mmode = 0x02; // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read -float aRes, gRes, mRes; // scale resolutions per LSB for the sensors - -// Pin definitions -int intP = 8; -int intG = 9; -int myLed = 13; - -int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output -int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output -int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output -float magCalibration[3] = {0, 0, 0}; // Factory mag calibration -float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0}; // Bias corrections for gyro, accelerometer, and mag -int16_t tempCount; // temperature raw count output -float gyrotemperature; // Stores the MPU9250 gyro internal chip temperature in degrees Celsius -float Temperature, Pressure; // stores LPS25H pressures sensor pressure and temperature -float SelfTest[6]; // holds results of gyro and accelerometer self test - -// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System) -float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s) -float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) -// There is a tradeoff in the beta parameter between accuracy and response speed. -// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. -// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion. -// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car! -// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec -// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense; -// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. -// In any case, this is the free parameter in the Madgwick filtering and fusion scheme. -float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta -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 -#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 -#define Ki 0.0f - -uint32_t delt_t = 0, count = 0, sumCount = 0; // used to control display output rate -float pitch, yaw, roll; -float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes -uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval -uint32_t Now = 0; // used to calculate integration interval - -float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values -float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion -float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method - - -void setup() -{ -// Wire.begin(); -// TWBR = 12; // 400 kbit/sec I2C speed for Pro Mini - // Setup for Master mode, pins 18/19, external pullups, 400kHz for Teensy 3.1 - Wire.begin(I2C_MASTER, 0x00, I2C_PINS_16_17, I2C_PULLUP_EXT, I2C_RATE_400); - delay(4000); - Serial.begin(38400); - - // Set up the interrupt pin, its set as active high, push-pull - pinMode(intP, INPUT); - pinMode(intG, INPUT); - pinMode(myLed, OUTPUT); - digitalWrite(myLed, HIGH); - - display.begin(); // Initialize the display - display.setContrast(58); // Set the contrast - -// Start device display with ID of sensor - display.clearDisplay(); - display.setTextSize(2); - display.setCursor(0,0); display.print("MPU9250"); - display.setTextSize(1); - display.setCursor(0, 20); display.print("9-DOF 16-bit"); - display.setCursor(0, 30); display.print("motion sensor"); - display.setCursor(20,40); display.print("60 ug LSB"); - display.display(); - delay(1000); - -// Set up for data display - display.setTextSize(1); // Set text size to normal, 2 is twice normal etc. - display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen - display.clearDisplay(); // clears the screen and buffer - - // Read the WHO_AM_I register, this is a good test of communication - Serial.println("MPU9250 9-axis motion sensor..."); - byte c = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 - Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x71, HEX); - display.setCursor(20,0); display.print("MPU9250"); - display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(c, HEX); - display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0x71, HEX); - display.display(); - delay(1000); - - if (c == 0x71) // WHO_AM_I should always be 0x68 - { - Serial.println("MPU9250 is online..."); - - MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values - Serial.print("x-axis self test: acceleration trim within : "); Serial.print(SelfTest[0],1); Serial.println("% of factory value"); - Serial.print("y-axis self test: acceleration trim within : "); Serial.print(SelfTest[1],1); Serial.println("% of factory value"); - Serial.print("z-axis self test: acceleration trim within : "); Serial.print(SelfTest[2],1); Serial.println("% of factory value"); - Serial.print("x-axis self test: gyration trim within : "); Serial.print(SelfTest[3],1); Serial.println("% of factory value"); - Serial.print("y-axis self test: gyration trim within : "); Serial.print(SelfTest[4],1); Serial.println("% of factory value"); - Serial.print("z-axis self test: gyration trim within : "); Serial.print(SelfTest[5],1); Serial.println("% of factory value"); - delay(1000); - - // get sensor resolutions, only need to do this once - getAres(); - getGres(); - getMres(); - - Serial.println(" Calibrate gyro and accel"); - accelgyrocalMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers - Serial.println("accel biases (mg)"); Serial.println(1000.*accelBias[0]); Serial.println(1000.*accelBias[1]); Serial.println(1000.*accelBias[2]); - Serial.println("gyro biases (dps)"); Serial.println(gyroBias[0]); Serial.println(gyroBias[1]); Serial.println(gyroBias[2]); - - display.clearDisplay(); - - display.setCursor(0, 0); display.print("MPU9250 bias"); - display.setCursor(0, 8); display.print(" x y z "); - - display.setCursor(0, 16); display.print((int)(1000*accelBias[0])); - display.setCursor(24, 16); display.print((int)(1000*accelBias[1])); - display.setCursor(48, 16); display.print((int)(1000*accelBias[2])); - display.setCursor(72, 16); display.print("mg"); - - display.setCursor(0, 24); display.print(gyroBias[0], 1); - display.setCursor(24, 24); display.print(gyroBias[1], 1); - display.setCursor(48, 24); display.print(gyroBias[2], 1); - display.setCursor(66, 24); display.print("o/s"); - - display.display(); - delay(1000); - - initMPU9250(); - Serial.println("MPU9250 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature - - // Read the WHO_AM_I register of the magnetometer, this is a good test of communication - byte d = readByte(AK8963_ADDRESS, WHO_AM_I_AK8963); // Read WHO_AM_I register for AK8963 - Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" I should be "); Serial.println(0x48, HEX); - display.clearDisplay(); - display.setCursor(20,0); display.print("AK8963"); - display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(d, HEX); - display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0x48, HEX); - display.display(); - delay(1000); - - // Get magnetometer calibration from AK8963 ROM - initAK8963(magCalibration); Serial.println("AK8963 initialized for active data mode...."); // Initialize device for active mode read of magnetometer - - magcalMPU9250(magBias); - Serial.println("mag biases (mG)"); Serial.println(magBias[0]); Serial.println(magBias[1]); Serial.println(magBias[2]); - delay(2000); // add delay to see results before serial spew of data - - if(SerialDebug) { -// Serial.println("Calibration values: "); - Serial.print("X-Axis sensitivity adjustment value "); Serial.println(magCalibration[0], 2); - Serial.print("Y-Axis sensitivity adjustment value "); Serial.println(magCalibration[1], 2); - Serial.print("Z-Axis sensitivity adjustment value "); Serial.println(magCalibration[2], 2); - } - - display.clearDisplay(); - display.setCursor(20,0); display.print("AK8963"); - display.setCursor(0,10); display.print("ASAX "); display.setCursor(50,10); display.print(magCalibration[0], 2); - display.setCursor(0,20); display.print("ASAY "); display.setCursor(50,20); display.print(magCalibration[1], 2); - display.setCursor(0,30); display.print("ASAZ "); display.setCursor(50,30); display.print(magCalibration[2], 2); - display.display(); - delay(1000); - - // Read the WHO_AM_I register of the altimeter this is a good test of communication - byte e = readByte(LPS25H_ADDRESS, LPS25H_WHOAMI); // Read WHO_AM_I register for LPS25H - Serial.print("LPS25H "); Serial.print("I AM "); Serial.print(e, HEX); Serial.print(" I should be "); Serial.println(0xBD, HEX); - display.clearDisplay(); - display.setCursor(20,0); display.print("LPS25H"); - display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(e, HEX); - display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0xBD, HEX); - display.display(); - delay(1000); - - LPS25HInit(); // Initialize LPS25H altimeter - - } - else - { - Serial.print("Could not connect to MPU9250: 0x"); - Serial.println(c, HEX); - while(1) ; // Loop forever if communication doesn't happen - } -} - -void loop() -{ - // If intPin goes high, all data registers have new data - if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt - // if (digitalRead(intPin)) { // On interrupt, read data - readAccelData(accelCount); // Read the x/y/z adc values - - // Now we'll calculate the accleration value into actual g's - ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set - ay = (float)accelCount[1]*aRes - accelBias[1]; - az = (float)accelCount[2]*aRes - accelBias[2]; - - readGyroData(gyroCount); // Read the x/y/z adc values - - // Calculate the gyro value into actual degrees per second - gx = (float)gyroCount[0]*gRes; // get actual gyro value, this depends on scale being set - gy = (float)gyroCount[1]*gRes; - gz = (float)gyroCount[2]*gRes; - - readMagData(magCount); // Read the x/y/z adc values - - // Calculate the magnetometer values in milliGauss - // Include factory calibration per data sheet and user environmental corrections - mx = (float)magCount[0]*mRes*magCalibration[0] - magBias[0]; // get actual magnetometer value, this depends on scale being set - my = (float)magCount[1]*mRes*magCalibration[1] - magBias[1]; - mz = (float)magCount[2]*mRes*magCalibration[2] - magBias[2]; - } - - Now = micros(); - deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update - lastUpdate = Now; - - sum += deltat; // sum for averaging filter update rate - sumCount++; - - // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of the magnetometer; - // the magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro! - // We have to make some allowance for this orientationmismatch in feeding the output to the quaternion filter. - // For the MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like - // in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention. - // This is ok by aircraft orientation standards! - // Pass gyro rate as rad/s - MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); -// MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); - - // Serial print and/or display at 0.5 s rate independent of data rates - delt_t = millis() - count; - if (delt_t > 500) { // update LCD once per half-second independent of read rate - - if(SerialDebug) { - Serial.print("ax = "); Serial.print((int)1000*ax); - Serial.print(" ay = "); Serial.print((int)1000*ay); - Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg"); - Serial.print("gx = "); Serial.print( gx, 2); - Serial.print(" gy = "); Serial.print( gy, 2); - Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s"); - Serial.print("mx = "); Serial.print( (int)mx ); - Serial.print(" my = "); Serial.print( (int)my ); - Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG"); - - Serial.print("q0 = "); Serial.print(q[0]); - Serial.print(" qx = "); Serial.print(q[1]); - Serial.print(" qy = "); Serial.print(q[2]); - Serial.print(" qz = "); Serial.println(q[3]); - } - - tempCount = readGyroTempData(); // Read the gyro adc values - gyrotemperature = ((float) tempCount) / 333.87 + 21.0; // Gyro chip temperature in degrees Centigrade - // Print temperature in degrees Centigrade - Serial.print("Gyro temperature is "); Serial.print(gyrotemperature, 1); Serial.println(" degrees C"); // Print T values to tenths of s degree C - - // Get altimeter data - if(readByte(LPS25H_ADDRESS, LPS25H_STATUS_REG) & 0x20) { // check if new altimeter data ready - Pressure = (float) readAltimeterPressure()/4096.0f; - Temperature = (float) readAltimeterTemperature()/480.0f + 42.5f; - } -/* - const int station_elevation_m = 1050.0f*0.3048f; // Accurate for the roof on my house; convert from feet to meters - float baroin = Pressure; // pressure is now in millibars - // Formula to correct absolute pressure in millbars to "altimeter pressure" in inches of mercury - // comparable to weather report pressure - float part1 = baroin - 0.3f; //Part 1 of formula - const float part2 = 0.0000842288f; - float part3 = pow(part1, 0.190284f); - float part4 = (float)station_elevation_m / part3; - float part5 = (1.0f + (part2 * part4)); - float part6 = pow(part5, 5.2553026f); - float altimeter_setting_pressure_mb = part1 * part6; // Output is now in adjusted millibars - baroin = altimeter_setting_pressure_mb * 0.02953f; -*/ - float altitude = 145366.45f*(1. - pow((Pressure/1013.25f), 0.190284f)); - - if(SerialDebug) { - Serial.print("Altimeter temperature = "); Serial.print( Temperature, 2); Serial.println(" C"); // temperature in degrees Celsius - Serial.print("Altimeter temperature = "); Serial.print(9.*Temperature/5. + 32., 2); Serial.println(" F"); // temperature in degrees Fahrenheit - Serial.print("Altimeter pressure = "); Serial.print(Pressure, 2); Serial.println(" mbar");// pressure in millibar - Serial.print("Altitude = "); Serial.print(altitude, 2); Serial.println(" feet"); - } - - // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. - // In this coordinate system, the positive z-axis is down toward Earth. - // 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. - // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. - // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. - // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. - // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be - // applied in the correct order which for this configuration is yaw, pitch, and then roll. - // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. - 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]); - pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); - 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]); - pitch *= 180.0f / PI; - yaw *= 180.0f / PI; - yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 - roll *= 180.0f / PI; - - if(SerialDebug) { - Serial.print("Yaw, Pitch, Roll: "); - Serial.print(yaw, 2); - Serial.print(", "); - Serial.print(pitch, 2); - Serial.print(", "); - Serial.println(roll, 2); - - Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz"); - } - - display.clearDisplay(); - - display.setCursor(0, 0); display.print(" x y z "); - - display.setCursor(0, 8); display.print((int)(1000*ax)); - display.setCursor(24, 8); display.print((int)(1000*ay)); - display.setCursor(48, 8); display.print((int)(1000*az)); - display.setCursor(72, 8); display.print("mg"); - - display.setCursor(0, 16); display.print((int)(gx)); - display.setCursor(24, 16); display.print((int)(gy)); - display.setCursor(48, 16); display.print((int)(gz)); - display.setCursor(66, 16); display.print("o/s"); - - display.setCursor(0, 24); display.print((int)(mx)); - display.setCursor(24, 24); display.print((int)(my)); - display.setCursor(48, 24); display.print((int)(mz)); - display.setCursor(72, 24); display.print("mG"); - - display.setCursor(0, 32); display.print((int)(yaw)); - display.setCursor(24, 32); display.print((int)(pitch)); - display.setCursor(48, 32); display.print((int)(roll)); - display.setCursor(66, 32); display.print("ypr"); - - // With these settings the filter is updating at a ~145 Hz rate using the Madgwick scheme and - // >200 Hz using the Mahony scheme even though the display refreshes at only 2 Hz. - // The filter update rate is determined mostly by the mathematical steps in the respective algorithms, - // the processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR: - // an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces - // filter update rates of 36 - 145 and ~38 Hz for the Madgwick and Mahony schemes, respectively. - // This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads. - // This filter update rate should be fast enough to maintain accurate platform orientation for - // stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz - // produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors. - // The 3.3 V 8 MHz Pro Mini is doing pretty well! - display.setCursor(0, 40); display.print(altitude, 0); display.print("ft"); - display.setCursor(68, 0); display.print(9.*Temperature/5. + 32., 0); - display.setCursor(42, 40); display.print((float) sumCount / (1000.*sum), 2); display.print("kHz"); - display.display(); - - digitalWrite(myLed, !digitalRead(myLed)); - count = millis(); - sumCount = 0; - sum = 0; - } - -} - -//=================================================================================================================== -//====== Set of useful function to access acceleration. gyroscope, magnetometer, and temperature data -//=================================================================================================================== - -void getMres() { - switch (Mscale) - { - // Possible magnetometer scales (and their register bit settings) are: - // 14 bit resolution (0) and 16 bit resolution (1) - case MFS_14BITS: - mRes = 10.*4912./8190.; // Proper scale to return milliGauss - break; - case MFS_16BITS: - mRes = 10.*4912./32760.0; // Proper scale to return milliGauss - break; - } -} - -void getGres() { - switch (Gscale) - { - // Possible gyro scales (and their register bit settings) are: - // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: - case GFS_250DPS: - gRes = 250.0/32768.0; - break; - case GFS_500DPS: - gRes = 500.0/32768.0; - break; - case GFS_1000DPS: - gRes = 1000.0/32768.0; - break; - case GFS_2000DPS: - gRes = 2000.0/32768.0; - break; - } -} - -void getAres() { - switch (Ascale) - { - // Possible accelerometer scales (and their register bit settings) are: - // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: - case AFS_2G: - aRes = 2.0/32768.0; - break; - case AFS_4G: - aRes = 4.0/32768.0; - break; - case AFS_8G: - aRes = 8.0/32768.0; - break; - case AFS_16G: - aRes = 16.0/32768.0; - break; - } -} - - -void readAccelData(int16_t * destination) -{ - uint8_t rawData[6]; // x/y/z accel register data stored here - readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array - destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; - destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; -} - - -void readGyroData(int16_t * destination) -{ - uint8_t rawData[6]; // x/y/z gyro register data stored here - readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array - destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; - destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; -} - -void readMagData(int16_t * destination) -{ - uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition - if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set - readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array - uint8_t c = rawData[6]; // End data read by reading ST2 register - if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data - destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian - destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; - } - } -} - -int16_t readGyroTempData() -{ - uint8_t rawData[2]; // x/y/z gyro register data stored here - readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array - return ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a 16-bit value -} - -int32_t readAltimeterPressure() -{ - uint8_t rawData[3]; // 24-bit pressure register data stored here - readBytes(LPS25H_ADDRESS, (LPS25H_PRESS_OUT_XL | 0x80), 3, &rawData[0]); // bit 7 must be one to read multiple bytes - return (int32_t) ((int32_t) rawData[2] << 16 | (int32_t) rawData[1] << 8 | rawData[0]); -} - -int16_t readAltimeterTemperature() -{ - uint8_t rawData[2]; // 16-bit pressure register data stored here - readBytes(LPS25H_ADDRESS, (LPS25H_TEMP_OUT_L | 0x80), 2, &rawData[0]); // bit 7 must be one to read multiple bytes - return (int16_t)((int16_t) rawData[1] << 8 | rawData[0]); -} - - -void initAK8963(float * destination) -{ - // First extract the factory calibration for each magnetometer axis - uint8_t rawData[3]; // x/y/z gyro calibration data stored here - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer - delay(10); - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode - delay(10); - readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values - destination[0] = (float)(rawData[0] - 128)/256. + 1.; // Return x-axis sensitivity adjustment values, etc. - destination[1] = (float)(rawData[1] - 128)/256. + 1.; - destination[2] = (float)(rawData[2] - 128)/256. + 1.; - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer - delay(10); - // Configure the magnetometer for continuous read and highest resolution - // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, - // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates - writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR - delay(10); -} - - -void initMPU9250() -{ - // wake up device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors - delay(100); // Wait for all registers to reset - - // get stable time source - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else - delay(200); - - // Configure Gyro and Thermometer - // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively; - // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot - // be higher than 1 / 0.0059 = 170 Hz - // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both - // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x03); - - // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; a rate consistent with the filter update rate - // determined inset in CONFIG above - -// Set gyroscope full scale range - // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 - uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value - // c = c & ~0xE0; // Clear self-test bits [7:5] - c = c & ~0x02; // Clear Fchoice bits [1:0] - c = c & ~0x18; // Clear AFS bits [4:3] - c = c | Gscale << 3; // Set full scale range for the gyro - // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register - - // Set accelerometer full-scale range configuration - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value - // c = c & ~0xE0; // Clear self-test bits [7:5] - c = c & ~0x18; // Clear AFS bits [4:3] - c = c | Ascale << 3; // Set full scale range for the accelerometer - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value - - // Set accelerometer sample rate configuration - // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for - // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value - c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) - c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value - - // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, - // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting - - // Configure Interrupts and Bypass Enable - // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared, - // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips - // can join the I2C bus and all can be controlled by the Arduino as master - writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22); - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt - delay(100); -} - - -// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average -// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. -void accelgyrocalMPU9250(float * dest1, float * dest2) -{ - uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data - uint16_t ii, packet_count, fifo_count; - int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; - - // reset device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device - delay(100); - - // get stable time source; Auto select clock source to be PLL gyroscope reference if ready - // else use the internal oscillator, bits 2:0 = 001 - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); - writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); - delay(200); - -// Configure device for bias calculation - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source - writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP - delay(15); - -// Configure MPU6050 gyro and accelerometer for bias calculation - writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity - - uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec - uint16_t accelsensitivity = 16384; // = 16384 LSB/g - - // Configure FIFO to capture accelerometer and gyro data for bias calculation - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150) - delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes - -// At end of sample accumulation, turn off FIFO sensor read - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO - readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count - fifo_count = ((uint16_t)data[0] << 8) | data[1]; - packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging - - for (ii = 0; ii < packet_count; ii++) { - int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; - readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging - accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO - accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; - accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; - gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; - gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; - gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; - - accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases - accel_bias[1] += (int32_t) accel_temp[1]; - accel_bias[2] += (int32_t) accel_temp[2]; - gyro_bias[0] += (int32_t) gyro_temp[0]; - gyro_bias[1] += (int32_t) gyro_temp[1]; - gyro_bias[2] += (int32_t) gyro_temp[2]; - -} - accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases - accel_bias[1] /= (int32_t) packet_count; - accel_bias[2] /= (int32_t) packet_count; - gyro_bias[0] /= (int32_t) packet_count; - gyro_bias[1] /= (int32_t) packet_count; - gyro_bias[2] /= (int32_t) packet_count; - - if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation - else {accel_bias[2] += (int32_t) accelsensitivity;} - -// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup - 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 - data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases - data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; - data[3] = (-gyro_bias[1]/4) & 0xFF; - data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; - data[5] = (-gyro_bias[2]/4) & 0xFF; - -// Push gyro biases to hardware registers - writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]); - -// Output scaled gyro biases for display in the main program - dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; - dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; - dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; - -// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain -// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold -// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature -// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that -// the accelerometer biases calculated above must be divided by 8. - - int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases - readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values - accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); - accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); - accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - - uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers - uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis - - for(ii = 0; ii < 3; ii++) { - if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit - } - - // Construct total accelerometer bias, including calculated average accelerometer bias from above - accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) - accel_bias_reg[1] -= (accel_bias[1]/8); - accel_bias_reg[2] -= (accel_bias[2]/8); - - data[0] = (accel_bias_reg[0] >> 8) & 0xFF; - data[1] = (accel_bias_reg[0]) & 0xFF; - data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers - data[2] = (accel_bias_reg[1] >> 8) & 0xFF; - data[3] = (accel_bias_reg[1]) & 0xFF; - data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers - data[4] = (accel_bias_reg[2] >> 8) & 0xFF; - data[5] = (accel_bias_reg[2]) & 0xFF; - data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers - -// Apparently this is not working for the acceleration biases in the MPU-9250 -// Are we handling the temperature correction bit properly? -// Push accelerometer biases to hardware registers -/* writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); -*/ -// Output scaled accelerometer biases for display in the main program - dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; - dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; - dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; -} - - - -void magcalMPU9250(float * dest1) -{ - uint16_t ii = 0, sample_count = 0; - int32_t mag_bias[3] = {0, 0, 0}; - int16_t mag_max[3] = {0, 0, 0}, mag_min[3] = {0, 0, 0}, mag_temp[3] = {0, 0, 0}; - - Serial.println("Mag Calibration: Wave device in a figure eight until done!"); - delay(4000); - - sample_count = 64; - for(ii = 0; ii < sample_count; ii++) { - readMagData(mag_temp); // Read the mag data - for (int jj = 0; jj < 3; jj++) { - if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj]; - if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj]; - } - delay(135); // at 8 Hz ODR, new mag data is available every 125 ms - } - -// Serial.println("mag x min/max:"); Serial.println(mag_max[0]); Serial.println(mag_min[0]); -// Serial.println("mag y min/max:"); Serial.println(mag_max[1]); Serial.println(mag_min[1]); -// Serial.println("mag z min/max:"); Serial.println(mag_max[2]); Serial.println(mag_min[2]); - - mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts - mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts - mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts - - dest1[0] = (float) mag_bias[0]*mRes*magCalibration[0]; // save mag biases in G for main program - dest1[1] = (float) mag_bias[1]*mRes*magCalibration[1]; - dest1[2] = (float) mag_bias[2]*mRes*magCalibration[2]; - - Serial.println("Mag Calibration done!"); -} - -// Accelerometer and gyroscope self test; check calibration wrt factory settings -void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass -{ - uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; - uint8_t selfTest[6]; - int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3]; - float factoryTrim[6]; - uint8_t FS = 0; - - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1< -#include -#include -#include - -// Using NOKIA 5110 monochrome 84 x 48 pixel display -// pin 9 - Serial clock out (SCLK) -// pin 8 - Serial data out (DIN) -// pin 7 - Data/Command select (D/C) -// pin 5 - LCD chip select (CS) -// pin 6 - LCD reset (RST) -Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); - -// 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 -// above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map -// -//Magnetometer Registers -#define AK8963_ADDRESS 0x0C -#define WHO_AM_I_AK8963 0x00 // should return 0x48 -#define INFO 0x01 -#define AK8963_ST1 0x02 // data ready status bit 0 -#define AK8963_XOUT_L 0x03 // data -#define AK8963_XOUT_H 0x04 -#define AK8963_YOUT_L 0x05 -#define AK8963_YOUT_H 0x06 -#define AK8963_ZOUT_L 0x07 -#define AK8963_ZOUT_H 0x08 -#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 -#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 -#define AK8963_ASTC 0x0C // Self test control -#define AK8963_I2CDIS 0x0F // I2C disable -#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value -#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value -#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value - -#define SELF_TEST_X_GYRO 0x00 -#define SELF_TEST_Y_GYRO 0x01 -#define SELF_TEST_Z_GYRO 0x02 - -/*#define X_FINE_GAIN 0x03 // [7:0] fine gain -#define Y_FINE_GAIN 0x04 -#define Z_FINE_GAIN 0x05 -#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer -#define XA_OFFSET_L_TC 0x07 -#define YA_OFFSET_H 0x08 -#define YA_OFFSET_L_TC 0x09 -#define ZA_OFFSET_H 0x0A -#define ZA_OFFSET_L_TC 0x0B */ - -#define SELF_TEST_X_ACCEL 0x0D -#define SELF_TEST_Y_ACCEL 0x0E -#define SELF_TEST_Z_ACCEL 0x0F - -#define SELF_TEST_A 0x10 - -#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope -#define XG_OFFSET_L 0x14 -#define YG_OFFSET_H 0x15 -#define YG_OFFSET_L 0x16 -#define ZG_OFFSET_H 0x17 -#define ZG_OFFSET_L 0x18 -#define SMPLRT_DIV 0x19 -#define CONFIG 0x1A -#define GYRO_CONFIG 0x1B -#define ACCEL_CONFIG 0x1C -#define ACCEL_CONFIG2 0x1D -#define LP_ACCEL_ODR 0x1E -#define WOM_THR 0x1F - -#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms -#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] -#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms - -#define FIFO_EN 0x23 -#define I2C_MST_CTRL 0x24 -#define I2C_SLV0_ADDR 0x25 -#define I2C_SLV0_REG 0x26 -#define I2C_SLV0_CTRL 0x27 -#define I2C_SLV1_ADDR 0x28 -#define I2C_SLV1_REG 0x29 -#define I2C_SLV1_CTRL 0x2A -#define I2C_SLV2_ADDR 0x2B -#define I2C_SLV2_REG 0x2C -#define I2C_SLV2_CTRL 0x2D -#define I2C_SLV3_ADDR 0x2E -#define I2C_SLV3_REG 0x2F -#define I2C_SLV3_CTRL 0x30 -#define I2C_SLV4_ADDR 0x31 -#define I2C_SLV4_REG 0x32 -#define I2C_SLV4_DO 0x33 -#define I2C_SLV4_CTRL 0x34 -#define I2C_SLV4_DI 0x35 -#define I2C_MST_STATUS 0x36 -#define INT_PIN_CFG 0x37 -#define INT_ENABLE 0x38 -#define DMP_INT_STATUS 0x39 // Check DMP interrupt -#define INT_STATUS 0x3A -#define ACCEL_XOUT_H 0x3B -#define ACCEL_XOUT_L 0x3C -#define ACCEL_YOUT_H 0x3D -#define ACCEL_YOUT_L 0x3E -#define ACCEL_ZOUT_H 0x3F -#define ACCEL_ZOUT_L 0x40 -#define TEMP_OUT_H 0x41 -#define TEMP_OUT_L 0x42 -#define GYRO_XOUT_H 0x43 -#define GYRO_XOUT_L 0x44 -#define GYRO_YOUT_H 0x45 -#define GYRO_YOUT_L 0x46 -#define GYRO_ZOUT_H 0x47 -#define GYRO_ZOUT_L 0x48 -#define EXT_SENS_DATA_00 0x49 -#define EXT_SENS_DATA_01 0x4A -#define EXT_SENS_DATA_02 0x4B -#define EXT_SENS_DATA_03 0x4C -#define EXT_SENS_DATA_04 0x4D -#define EXT_SENS_DATA_05 0x4E -#define EXT_SENS_DATA_06 0x4F -#define EXT_SENS_DATA_07 0x50 -#define EXT_SENS_DATA_08 0x51 -#define EXT_SENS_DATA_09 0x52 -#define EXT_SENS_DATA_10 0x53 -#define EXT_SENS_DATA_11 0x54 -#define EXT_SENS_DATA_12 0x55 -#define EXT_SENS_DATA_13 0x56 -#define EXT_SENS_DATA_14 0x57 -#define EXT_SENS_DATA_15 0x58 -#define EXT_SENS_DATA_16 0x59 -#define EXT_SENS_DATA_17 0x5A -#define EXT_SENS_DATA_18 0x5B -#define EXT_SENS_DATA_19 0x5C -#define EXT_SENS_DATA_20 0x5D -#define EXT_SENS_DATA_21 0x5E -#define EXT_SENS_DATA_22 0x5F -#define EXT_SENS_DATA_23 0x60 -#define MOT_DETECT_STATUS 0x61 -#define I2C_SLV0_DO 0x63 -#define I2C_SLV1_DO 0x64 -#define I2C_SLV2_DO 0x65 -#define I2C_SLV3_DO 0x66 -#define I2C_MST_DELAY_CTRL 0x67 -#define SIGNAL_PATH_RESET 0x68 -#define MOT_DETECT_CTRL 0x69 -#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP -#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode -#define PWR_MGMT_2 0x6C -#define DMP_BANK 0x6D // Activates a specific bank in the DMP -#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank -#define DMP_REG 0x6F // Register in DMP from which to read or to which to write -#define DMP_REG_1 0x70 -#define DMP_REG_2 0x71 -#define FIFO_COUNTH 0x72 -#define FIFO_COUNTL 0x73 -#define FIFO_R_W 0x74 -#define WHO_AM_I_MPU9250 0x75 // Should return 0x71 -#define XA_OFFSET_H 0x77 -#define XA_OFFSET_L 0x78 -#define YA_OFFSET_H 0x7A -#define YA_OFFSET_L 0x7B -#define ZA_OFFSET_H 0x7D -#define ZA_OFFSET_L 0x7E - - -// Standard 7-bit I2C slave address is 1100000 = 0x60, 8-bit read address is 0xC1, 8-bit write is 0xC0 -#define MPL3115A2_ADDRESS 0x60 // SA0 is high, 0x1C if low - -// Register defines courtesy A. Weiss and Nathan Seidle, SparkFun Electronics -#define MPL3115A2_STATUS 0x00 -#define MPL3115A2_OUT_P_MSB 0x01 -#define MPL3115A2_OUT_P_CSB 0x02 -#define MPL3115A2_OUT_P_LSB 0x03 -#define MPL3115A2_OUT_T_MSB 0x04 -#define MPL3115A2_OUT_T_LSB 0x05 -#define MPL3115A2_DR_STATUS 0x06 -#define MPL3115A2_OUT_P_DELTA_MSB 0x07 -#define MPL3115A2_OUT_P_DELTA_CSB 0x08 -#define MPL3115A2_OUT_P_DELTA_LSB 0x09 -#define MPL3115A2_OUT_T_DELTA_MSB 0x0A -#define MPL3115A2_OUT_T_DELTA_LSB 0x0B -#define MPL3115A2_WHO_AM_I 0x0C -#define MPL3115A2_F_STATUS 0x0D -#define MPL3115A2_F_DATA 0x0E -#define MPL3115A2_F_SETUP 0x0F -#define MPL3115A2_TIME_DLY 0x10 -#define MPL3115A2_SYSMOD 0x11 -#define MPL3115A2_INT_SOURCE 0x12 -#define MPL3115A2_PT_DATA_CFG 0x13 -#define MPL3115A2_BAR_IN_MSB 0x14 // Set at factory to equivalent sea level pressure for measurement location, generally no need to change -#define MPL3115A2_BAR_IN_LSB 0x15 // Set at factory to equivalent sea level pressure for measurement location, generally no need to change -#define MPL3115A2_P_TGT_MSB 0x16 -#define MPL3115A2_P_TGT_LSB 0x17 -#define MPL3115A2_T_TGT 0x18 -#define MPL3115A2_P_WND_MSB 0x19 -#define MPL3115A2_P_WND_LSB 0x1A -#define MPL3115A2_T_WND 0x1B -#define MPL3115A2_P_MIN_MSB 0x1C -#define MPL3115A2_P_MIN_CSB 0x1D -#define MPL3115A2_P_MIN_LSB 0x1E -#define MPL3115A2_T_MIN_MSB 0x1F -#define MPL3115A2_T_MIN_LSB 0x20 -#define MPL3115A2_P_MAX_MSB 0x21 -#define MPL3115A2_P_MAX_CSB 0x22 -#define MPL3115A2_P_MAX_LSB 0x23 -#define MPL3115A2_T_MAX_MSB 0x24 -#define MPL3115A2_T_MAX_LSB 0x25 -#define MPL3115A2_CTRL_REG1 0x26 -#define MPL3115A2_CTRL_REG2 0x27 -#define MPL3115A2_CTRL_REG3 0x28 -#define MPL3115A2_CTRL_REG4 0x29 -#define MPL3115A2_CTRL_REG5 0x2A -#define MPL3115A2_OFF_P 0x2B -#define MPL3115A2_OFF_T 0x2C -#define MPL3115A2_OFF_H 0x2D - -// Using the Teensy 3.1 Mini add-on shield, ADO is set to 1 -// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 -#define ADO 1 -#if ADO -#define MPU9250_ADDRESS 0x69 // Device address when ADO = 1 -#define AK8963_ADDRESS 0x0C // Address of magnetometer -#else -#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0 -#define AK8963_ADDRESS 0x0C // Address of magnetometer -#endif - -#define AHRS true // set to false for basic data read -#define SerialDebug true // set to true to get Serial output for debugging - -// Set initial input parameters -enum Ascale { - AFS_2G = 0, - AFS_4G, - AFS_8G, - AFS_16G -}; - -enum Gscale { - GFS_250DPS = 0, - GFS_500DPS, - GFS_1000DPS, - GFS_2000DPS -}; - -enum Mscale { - MFS_14BITS = 0, // 0.6 mG per LSB - MFS_16BITS // 0.15 mG per LSB -}; - -// Specify sensor full scale -uint8_t Gscale = GFS_250DPS; -uint8_t Ascale = AFS_2G; -uint8_t Mscale = MFS_16BITS; // Choose either 14-bit or 16-bit magnetometer resolution -uint8_t Mmode = 0x06; // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read -float aRes, gRes, mRes; // scale resolutions per LSB for the sensors - - -enum SAMPLERATE { - OS_6ms = 0, // 6 ms is minimum oversampling interval, corresponds to an oversample ration of 2^0 = 1 - OS_10ms, - OS_18ms, - OS_34ms, - OS_66ms, - OS_130ms, // 130 ms oversampling interval, 2^5 = 32 oversample ratio - OS_258ms, - OS_512ms -}; - -enum ST_VALUE { - ATS_1s = 0, // 6 ms is minimum oversampling interval, corresponds to an oversample ration of 2^0 = 1 - ATS_2s, - ATS_4s, - ATS_8s, - ATS_16s, - ATS_32s, - ATS_64s, // 2^6 = 64 s interval between up to 32 FIFO samples for half an hour of data logging - ATS_128s, - ATS_256s, - ATS_512s, - ATS_1024s, - ATS_2048s, - ATS_4096s, - ATS_8192s, - ATS_16384s, - ATS_32768s // 2^15 = 32768 s interval between up to 32 FIFO samples = 12 days of data logging! -}; - -uint8_t SAMPLERATE = OS_130ms; -uint8_t ST_VALUE = ATS_4s; -int AltimeterMode = 0; // use to choose between altimeter and barometer modes for FIFO data - -// Define device outputs -float altitude = 0.; -float pressure = 0.; - -// Pin definitions -int INTGX = 10; // Interrupt for MPU9250 accelerometer/gyro -int INTP1 = 9; // Interrupt 1 for MPL3115A2 pressure sensor -int INTP2 = 8; // Interrupt 2 for MPL3115A2 pressure sensor -int myLed = 13; // Teensy led is on pin 13 - -int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output -int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output -int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output -float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0}; // Factory mag calibration and mag bias -float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer -int16_t tempCount; // temperature raw count output -float temperature, Temperature; // Stores the real internal chip temperature in degrees Celsius -float SelfTest[6]; // holds results of gyro and accelerometer self test - -// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System) -float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s) -float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) -// There is a tradeoff in the beta parameter between accuracy and response speed. -// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. -// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion. -// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car! -// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec -// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense; -// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. -// In any case, this is the free parameter in the Madgwick filtering and fusion scheme. -float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta -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 -#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 -#define Ki 0.0f - -uint32_t delt_t = 0; // used to control display output rate -uint32_t count = 0, sumCount = 0; // used to control display output rate -float pitch, yaw, roll; -float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes -uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval -uint32_t Now = 0; // used to calculate integration interval - -float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values -float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion -float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method - - -void setup() -{ - // Setup for Master mode, pins 16/18, external pullups, 400kHz - Wire.begin(I2C_MASTER, 0x00, I2C_PINS_16_17, I2C_PULLUP_EXT, I2C_RATE_400); - delay(1000); - Serial.begin(38400); - - // Set up the interrupt pin, its set as active high, push-pull - pinMode(INTGX, INPUT); - pinMode(INTP1, INPUT); - pinMode(INTP2, INPUT); - pinMode(myLed, OUTPUT); - digitalWrite(myLed, HIGH); - - display.begin(); // Initialize the display - display.setContrast(58); // Set the contrast - -// Start device display with ID of sensor - display.clearDisplay(); - display.setTextSize(2); - display.setCursor(0,0); display.print("MPU9250"); - display.setTextSize(1); - display.setCursor(0, 20); display.print("9-DOF 16-bit"); - display.setCursor(0, 30); display.print("motion sensor"); - display.setCursor(20,40); display.print("60 ug LSB"); - display.display(); - delay(1000); - -// Set up for data display - display.setTextSize(1); // Set text size to normal, 2 is twice normal etc. - display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen - display.clearDisplay(); // clears the screen and buffer - - // Read the WHO_AM_I register, this is a good test of communication - Serial.println("MPU9250 + MPL3115A2 devices..."); - byte c = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 - Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x71, HEX); - display.setCursor(20,0); display.print("MPU9250"); - display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(c, HEX); - display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0x71, HEX); - display.display(); - delay(5000); - - if (c == 0x71) // WHO_AM_I should always be 0x68 - { - Serial.println("MPU9250 is online..."); - - MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values - Serial.print("x-axis self test: acceleration trim within : "); Serial.print(SelfTest[0],1); Serial.println("% of factory value"); - Serial.print("y-axis self test: acceleration trim within : "); Serial.print(SelfTest[1],1); Serial.println("% of factory value"); - Serial.print("z-axis self test: acceleration trim within : "); Serial.print(SelfTest[2],1); Serial.println("% of factory value"); - Serial.print("x-axis self test: gyration trim within : "); Serial.print(SelfTest[3],1); Serial.println("% of factory value"); - Serial.print("y-axis self test: gyration trim within : "); Serial.print(SelfTest[4],1); Serial.println("% of factory value"); - Serial.print("z-axis self test: gyration trim within : "); Serial.print(SelfTest[5],1); Serial.println("% of factory value"); - delay(5000); - - calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers - display.clearDisplay(); - - display.setCursor(0, 0); display.print("MPU9250 bias"); - display.setCursor(0, 8); display.print(" x y z "); - - display.setCursor(0, 16); display.print((int)(1000*accelBias[0])); - display.setCursor(24, 16); display.print((int)(1000*accelBias[1])); - display.setCursor(48, 16); display.print((int)(1000*accelBias[2])); - display.setCursor(72, 16); display.print("mg"); - - display.setCursor(0, 24); display.print(gyroBias[0], 1); - display.setCursor(24, 24); display.print(gyroBias[1], 1); - display.setCursor(48, 24); display.print(gyroBias[2], 1); - display.setCursor(66, 24); display.print("o/s"); - - display.display(); - delay(1000); - - initMPU9250(); - Serial.println("MPU9250 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature - - // Read the WHO_AM_I register of the magnetometer, this is a good test of communication - byte d = readByte(AK8963_ADDRESS, WHO_AM_I_AK8963); // Read WHO_AM_I register for AK8963 - Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" I should be "); Serial.println(0x48, HEX); - display.clearDisplay(); - display.setCursor(20,0); display.print("AK8963"); - display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(d, HEX); - display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0x48, HEX); - display.display(); - delay(1000); - - // Get magnetometer calibration from AK8963 ROM - initAK8963(magCalibration); Serial.println("AK8963 initialized for active data mode...."); // Initialize device for active mode read of magnetometer - - if(SerialDebug) { -// Serial.println("Calibration values: "); - Serial.print("X-Axis sensitivity adjustment value "); Serial.println(magCalibration[0], 2); - Serial.print("Y-Axis sensitivity adjustment value "); Serial.println(magCalibration[1], 2); - Serial.print("Z-Axis sensitivity adjustment value "); Serial.println(magCalibration[2], 2); - } - - display.clearDisplay(); - display.setCursor(20,0); display.print("AK8963"); - display.setCursor(0,10); display.print("ASAX "); display.setCursor(50,10); display.print(magCalibration[0], 2); - display.setCursor(0,20); display.print("ASAY "); display.setCursor(50,20); display.print(magCalibration[1], 2); - display.setCursor(0,30); display.print("ASAZ "); display.setCursor(50,30); display.print(magCalibration[2], 2); - display.display(); - delay(1000); - } - else - { - Serial.print("Could not connect to MPU9250: 0x"); - Serial.println(c, HEX); - while(1) ; // Loop forever if communication doesn't happen - } - - - // Read the WHO_AM_I register of the MPL3115A2, this is a good test of communication - c = readByte(MPL3115A2_ADDRESS, MPL3115A2_WHO_AM_I); // Read WHO_AM_I register - Serial.print("MPL3115A2 WHO_AM_I returned: 0x"); - Serial.println(c, HEX); - Serial.println("Should be 0xC4"); - Serial.println(); - display.setCursor(0, 0); display.print("MPL3115A2"); - display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(c, HEX); - display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0xC4, HEX); - display.display(); - delay(1000); - - if (c == 0xC4) // WHO_AM_I should always be 0xC4 - { - - MPL3115A2Reset(); // Start off by resetting all registers to the default - initRealTimeMPL3115A2(); // initialize the accelerometer for realtime data acquisition if communication is OK - MPL3115A2SampleRate(SAMPLERATE); // Set oversampling ratio - Serial.print("Oversampling Ratio is "); Serial.println(1< 500) { - - if(SerialDebug) { - // Print acceleration values in milligs! - Serial.print("X-acceleration: "); Serial.print(1000*ax); Serial.print(" mg "); - Serial.print("Y-acceleration: "); Serial.print(1000*ay); Serial.print(" mg "); - Serial.print("Z-acceleration: "); Serial.print(1000*az); Serial.println(" mg "); - - // Print gyro values in degree/sec - Serial.print("X-gyro rate: "); Serial.print(gx, 3); Serial.print(" degrees/sec "); - Serial.print("Y-gyro rate: "); Serial.print(gy, 3); Serial.print(" degrees/sec "); - Serial.print("Z-gyro rate: "); Serial.print(gz, 3); Serial.println(" degrees/sec"); - - // Print mag values in degree/sec - Serial.print("X-mag field: "); Serial.print(mx); Serial.print(" mG "); - Serial.print("Y-mag field: "); Serial.print(my); Serial.print(" mG "); - Serial.print("Z-mag field: "); Serial.print(mz); Serial.println(" mG"); - - tempCount = readTempData(); // Read the adc values - Temperature = ((float) tempCount) / 333.87 + 21.0; // Temperature in degrees Centigrade - // Print temperature in degrees Centigrade - Serial.print("Temperature is "); Serial.print(Temperature, 2); Serial.println(" degrees C"); // Print T values to tenths of s degree C - } - - display.clearDisplay(); - display.setCursor(0, 0); display.print("MPU9250/AK8963"); - display.setCursor(0, 8); display.print(" x y z "); - - display.setCursor(0, 16); display.print((int)(1000*ax)); - display.setCursor(24, 16); display.print((int)(1000*ay)); - display.setCursor(48, 16); display.print((int)(1000*az)); - display.setCursor(72, 16); display.print("mg"); - - display.setCursor(0, 24); display.print((int)(gx)); - display.setCursor(24, 24); display.print((int)(gy)); - display.setCursor(48, 24); display.print((int)(gz)); - display.setCursor(66, 24); display.print("o/s"); - - display.setCursor(0, 32); display.print((int)(mx)); - display.setCursor(24, 32); display.print((int)(my)); - display.setCursor(48, 32); display.print((int)(mz)); - display.setCursor(72, 32); display.print("mG"); - - display.setCursor(0, 40); display.print("Gyro T "); - display.setCursor(50, 40); display.print(temperature, 1); display.print(" C"); - display.display(); - - count = millis(); - } - } - else { - - // Serial print and/or display at 0.5 s rate independent of data rates - delt_t = millis() - count; - if (delt_t > 500) { // update LCD once per half-second independent of read rate - - MPL3115A2ActiveAltimeterMode(); - MPL3115A2readAltitude(); // Read the altitude - - MPL3115A2ActiveBarometerMode(); - MPL3115A2readPressure(); // Read the pressure - - const int station_elevation_m = 1050.0*0.3048; // Accurate for the roof on my house; convert from feet to meters - - float baroin = pressure/100; // pressure is now in millibars - - // Formula to correct absolute pressure in millbars to "altimeter pressure" in inches of mercury - // comparable to weather report pressure - float part1 = baroin - 0.3; //Part 1 of formula - const float part2 = 0.0000842288; - float part3 = pow(part1, 0.190284); - float part4 = (float)station_elevation_m / part3; - float part5 = (1.0 + (part2 * part4)); - float part6 = pow(part5, 5.2553026); - float altimeter_setting_pressure_mb = part1 * part6; // Output is now in adjusted millibars - baroin = altimeter_setting_pressure_mb * 0.02953; - - Serial.print("pressure is "); Serial.print(pressure, 2); Serial.println(" Pa"); // Print pressure in Pa - Serial.print("altitude is "); Serial.print(altitude, 2); Serial.print(" m, "); // Print altitude in meters - Serial.print(altitude/0.3048, 2); Serial.println(" ft"); // Print altitude in feet - Serial.print("temperature is "); Serial.print(temperature, 2); Serial.print(" C, "); // Print temperature in C - Serial.print((9.*temperature/5. + 32.), 1); Serial.println(" F"); // Print temperature in C - - display.clearDisplay(); // clears the screen and buffer - display.setCursor(10, 0); display.print("MPL3115A2"); - display.setCursor(0,10); display.print("P "); display.setCursor(30,10); display.print(pressure/1000., 2); display.print(" kPa"); - display.setCursor(0,20); display.print("Alt "); display.setCursor(24,20); display.print(altitude, 1); display.print(" m"); - display.setCursor(0,30); display.print("Alt "); display.setCursor(24,30); display.print(3.28084*altitude, 1); display.print(" ft"); - display.setCursor(0,40); display.print("T "); display.setCursor(30,40); display.print(temperature, 1); display.print(" C"); - display.display(); - - if(SerialDebug) { - Serial.print("ax = "); Serial.print((int)1000*ax); - Serial.print(" ay = "); Serial.print((int)1000*ay); - Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg"); - Serial.print("gx = "); Serial.print( gx, 2); - Serial.print(" gy = "); Serial.print( gy, 2); - Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s"); - Serial.print("mx = "); Serial.print( (int)mx ); - Serial.print(" my = "); Serial.print( (int)my ); - Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG"); - - Serial.print("q0 = "); Serial.print(q[0]); - Serial.print(" qx = "); Serial.print(q[1]); - Serial.print(" qy = "); Serial.print(q[2]); - Serial.print(" qz = "); Serial.println(q[3]); - - tempCount = readTempData(); // Read the adc values - Temperature = ((float) tempCount) / 333.87 + 21.0; // Temperature in degrees Centigrade - // Print temperature in degrees Centigrade - Serial.print("Temperature is "); Serial.print(Temperature, 1); Serial.println(" degrees C"); // Print T values to tenths of s degree C - - } - - // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. - // In this coordinate system, the positive z-axis is down toward Earth. - // 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. - // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. - // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. - // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. - // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be - // applied in the correct order which for this configuration is yaw, pitch, and then roll. - // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. - 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]); - pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); - 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]); - pitch *= 180.0f / PI; - yaw *= 180.0f / PI; - yaw -= 13.8; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 - roll *= 180.0f / PI; - - if(SerialDebug) { - Serial.print("Yaw, Pitch, Roll: "); - Serial.print(yaw, 2); - Serial.print(", "); - Serial.print(pitch, 2); - Serial.print(", "); - Serial.println(roll, 2); - - Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz"); - } - - display.clearDisplay(); - - display.setCursor(0, 0); display.print(" x y z "); - - display.setCursor(0, 8); display.print((int)(1000*ax)); - display.setCursor(24, 8); display.print((int)(1000*ay)); - display.setCursor(48, 8); display.print((int)(1000*az)); - display.setCursor(72, 8); display.print("mg"); - - display.setCursor(0, 16); display.print((int)(gx)); - display.setCursor(24, 16); display.print((int)(gy)); - display.setCursor(48, 16); display.print((int)(gz)); - display.setCursor(66, 16); display.print("o/s"); - - display.setCursor(0, 24); display.print((int)(mx)); - display.setCursor(24, 24); display.print((int)(my)); - display.setCursor(48, 24); display.print((int)(mz)); - display.setCursor(72, 24); display.print("mG"); - - display.setCursor(0, 32); display.print((int)(yaw)); - display.setCursor(24, 32); display.print((int)(pitch)); - display.setCursor(48, 32); display.print((int)(roll)); - display.setCursor(66, 32); display.print("ypr"); - - // With these settings the filter is updating at a ~145 Hz rate using the Madgwick scheme and - // >200 Hz using the Mahony scheme even though the display refreshes at only 2 Hz. - // The filter update rate is determined mostly by the mathematical steps in the respective algorithms, - // the processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR: - // an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces - // filter update rates of 36 - 145 and ~38 Hz for the Madgwick and Mahony schemes, respectively. - // This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads. - // This filter update rate should be fast enough to maintain accurate platform orientation for - // stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz - // produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors. - // The 3.3 V 8 MHz Pro Mini is doing pretty well! - display.setCursor(0, 40); display.print("rt: "); display.print((float) sumCount / sum, 2); display.print(" Hz"); - display.display(); - - digitalWrite(myLed, !digitalRead(myLed)); - count = millis(); - sumCount = 0; - sum = 0; - } - } - -} - -//=================================================================================================================== -//====== Set of useful function to access acceleration. gyroscope, magnetometer, and temperature data -//=================================================================================================================== - -void getMres() { - switch (Mscale) - { - // Possible magnetometer scales (and their register bit settings) are: - // 14 bit resolution (0) and 16 bit resolution (1) - case MFS_14BITS: - mRes = 10.*4912./8190.; // Proper scale to return milliGauss - break; - case MFS_16BITS: - mRes = 10.*4912./32760.0; // Proper scale to return milliGauss - break; - } -} - -void getGres() { - switch (Gscale) - { - // Possible gyro scales (and their register bit settings) are: - // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: - case GFS_250DPS: - gRes = 250.0/32768.0; - break; - case GFS_500DPS: - gRes = 500.0/32768.0; - break; - case GFS_1000DPS: - gRes = 1000.0/32768.0; - break; - case GFS_2000DPS: - gRes = 2000.0/32768.0; - break; - } -} - -void getAres() { - switch (Ascale) - { - // Possible accelerometer scales (and their register bit settings) are: - // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: - case AFS_2G: - aRes = 2.0/32768.0; - break; - case AFS_4G: - aRes = 4.0/32768.0; - break; - case AFS_8G: - aRes = 8.0/32768.0; - break; - case AFS_16G: - aRes = 16.0/32768.0; - break; - } -} - - -void readAccelData(int16_t * destination) -{ - uint8_t rawData[6]; // x/y/z accel register data stored here - readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array - destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; - destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; -} - - -void readGyroData(int16_t * destination) -{ - uint8_t rawData[6]; // x/y/z gyro register data stored here - readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array - destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; - destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; -} - -void readMagData(int16_t * destination) -{ - uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition - if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set - readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array - uint8_t c = rawData[6]; // End data read by reading ST2 register - if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data - destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian - destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; - } - } -} - -int16_t readTempData() -{ - uint8_t rawData[2]; // x/y/z gyro register data stored here - readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array - return ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a 16-bit value -} - -void initAK8963(float * destination) -{ - // First extract the factory calibration for each magnetometer axis - uint8_t rawData[3]; // x/y/z gyro calibration data stored here - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer - delay(10); - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode - delay(10); - readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values - destination[0] = (float)(rawData[0] - 128)/256. + 1.; // Return x-axis sensitivity adjustment values, etc. - destination[1] = (float)(rawData[1] - 128)/256. + 1.; - destination[2] = (float)(rawData[2] - 128)/256. + 1.; - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer - delay(10); - // Configure the magnetometer for continuous read and highest resolution - // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, - // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates - writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR - delay(10); -} - - -void initMPU9250() -{ - // wake up device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors - delay(100); // Wait for all registers to reset - - // get stable time source - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else - delay(200); - - // Configure Gyro and Thermometer - // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively; - // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot - // be higher than 1 / 0.0059 = 170 Hz - // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both - // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x03); - - // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; a rate consistent with the filter update rate - // determined inset in CONFIG above - - // Set gyroscope full scale range - // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 - uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value - // c = c & ~0xE0; // Clear self-test bits [7:5] - c = c & ~0x02; // Clear Fchoice bits [1:0] - c = c & ~0x18; // Clear AFS bits [4:3] - c = c | Gscale << 3; // Set full scale range for the gyro - // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register - - // Set accelerometer full-scale range configuration - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value - // c = c & ~0xE0; // Clear self-test bits [7:5] - c = c & ~0x18; // Clear AFS bits [4:3] - c = c | Ascale << 3; // Set full scale range for the accelerometer - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value - - // Set accelerometer sample rate configuration - // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for - // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value - c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) - c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value - - // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, - // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting - - // Configure Interrupts and Bypass Enable - // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared, - // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips - // can join the I2C bus and all can be controlled by the Arduino as master - writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22); - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt - delay(100); -} - - -// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average -// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. -void calibrateMPU9250(float * dest1, float * dest2) -{ - uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data - uint16_t ii, packet_count, fifo_count; - int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; - - // reset device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device - delay(100); - - // get stable time source; Auto select clock source to be PLL gyroscope reference if ready - // else use the internal oscillator, bits 2:0 = 001 - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); - writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); - delay(200); - -// Configure device for bias calculation - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source - writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP - delay(15); - -// Configure MPU6050 gyro and accelerometer for bias calculation - writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity - - uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec - uint16_t accelsensitivity = 16384; // = 16384 LSB/g - - // Configure FIFO to capture accelerometer and gyro data for bias calculation - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150) - delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes - -// At end of sample accumulation, turn off FIFO sensor read - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO - readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count - fifo_count = ((uint16_t)data[0] << 8) | data[1]; - packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging - - for (ii = 0; ii < packet_count; ii++) { - int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; - readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging - accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO - accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; - accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; - gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; - gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; - gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; - - accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases - accel_bias[1] += (int32_t) accel_temp[1]; - accel_bias[2] += (int32_t) accel_temp[2]; - gyro_bias[0] += (int32_t) gyro_temp[0]; - gyro_bias[1] += (int32_t) gyro_temp[1]; - gyro_bias[2] += (int32_t) gyro_temp[2]; - -} - accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases - accel_bias[1] /= (int32_t) packet_count; - accel_bias[2] /= (int32_t) packet_count; - gyro_bias[0] /= (int32_t) packet_count; - gyro_bias[1] /= (int32_t) packet_count; - gyro_bias[2] /= (int32_t) packet_count; - - if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation - else {accel_bias[2] += (int32_t) accelsensitivity;} - -// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup - 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 - data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases - data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; - data[3] = (-gyro_bias[1]/4) & 0xFF; - data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; - data[5] = (-gyro_bias[2]/4) & 0xFF; - -// Push gyro biases to hardware registers - writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]); - -// Output scaled gyro biases for display in the main program - dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; - dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; - dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; - -// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain -// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold -// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature -// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that -// the accelerometer biases calculated above must be divided by 8. - - int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases - readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values - accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); - accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); - accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - - uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers - uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis - - for(ii = 0; ii < 3; ii++) { - if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit - } - - // Construct total accelerometer bias, including calculated average accelerometer bias from above - accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) - accel_bias_reg[1] -= (accel_bias[1]/8); - accel_bias_reg[2] -= (accel_bias[2]/8); - - data[0] = (accel_bias_reg[0] >> 8) & 0xFF; - data[1] = (accel_bias_reg[0]) & 0xFF; - data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers - data[2] = (accel_bias_reg[1] >> 8) & 0xFF; - data[3] = (accel_bias_reg[1]) & 0xFF; - data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers - data[4] = (accel_bias_reg[2] >> 8) & 0xFF; - data[5] = (accel_bias_reg[2]) & 0xFF; - data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers - -// Apparently this is not working for the acceleration biases in the MPU-9250 -// Are we handling the temperature correction bit properly? -// Push accelerometer biases to hardware registers -/* -writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); -*/ -// Output scaled accelerometer biases for display in the main program - dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; - dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; - dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; -} - -// Accelerometer and gyroscope self test; check calibration wrt factory settings -void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass -{ - uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; - uint8_t selfTest[6]; - int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3]; - float factoryTrim[6]; - uint8_t FS = 0; - - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1< 0x7F) { - foo = ~((long)msbA << 16 | (long)csbA << 8 | (long)lsbA) + 1; // 2's complement the data - altitude = (float) (foo >> 8) + (float) ((lsbA >> 4)/16.0); // Whole number plus fraction altitude in meters for negative altitude - altitude *= -1.; - } - else { - altitude = (float) ( (msbA << 8) | csbA) + (float) ((lsbA >> 4)/16.0); // Whole number plus fraction altitude in meters - } - -// Calculate temperature, check for negative sign -if(msbT > 0x7F) { - foo = ~(msbT << 8 | lsbT) + 1 ; // 2's complement - temperature = (float) (foo >> 8) + (float)((lsbT >> 4)/16.0); // add whole and fractional degrees Centigrade - temperature *= -1.; - } - else { - temperature = (float) (msbT) + (float)((lsbT >> 4)/16.0); // add whole and fractional degrees Centigrade - } -} - -void MPL3115A2readPressure() -{ - uint8_t rawData[5]; // msb/csb/lsb pressure and msb/lsb temperature stored in five contiguous registers - -// We can read the data either by polling or interrupt; see data sheet for relative advantages -// First we try hardware interrupt, which should take less power, etc. -// while (digitalRead(int1Pin) == LOW); // Wait for interrupt pin int1Pin to go HIGH -// digitalWrite(int1Pin, LOW); // Reset interrupt pin int1Pin -// while((readByte(MPL3115A2_ADDRESS, MPL3115A2_INT_SOURCE) & 0x80) == 0); // Check that the interrupt source is a data ready interrupt -// or use a polling method -// Check data read status; if PTDR (bit 4) not set, then -// toggle OST bit to cause sensor to immediately take a reading -// Setting the one shot toggle is the way to get faster than 1 Hz data read rates - while ((readByte(MPL3115A2_ADDRESS, MPL3115A2_STATUS) & 0x08) == 0); MPL3115A2toggleOneShot(); - - readBytes(MPL3115A2_ADDRESS, MPL3115A2_OUT_P_MSB, 5, &rawData[0]); // Read the five raw data registers into data array - -// Pressure bytes - uint8_t msbP = rawData[0]; - uint8_t csbP = rawData[1]; - uint8_t lsbP = rawData[2]; -// Temperature bytes - uint8_t msbT = rawData[3]; - uint8_t lsbT = rawData[4]; - - long pressure_whole = ((long)msbP << 16 | (long)csbP << 8 | (long)lsbP) ; // Construct whole number pressure - pressure_whole >>= 6; // Only two most significant bits of lsbP contribute to whole pressure; its an 18-bit number - - lsbP &= 0x30; // Keep only bits 5 and 6, the fractional pressure - lsbP >>= 4; // Shift to get the fractional pressure in terms of quarters of a Pascal - float pressure_frac = (float) lsbP/4.0; // Convert numbers of fractional quarters to fractional pressure n Pasacl - - pressure = (float) (pressure_whole) + pressure_frac; // Combine whole and fractional parts to get entire pressure in Pascal - -// Calculate temperature, check for negative sign -long foo = 0; -if(msbT > 0x7F) { // Is the most significant bit a 1? Then its a negative number in two's complement form - foo = ~((long) msbT << 8 | lsbT) + 1 ; // 2's complement - temperature = (float) ((foo >> 8) + ((lsbT >> 4)/16.0)); // add whole and fractional degrees Centigrade - temperature *= -1.; - } - else { - temperature = (float) (msbT) + (float)((lsbT >> 4)/16.0); // add whole and fractional degrees Centigrade - } -} - -/* -===================================================================================================== -Define functions according to -"Data Manipulation and Basic Settings of the MPL3115A2 Command Line Interface Drive Code" -by Miguel Salhuana -Freescale Semiconductor Application Note AN4519 Rev 0.1, 08/2012 -===================================================================================================== -*/ -//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -// Clears then sets OST bit which causes the sensor to immediately take another reading -void MPL3115A2toggleOneShot() -{ - MPL3115A2Active(); // Set to active to start reading - uint8_t c = readByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG1); - writeByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG1, c & ~(1<<1)); // Clear OST (bit 1) - c = readByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG1); - writeByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG1, c | (1<<1)); // Set OST bit to 1 -} - -//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -// Set the Outputting Sample Rate -void MPL3115A2SampleRate(uint8_t samplerate) -{ - MPL3115A2Standby(); // Must be in standby to change registers - - uint8_t c = readByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG1); - writeByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG1, c & ~(0x38)); // Clear OSR bits 3,4,5 - if(samplerate < 8) { // OSR between 1 and 7 - writeByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG1, c | (samplerate << 3)); // Write OSR to bits 3,4,5 - } - - MPL3115A2Active(); // Set to active to start reading - } - -//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -// Initialize the MPL3115A2 registers for FIFO mode -void initFIFOMPL3115A2() -{ - // Clear all interrupts by reading the data output registers - uint8_t temp; - temp = readByte(MPL3115A2_ADDRESS, MPL3115A2_OUT_P_MSB); - temp = readByte(MPL3115A2_ADDRESS, MPL3115A2_OUT_P_CSB); - temp = readByte(MPL3115A2_ADDRESS, MPL3115A2_OUT_P_LSB); - temp = readByte(MPL3115A2_ADDRESS, MPL3115A2_OUT_T_MSB); - temp = readByte(MPL3115A2_ADDRESS, MPL3115A2_OUT_T_LSB); - temp = readByte(MPL3115A2_ADDRESS, MPL3115A2_F_STATUS); - - MPL3115A2Standby(); // Must be in standby to change registers - - // Set CTRL_REG4 register to configure interupt enable - // Enable data ready interrupt (bit 7), enable FIFO (bit 6), enable pressure window (bit 5), temperature window (bit 4), - // pressure threshold (bit 3), temperature threshold (bit 2), pressure change (bit 1) and temperature change (bit 0) - writeByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG4, 0x40); // enable FIFO - - // Configure INT 1 for data ready, all other (inc. FIFO) interrupts to INT2 - writeByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG5, 0x80); - - // Set CTRL_REG3 register to configure interupt signal type - // Active HIGH, push-pull interupts INT1 and INT 2 - writeByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG3, 0x22); - - // Set FIFO mode - writeByte(MPL3115A2_ADDRESS, MPL3115A2_F_SETUP, 0x00); // Clear FIFO mode -// In overflow mode, when FIFO fills up, no more data is taken until the FIFO registers are read -// In watermark mode, the oldest data is overwritten by new data until the FIFO registers are read - writeByte(MPL3115A2_ADDRESS, MPL3115A2_F_SETUP, 0x80); // Set F_MODE to interrupt when overflow = 32 reached -// writeByte(MPL3115A2_ADDRESS, MPL3115A2_F_SETUP, 0x60); // Set F_MODE to accept 32 data samples and interrupt when watermark = 32 reached - - MPL3115A2Active(); // Set to active to start reading -} - -//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -// Initialize the MPL3115A2 for realtime data collection -void initRealTimeMPL3115A2() -{ - // Clear all interrupts by reading the data output registers - uint8_t temp; - temp = readByte(MPL3115A2_ADDRESS, MPL3115A2_OUT_P_MSB); - temp = readByte(MPL3115A2_ADDRESS, MPL3115A2_OUT_P_CSB); - temp = readByte(MPL3115A2_ADDRESS, MPL3115A2_OUT_P_LSB); - temp = readByte(MPL3115A2_ADDRESS, MPL3115A2_OUT_T_MSB); - temp = readByte(MPL3115A2_ADDRESS, MPL3115A2_OUT_T_LSB); - temp = readByte(MPL3115A2_ADDRESS, MPL3115A2_F_STATUS); - - MPL3115A2Standby(); // Must be in standby to change registers - - // Set CTRL_REG4 register to configure interupt enable - // Enable data ready interrupt (bit 7), enable FIFO (bit 6), enable pressure window (bit 5), temperature window (bit 4), - // pressure threshold (bit 3), temperature threshold (bit 2), pressure change (bit 1) and temperature change (bit 0) - writeByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG4, 0x80); - - // Configure INT 1 for data ready, all other interrupts to INT2 - writeByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG5, 0x80); - - // Set CTRL_REG3 register to configure interupt signal type - // Active HIGH, push-pull interupts INT1 and INT 2 - writeByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG3, 0x22); - - // Set FIFO mode - writeByte(MPL3115A2_ADDRESS, MPL3115A2_F_SETUP, 0x00); // disable FIFO mode - - MPL3115A2Active(); // Set to active to start reading -} - -//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -// Set the Auto Acquisition Time Step -void MPL3115A2TimeStep(uint8_t ST_Value) -{ - MPL3115A2Standby(); // First put device in standby mode to allow write to registers - - uint8_t c = readByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG2); // Read contents of register CTRL_REG2 - if (ST_Value <= 0xF) { - writeByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG2, (c | ST_Value)); // Set time step n from 0x0 to 0xF (bits 0 - 3) for time intervals from 1 to 32768 (2^n) seconds - } - - MPL3115A2Active(); // Set to active to start reading -} - -//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -// Enable the pressure and temperature event flags - // Bit 2 is general data ready event mode on new Pressure/Altitude or temperature data - // Bit 1 is event flag on new Pressure/Altitude data - // Bit 0 is event flag on new Temperature data -void MPL3115A2enableEventflags() -{ - MPL3115A2Standby(); // Must be in standby to change registers - writeByte(MPL3115A2_ADDRESS, MPL3115A2_PT_DATA_CFG, 0x07); //Enable all three pressure and temperature event flags - MPL3115A2Active(); // Set to active to start reading -} - -//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -// Enter Active Altimeter mode -void MPL3115A2ActiveAltimeterMode() -{ - MPL3115A2Standby(); // First put device in standby mode to allow write to registers - uint8_t c = readByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG1); // Read contents of register CTRL_REG1 - writeByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG1, c | (0x80)); // Set ALT (bit 7) to 1 - MPL3115A2Active(); // Set to active to start reading -} - -//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -// Enter Active Barometer mode -void MPL3115A2ActiveBarometerMode() -{ - MPL3115A2Standby(); // First put device in standby mode to allow write to registers - uint8_t c = readByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG1); // Read contents of register CTRL_REG1 - writeByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG1, c & ~(0x80)); // Set ALT (bit 7) to 0 - MPL3115A2Active(); // Set to active to start reading -} - -//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -// Software resets the MPL3115A2. -// It must be in standby to change most register settings -void MPL3115A2Reset() -{ - writeByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG1, 0x04); // Set RST (bit 2) to 1 -} - -//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -// Sets the MPL3115A2 to standby mode. -// It must be in standby to change most register settings -void MPL3115A2Standby() -{ - uint8_t c = readByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG1); // Read contents of register CTRL_REG1 - writeByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG1, c & ~(0x01)); // Set SBYB (bit 0) to 0 -} - -//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -// Sets the MPL3115A2 to active mode. -// Needs to be in this mode to output data -void MPL3115A2Active() -{ - uint8_t c = readByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG1); // Read contents of register CTRL_REG1 - writeByte(MPL3115A2_ADDRESS, MPL3115A2_CTRL_REG1, c | 0x01); // Set SBYB (bit 0) to 1 -} - - void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) -{ - Wire.beginTransmission(address); // Initialize the Tx buffer - Wire.write(subAddress); // Put slave register address in Tx buffer - Wire.write(data); // Put data in Tx buffer - Wire.endTransmission(); // Send the Tx buffer -} - - uint8_t readByte(uint8_t address, uint8_t subAddress) -{ - uint8_t data; // `data` will store the register data - Wire.beginTransmission(address); // Initialize the Tx buffer - Wire.write(subAddress); // Put slave register address in Tx buffer - Wire.endTransmission(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive -// Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive -// Wire.requestFrom(address, 1); // Read one byte from slave register address - Wire.requestFrom(address, (size_t) 1); // Read one byte from slave register address - data = Wire.read(); // Fill Rx buffer with result - return data; // Return data read from slave register -} - - void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) -{ - Wire.beginTransmission(address); // Initialize the Tx buffer - Wire.write(subAddress); // Put slave register address in Tx buffer - Wire.endTransmission(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive -// Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive - uint8_t i = 0; -// Wire.requestFrom(address, count); // Read bytes from slave register address - Wire.requestFrom(address, (size_t) count); // Read bytes from slave register address - while (Wire.available()) { - dest[i++] = Wire.read(); } // Put read results in the Rx buffer -} - diff --git a/extras/MPU9250_MS5611_BasicAHRS_t3.ino b/extras/MPU9250_MS5611_BasicAHRS_t3.ino deleted file mode 100644 index 5f288dd..0000000 --- a/extras/MPU9250_MS5611_BasicAHRS_t3.ino +++ /dev/null @@ -1,1214 +0,0 @@ -/* MPU9250_MS5611_t3 Basic Example Code - by: Kris Winer - date: April 1, 2014 - license: Beerware - Use this code however you'd like. If you - find it useful you can buy me a beer some time. - - Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, - getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to - allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and - Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1. - - This sketch is intended specifically for the MPU9250MS5611 Add-on shield for the Teensy 3.1. - It uses SDA/SCL on pins 17/16, respectively, and it uses the Teensy 3.1-specific Wire library i2c_t3.h. - The MS5611 is a simple but high resolution pressure sensor, which can be used in its high resolution - mode but with power consumption od 20 microAmp, or in a lower resolution mode with power consumption of - only 1 microAmp. The choice will depend on the application. - - SDA and SCL should have external pull-up resistors (to 3.3V). - 10k resistors are on the EMSENSR-9250 breakout board. - - Hardware setup: - MPU9250 Breakout --------- Arduino - VDD ---------------------- 3.3V - VDDI --------------------- 3.3V - SDA ----------------------- A4 - SCL ----------------------- A5 - GND ---------------------- GND - - Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library. - 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. - We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file. - We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file. - */ -//#include "Wire.h" -#include -#include -//#include -//#include - -// Using NOKIA 5110 monochrome 84 x 48 pixel display -// pin 7 - Serial clock out (SCLK) -// pin 6 - Serial data out (DIN) -// pin 5 - Data/Command select (D/C) -// pin 3 - LCD chip select (SCE) -// pin 4 - LCD reset (RST) -//Adafruit_PCD8544 display = Adafruit_PCD8544(7, 6, 5, 3, 4); - -// See MS5611-02BA03 Low Voltage Barometric Pressure Sensor Data Sheet -#define MS5611_RESET 0x1E -#define MS5611_CONVERT_D1 0x40 -#define MS5611_CONVERT_D2 0x50 -#define MS5611_ADC_READ 0x00 - -// 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 -// above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map -// -//Magnetometer Registers -#define AK8963_ADDRESS 0x0C -#define WHO_AM_I_AK8963 0x00 // should return 0x48 -#define INFO 0x01 -#define AK8963_ST1 0x02 // data ready status bit 0 -#define AK8963_XOUT_L 0x03 // data -#define AK8963_XOUT_H 0x04 -#define AK8963_YOUT_L 0x05 -#define AK8963_YOUT_H 0x06 -#define AK8963_ZOUT_L 0x07 -#define AK8963_ZOUT_H 0x08 -#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 -#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 -#define AK8963_ASTC 0x0C // Self test control -#define AK8963_I2CDIS 0x0F // I2C disable -#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value -#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value -#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value - -#define SELF_TEST_X_GYRO 0x00 -#define SELF_TEST_Y_GYRO 0x01 -#define SELF_TEST_Z_GYRO 0x02 - -/*#define X_FINE_GAIN 0x03 // [7:0] fine gain -#define Y_FINE_GAIN 0x04 -#define Z_FINE_GAIN 0x05 -#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer -#define XA_OFFSET_L_TC 0x07 -#define YA_OFFSET_H 0x08 -#define YA_OFFSET_L_TC 0x09 -#define ZA_OFFSET_H 0x0A -#define ZA_OFFSET_L_TC 0x0B */ - -#define SELF_TEST_X_ACCEL 0x0D -#define SELF_TEST_Y_ACCEL 0x0E -#define SELF_TEST_Z_ACCEL 0x0F - -#define SELF_TEST_A 0x10 - -#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope -#define XG_OFFSET_L 0x14 -#define YG_OFFSET_H 0x15 -#define YG_OFFSET_L 0x16 -#define ZG_OFFSET_H 0x17 -#define ZG_OFFSET_L 0x18 -#define SMPLRT_DIV 0x19 -#define CONFIG 0x1A -#define GYRO_CONFIG 0x1B -#define ACCEL_CONFIG 0x1C -#define ACCEL_CONFIG2 0x1D -#define LP_ACCEL_ODR 0x1E -#define WOM_THR 0x1F - -#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms -#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] -#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms - -#define FIFO_EN 0x23 -#define I2C_MST_CTRL 0x24 -#define I2C_SLV0_ADDR 0x25 -#define I2C_SLV0_REG 0x26 -#define I2C_SLV0_CTRL 0x27 -#define I2C_SLV1_ADDR 0x28 -#define I2C_SLV1_REG 0x29 -#define I2C_SLV1_CTRL 0x2A -#define I2C_SLV2_ADDR 0x2B -#define I2C_SLV2_REG 0x2C -#define I2C_SLV2_CTRL 0x2D -#define I2C_SLV3_ADDR 0x2E -#define I2C_SLV3_REG 0x2F -#define I2C_SLV3_CTRL 0x30 -#define I2C_SLV4_ADDR 0x31 -#define I2C_SLV4_REG 0x32 -#define I2C_SLV4_DO 0x33 -#define I2C_SLV4_CTRL 0x34 -#define I2C_SLV4_DI 0x35 -#define I2C_MST_STATUS 0x36 -#define INT_PIN_CFG 0x37 -#define INT_ENABLE 0x38 -#define DMP_INT_STATUS 0x39 // Check DMP interrupt -#define INT_STATUS 0x3A -#define ACCEL_XOUT_H 0x3B -#define ACCEL_XOUT_L 0x3C -#define ACCEL_YOUT_H 0x3D -#define ACCEL_YOUT_L 0x3E -#define ACCEL_ZOUT_H 0x3F -#define ACCEL_ZOUT_L 0x40 -#define TEMP_OUT_H 0x41 -#define TEMP_OUT_L 0x42 -#define GYRO_XOUT_H 0x43 -#define GYRO_XOUT_L 0x44 -#define GYRO_YOUT_H 0x45 -#define GYRO_YOUT_L 0x46 -#define GYRO_ZOUT_H 0x47 -#define GYRO_ZOUT_L 0x48 -#define EXT_SENS_DATA_00 0x49 -#define EXT_SENS_DATA_01 0x4A -#define EXT_SENS_DATA_02 0x4B -#define EXT_SENS_DATA_03 0x4C -#define EXT_SENS_DATA_04 0x4D -#define EXT_SENS_DATA_05 0x4E -#define EXT_SENS_DATA_06 0x4F -#define EXT_SENS_DATA_07 0x50 -#define EXT_SENS_DATA_08 0x51 -#define EXT_SENS_DATA_09 0x52 -#define EXT_SENS_DATA_10 0x53 -#define EXT_SENS_DATA_11 0x54 -#define EXT_SENS_DATA_12 0x55 -#define EXT_SENS_DATA_13 0x56 -#define EXT_SENS_DATA_14 0x57 -#define EXT_SENS_DATA_15 0x58 -#define EXT_SENS_DATA_16 0x59 -#define EXT_SENS_DATA_17 0x5A -#define EXT_SENS_DATA_18 0x5B -#define EXT_SENS_DATA_19 0x5C -#define EXT_SENS_DATA_20 0x5D -#define EXT_SENS_DATA_21 0x5E -#define EXT_SENS_DATA_22 0x5F -#define EXT_SENS_DATA_23 0x60 -#define MOT_DETECT_STATUS 0x61 -#define I2C_SLV0_DO 0x63 -#define I2C_SLV1_DO 0x64 -#define I2C_SLV2_DO 0x65 -#define I2C_SLV3_DO 0x66 -#define I2C_MST_DELAY_CTRL 0x67 -#define SIGNAL_PATH_RESET 0x68 -#define MOT_DETECT_CTRL 0x69 -#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP -#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode -#define PWR_MGMT_2 0x6C -#define DMP_BANK 0x6D // Activates a specific bank in the DMP -#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank -#define DMP_REG 0x6F // Register in DMP from which to read or to which to write -#define DMP_REG_1 0x70 -#define DMP_REG_2 0x71 -#define FIFO_COUNTH 0x72 -#define FIFO_COUNTL 0x73 -#define FIFO_R_W 0x74 -#define WHO_AM_I_MPU9250 0x75 // Should return 0x71 -#define XA_OFFSET_H 0x77 -#define XA_OFFSET_L 0x78 -#define YA_OFFSET_H 0x7A -#define YA_OFFSET_L 0x7B -#define ZA_OFFSET_H 0x7D -#define ZA_OFFSET_L 0x7E - -// Using the MPU9250Teensy 3.1 Add-On shield, ADO is set to 0 -// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 -#define ADO 0 -#if ADO -#define MPU9250_ADDRESS 0x69 // Device address when ADO = 1 -#define AK8963_ADDRESS 0x0C // Address of magnetometer -#define MS5611_ADDRESS 0x76 // Address of altimeter -#else -#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0 -#define AK8963_ADDRESS 0x0C // Address of magnetometer -#define MS5611_ADDRESS 0x77 // Address of altimeter -#endif - -#define SerialDebug true // set to true to get Serial output for debugging - -// Set initial input parameters -enum Ascale { - AFS_2G = 0, - AFS_4G, - AFS_8G, - AFS_16G -}; - -enum Gscale { - GFS_250DPS = 0, - GFS_500DPS, - GFS_1000DPS, - GFS_2000DPS -}; - -enum Mscale { - MFS_14BITS = 0, // 0.6 mG per LSB - MFS_16BITS // 0.15 mG per LSB -}; - -#define ADC_256 0x00 // define pressure and temperature conversion rates -#define ADC_512 0x02 -#define ADC_1024 0x04 -#define ADC_2048 0x06 -#define ADC_4096 0x08 -#define ADC_D1 0x40 -#define ADC_D2 0x50 - -// Specify sensor full scale -uint8_t OSR = ADC_4096; // set pressure amd temperature oversample rate -uint8_t Gscale = GFS_250DPS; -uint8_t Ascale = AFS_2G; -uint8_t Mscale = MFS_16BITS; // Choose either 14-bit or 16-bit magnetometer resolution -uint8_t Mmode = 0x02; // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read -float aRes, gRes, mRes; // scale resolutions per LSB for the sensors - -// Pin definitions -int intPin = 15; // These can be changed, 2 and 3 are the Arduinos ext int pins -int myLed = 13; -int motorPin1 = 22; -int motorPin2 = 3; -int motorPin3 = 23; -int motorPin4 = 4; -int led1 = 18; -int led2 = 21; -int led3 = 5; -int led4 = 6; - -uint16_t Pcal[8]; // calibration constants from MS5611 PROM registers -unsigned char nCRC; // calculated check sum to ensure PROM integrity -uint32_t D1 = 0, D2 = 0; // raw MS5611 pressure and temperature data -double dT, OFFSET, SENS, T2, OFFSET2, SENS2; // First order and second order corrections for raw S5637 temperature and pressure data -int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output -int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output -int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output -float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0}; // Factory mag calibration and mag bias -float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer -int16_t tempCount; // temperature raw count output -float temperature; // Stores the MPU9250 gyro internal chip temperature in degrees Celsius -double Temperature, Pressure; // stores MS5611 pressures sensor pressure and temperature -float SelfTest[6]; // holds results of gyro and accelerometer self test - -// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System) -float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s) -float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) -// There is a tradeoff in the beta parameter between accuracy and response speed. -// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. -// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion. -// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car! -// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec -// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense; -// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. -// In any case, this is the free parameter in the Madgwick filtering and fusion scheme. -float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta -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 -#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 -#define Ki 0.0f - -uint32_t delt_t = 0, count = 0, sumCount = 0; // used to control display output rate -float pitch, yaw, roll; -float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes -uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval -uint32_t Now = 0; // used to calculate integration interval - -float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values -float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion -float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method - - -void setup() -{ -// Wire.begin(); -// TWBR = 12; // 400 kbit/sec I2C speed for Pro Mini - // Setup for Master mode, pins 16/17, external pullups, 400kHz for Teensy 3.1 - Wire.begin(I2C_MASTER, 0x00, I2C_PINS_16_17, I2C_PULLUP_EXT, I2C_RATE_400); - delay(4000); - Serial.begin(38400); - - // Set up the interrupt pin, its set as active high, push-pull - pinMode(intPin, INPUT); - digitalWrite(intPin, LOW); - pinMode(myLed, OUTPUT); - digitalWrite(myLed, HIGH); - pinMode(motorPin1, OUTPUT); - analogWrite(motorPin1, 10); - pinMode(motorPin2, OUTPUT); - analogWrite(motorPin2, 0); - pinMode(motorPin3, OUTPUT); - analogWrite(motorPin3, 0); - pinMode(motorPin4, OUTPUT); - analogWrite(motorPin4, 0); - pinMode(led1, OUTPUT); - digitalWrite(led1, LOW); - pinMode(led2, OUTPUT); - digitalWrite(led2, LOW); - pinMode(led3, OUTPUT); - digitalWrite(led3, LOW); - pinMode(led4, OUTPUT); - digitalWrite(led4, LOW); -/* - display.begin(); // Initialize the display - display.setContrast(40); // Set the contrast - -// Start device display with ID of sensor - display.clearDisplay(); - display.setTextSize(2); - display.setCursor(0,0); display.print("MPU9250"); - display.setTextSize(1); - display.setCursor(0, 20); display.print("9-DOF 16-bit"); - display.setCursor(0, 30); display.print("motion sensor"); - display.setCursor(20,40); display.print("60 ug LSB"); - display.display(); - delay(1000); - -// Set up for data display - display.setTextSize(1); // Set text size to normal, 2 is twice normal etc. - display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen - display.clearDisplay(); // clears the screen and buffer -*/ - - // Read the WHO_AM_I register, this is a good test of communication - Serial.println("MPU9250 9-axis motion sensor..."); - byte c = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 - Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x71, HEX); -/* display.setCursor(20,0); display.print("MPU9250"); - display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(c, HEX); - display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0x71, HEX); - display.display(); - delay(1000); - */ - - if (c == 0x71) // WHO_AM_I should always be 0x68 - { - Serial.println("MPU9250 is online..."); - - MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values - Serial.print("x-axis self test: acceleration trim within : "); Serial.print(SelfTest[0],1); Serial.println("% of factory value"); - Serial.print("y-axis self test: acceleration trim within : "); Serial.print(SelfTest[1],1); Serial.println("% of factory value"); - Serial.print("z-axis self test: acceleration trim within : "); Serial.print(SelfTest[2],1); Serial.println("% of factory value"); - Serial.print("x-axis self test: gyration trim within : "); Serial.print(SelfTest[3],1); Serial.println("% of factory value"); - Serial.print("y-axis self test: gyration trim within : "); Serial.print(SelfTest[4],1); Serial.println("% of factory value"); - Serial.print("z-axis self test: gyration trim within : "); Serial.print(SelfTest[5],1); Serial.println("% of factory value"); - delay(1000); - - calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers - /* display.clearDisplay(); - - display.setCursor(0, 0); display.print("MPU9250 bias"); - display.setCursor(0, 8); display.print(" x y z "); - - display.setCursor(0, 16); display.print((int)(1000*accelBias[0])); - display.setCursor(24, 16); display.print((int)(1000*accelBias[1])); - display.setCursor(48, 16); display.print((int)(1000*accelBias[2])); - display.setCursor(72, 16); display.print("mg"); - - display.setCursor(0, 24); display.print(gyroBias[0], 1); - display.setCursor(24, 24); display.print(gyroBias[1], 1); - display.setCursor(48, 24); display.print(gyroBias[2], 1); - display.setCursor(66, 24); display.print("o/s"); - - display.display(); - delay(1000); - */ - initMPU9250(); - Serial.println("MPU9250 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature - - // Read the WHO_AM_I register of the magnetometer, this is a good test of communication - byte d = readByte(AK8963_ADDRESS, WHO_AM_I_AK8963); // Read WHO_AM_I register for AK8963 - Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" I should be "); Serial.println(0x48, HEX); -/* display.clearDisplay(); - display.setCursor(20,0); display.print("AK8963"); - display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(d, HEX); - display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0x48, HEX); - display.display(); - delay(1000); - */ - // Get magnetometer calibration from AK8963 ROM - initAK8963(magCalibration); Serial.println("AK8963 initialized for active data mode...."); // Initialize device for active mode read of magnetometer - - if(SerialDebug) { -// Serial.println("Calibration values: "); - Serial.print("X-Axis sensitivity adjustment value "); Serial.println(magCalibration[0], 2); - Serial.print("Y-Axis sensitivity adjustment value "); Serial.println(magCalibration[1], 2); - Serial.print("Z-Axis sensitivity adjustment value "); Serial.println(magCalibration[2], 2); - } -/* - display.clearDisplay(); - display.setCursor(20,0); display.print("AK8963"); - display.setCursor(0,10); display.print("ASAX "); display.setCursor(50,10); display.print(magCalibration[0], 2); - display.setCursor(0,20); display.print("ASAY "); display.setCursor(50,20); display.print(magCalibration[1], 2); - display.setCursor(0,30); display.print("ASAZ "); display.setCursor(50,30); display.print(magCalibration[2], 2); - display.display(); - delay(1000); - */ - // Reset the MS5611 pressure sensor - MS5611Reset(); - delay(100); - Serial.println("MS5611 pressure sensor reset..."); - // Read PROM data from MS5611 pressure sensor - MS5611PromRead(Pcal); - Serial.println("PROM dta read:"); - Serial.print("C0 = "); Serial.println(Pcal[0]); - unsigned char refCRC = Pcal[0] >> 12; - Serial.print("C1 = "); Serial.println(Pcal[1]); - Serial.print("C2 = "); Serial.println(Pcal[2]); - Serial.print("C3 = "); Serial.println(Pcal[3]); - Serial.print("C4 = "); Serial.println(Pcal[4]); - Serial.print("C5 = "); Serial.println(Pcal[5]); - Serial.print("C6 = "); Serial.println(Pcal[6]); - - nCRC = MS5611checkCRC(Pcal); //calculate checksum to ensure integrity of MS5611 calibration data - Serial.print("Checksum = "); Serial.print(nCRC); Serial.print(" , should be "); Serial.println(refCRC); - -/* display.clearDisplay(); - display.setCursor(20,0); display.print("MS5611"); - display.setCursor(0,10); display.print("CRC is "); display.setCursor(50,10); display.print(nCRC); - display.setCursor(0,20); display.print("Should be "); display.setCursor(50,30); display.print(refCRC); - display.display(); - delay(1000); - */ - // get sensor resolutions, only need to do this once - getAres(); - getGres(); - getMres(); - magbias[0] = 0;//+470.; // User environmental x-axis correction in milliGauss, should be automatically calculated - magbias[1] = 0;//+120.; // User environmental x-axis correction in milliGauss - magbias[2] = 0;//+125.; // User environmental x-axis correction in milliGauss - - } - else - { - Serial.print("Could not connect to MPU9250: 0x"); - Serial.println(c, HEX); - while(1) ; // Loop forever if communication doesn't happen - } -} - -void loop() -{ - // If intPin goes high, all data registers have new data - if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt - // if (digitalRead(intPin)) { // On interrupt, read data - readAccelData(accelCount); // Read the x/y/z adc values - - // Now we'll calculate the accleration value into actual g's - ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set - ay = (float)accelCount[1]*aRes - accelBias[1]; - az = (float)accelCount[2]*aRes - accelBias[2]; - - readGyroData(gyroCount); // Read the x/y/z adc values - - // Calculate the gyro value into actual degrees per second - gx = (float)gyroCount[0]*gRes; // get actual gyro value, this depends on scale being set - gy = (float)gyroCount[1]*gRes; - gz = (float)gyroCount[2]*gRes; - - readMagData(magCount); // Read the x/y/z adc values - - // Calculate the magnetometer values in milliGauss - // Include factory calibration per data sheet and user environmental corrections - mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set - my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]; - mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; - } - - Now = micros(); - deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update - lastUpdate = Now; - - sum += deltat; // sum for averaging filter update rate - sumCount++; - - // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of the magnetometer; - // the magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro! - // We have to make some allowance for this orientationmismatch in feeding the output to the quaternion filter. - // For the MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like - // in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention. - // This is ok by aircraft orientation standards! - // Pass gyro rate as rad/s - MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); -// MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); - - // Serial print and/or display at 0.5 s rate independent of data rates - delt_t = millis() - count; - if (delt_t > 500) { // update LCD once per half-second independent of read rate - - if(SerialDebug) { - Serial.print("ax = "); Serial.print((int)1000*ax); - Serial.print(" ay = "); Serial.print((int)1000*ay); - Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg"); - Serial.print("gx = "); Serial.print( gx, 2); - Serial.print(" gy = "); Serial.print( gy, 2); - Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s"); - Serial.print("mx = "); Serial.print( (int)mx ); - Serial.print(" my = "); Serial.print( (int)my ); - Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG"); - - Serial.print("q0 = "); Serial.print(q[0]); - Serial.print(" qx = "); Serial.print(q[1]); - Serial.print(" qy = "); Serial.print(q[2]); - Serial.print(" qz = "); Serial.println(q[3]); - } - tempCount = readTempData(); // Read the gyro adc values - temperature = ((float) tempCount) / 333.87 + 21.0; // Gyro chip temperature in degrees Centigrade - // Print temperature in degrees Centigrade - Serial.print("Gyro temperature is "); Serial.print(temperature, 1); Serial.println(" degrees C"); // Print T values to tenths of s degree C - - D1 = MS5611Read(ADC_D1, OSR); // get raw pressure value - D2 = MS5611Read(ADC_D2, OSR); // get raw temperature value - dT = D2 - Pcal[5]*pow(2,8); // calculate temperature difference from reference - OFFSET = Pcal[2]*pow(2, 16) + dT*Pcal[4]/pow(2,7); - SENS = Pcal[1]*pow(2,15) + dT*Pcal[3]/pow(2,8); - - Temperature = (2000 + (dT*Pcal[6])/pow(2, 23))/100; // First-order Temperature in degrees Centigrade -// -// Second order corrections - if(Temperature > 20) - { - T2 = 5*dT*dT/pow(2, 38); // correction for high temperatures - OFFSET2 = 0; - SENS2 = 0; - } - if(Temperature < 20) // correction for low temperature - { - T2 = 3*dT*dT/pow(2, 33); - OFFSET2 = 61*(Temperature - 2000)*(Temperature - 2000)/16; - SENS2 = 29*(Temperature - 2000)*(Temperature - 2000)/16; - } - if(Temperature < -15) // correction for very low temperature - { - OFFSET2 = OFFSET2 + 17*(Temperature + 1500)*(Temperature + 1500); - SENS2 = SENS2 + 9*(Temperature + 1500)*(Temperature + 1500); - } - // End of second order corrections - // - Temperature = Temperature - T2; - OFFSET = OFFSET - OFFSET2; - SENS = SENS - SENS2; - - Pressure = (((D1*SENS)/pow(2, 21) - OFFSET)/pow(2, 15))/100; // Pressure in mbar or kPa - - const int station_elevation_m = 1050.0*0.3048; // Accurate for the roof on my house; convert from feet to meters - - float baroin = Pressure; // pressure is now in millibars - - // Formula to correct absolute pressure in millbars to "altimeter pressure" in inches of mercury - // comparable to weather report pressure - float part1 = baroin - 0.3; //Part 1 of formula - const float part2 = 0.0000842288; - float part3 = pow(part1, 0.190284); - float part4 = (float)station_elevation_m / part3; - float part5 = (1.0 + (part2 * part4)); - float part6 = pow(part5, 5.2553026); - float altimeter_setting_pressure_mb = part1 * part6; // Output is now in adjusted millibars - baroin = altimeter_setting_pressure_mb * 0.02953; - - float altitude = 145366.45*(1. - pow((Pressure/1013.25), 0.190284)); - - if(SerialDebug) { - Serial.print("Digital temperature value = "); Serial.print( (float)Temperature, 2); Serial.println(" C"); // temperature in degrees Celsius - Serial.print("Digital temperature value = "); Serial.print(9.*(float) Temperature/5. + 32., 2); Serial.println(" F"); // temperature in degrees Fahrenheit - Serial.print("Digital pressure value = "); Serial.print((float) Pressure, 2); Serial.println(" mbar");// pressure in millibar - Serial.print("Altitude = "); Serial.print(altitude, 2); Serial.println(" feet"); - } - - // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. - // In this coordinate system, the positive z-axis is down toward Earth. - // 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. - // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. - // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. - // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. - // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be - // applied in the correct order which for this configuration is yaw, pitch, and then roll. - // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. - 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]); - pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); - 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]); - pitch *= 180.0f / PI; - yaw *= 180.0f / PI; - yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 - roll *= 180.0f / PI; - - if(SerialDebug) { - Serial.print("Yaw, Pitch, Roll: "); - Serial.print(yaw, 2); - Serial.print(", "); - Serial.print(pitch, 2); - Serial.print(", "); - Serial.println(roll, 2); - - Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz"); - } -/* - display.clearDisplay(); - - display.setCursor(0, 0); display.print(" x y z "); - - display.setCursor(0, 8); display.print((int)(1000*ax)); - display.setCursor(24, 8); display.print((int)(1000*ay)); - display.setCursor(48, 8); display.print((int)(1000*az)); - display.setCursor(72, 8); display.print("mg"); - - display.setCursor(0, 16); display.print((int)(gx)); - display.setCursor(24, 16); display.print((int)(gy)); - display.setCursor(48, 16); display.print((int)(gz)); - display.setCursor(66, 16); display.print("o/s"); - - display.setCursor(0, 24); display.print((int)(mx)); - display.setCursor(24, 24); display.print((int)(my)); - display.setCursor(48, 24); display.print((int)(mz)); - display.setCursor(72, 24); display.print("mG"); - - display.setCursor(0, 32); display.print((int)(yaw)); - display.setCursor(24, 32); display.print((int)(pitch)); - display.setCursor(48, 32); display.print((int)(roll)); - display.setCursor(66, 32); display.print("ypr"); - */ - // With these settings the filter is updating at a ~145 Hz rate using the Madgwick scheme and - // >200 Hz using the Mahony scheme even though the display refreshes at only 2 Hz. - // The filter update rate is determined mostly by the mathematical steps in the respective algorithms, - // the processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR: - // an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces - // filter update rates of 36 - 145 and ~38 Hz for the Madgwick and Mahony schemes, respectively. - // This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads. - // This filter update rate should be fast enough to maintain accurate platform orientation for - // stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz - // produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors. - // The 3.3 V 8 MHz Pro Mini is doing pretty well! -/* display.setCursor(0, 40); display.print(altitude, 0); display.print("ft"); - display.setCursor(68, 0); display.print(9.*Temperature/5. + 32., 0); - display.setCursor(42, 40); display.print((float) sumCount / (1000.*sum), 2); display.print("kHz"); - display.display(); -*/ - - - digitalWrite(myLed, !digitalRead(myLed)); - count = millis(); - sumCount = 0; - sum = 0; - } - -} - -//=================================================================================================================== -//====== Set of useful function to access acceleration. gyroscope, magnetometer, and temperature data -//=================================================================================================================== - -void getMres() { - switch (Mscale) - { - // Possible magnetometer scales (and their register bit settings) are: - // 14 bit resolution (0) and 16 bit resolution (1) - case MFS_14BITS: - mRes = 10.*4912./8190.; // Proper scale to return milliGauss - break; - case MFS_16BITS: - mRes = 10.*4912./32760.0; // Proper scale to return milliGauss - break; - } -} - -void getGres() { - switch (Gscale) - { - // Possible gyro scales (and their register bit settings) are: - // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: - case GFS_250DPS: - gRes = 250.0/32768.0; - break; - case GFS_500DPS: - gRes = 500.0/32768.0; - break; - case GFS_1000DPS: - gRes = 1000.0/32768.0; - break; - case GFS_2000DPS: - gRes = 2000.0/32768.0; - break; - } -} - -void getAres() { - switch (Ascale) - { - // Possible accelerometer scales (and their register bit settings) are: - // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: - case AFS_2G: - aRes = 2.0/32768.0; - break; - case AFS_4G: - aRes = 4.0/32768.0; - break; - case AFS_8G: - aRes = 8.0/32768.0; - break; - case AFS_16G: - aRes = 16.0/32768.0; - break; - } -} - - -void readAccelData(int16_t * destination) -{ - uint8_t rawData[6]; // x/y/z accel register data stored here - readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array - destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; - destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; -} - - -void readGyroData(int16_t * destination) -{ - uint8_t rawData[6]; // x/y/z gyro register data stored here - readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array - destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; - destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; -} - -void readMagData(int16_t * destination) -{ - uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition - if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set - readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array - uint8_t c = rawData[6]; // End data read by reading ST2 register - if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data - destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian - destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; - } - } -} - -int16_t readTempData() -{ - uint8_t rawData[2]; // x/y/z gyro register data stored here - readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array - return ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a 16-bit value -} - -void initAK8963(float * destination) -{ - // First extract the factory calibration for each magnetometer axis - uint8_t rawData[3]; // x/y/z gyro calibration data stored here - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer - delay(10); - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode - delay(10); - readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values - destination[0] = (float)(rawData[0] - 128)/256. + 1.; // Return x-axis sensitivity adjustment values, etc. - destination[1] = (float)(rawData[1] - 128)/256. + 1.; - destination[2] = (float)(rawData[2] - 128)/256. + 1.; - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer - delay(10); - // Configure the magnetometer for continuous read and highest resolution - // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, - // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates - writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR - delay(10); -} - - -void initMPU9250() -{ - // wake up device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors - delay(100); // Wait for all registers to reset - - // get stable time source - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else - delay(200); - - // Configure Gyro and Thermometer - // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively; - // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot - // be higher than 1 / 0.0059 = 170 Hz - // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both - // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x03); - - // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; a rate consistent with the filter update rate - // determined inset in CONFIG above - - // Set gyroscope full scale range - // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 - uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value - // c = c & ~0xE0; // Clear self-test bits [7:5] - c = c & ~0x02; // Clear Fchoice bits [1:0] - c = c & ~0x18; // Clear AFS bits [4:3] - c = c | Gscale << 3; // Set full scale range for the gyro - // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register - - // Set accelerometer full-scale range configuration - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value - // c = c & ~0xE0; // Clear self-test bits [7:5] - c = c & ~0x18; // Clear AFS bits [4:3] - c = c | Ascale << 3; // Set full scale range for the accelerometer - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value - - // Set accelerometer sample rate configuration - // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for - // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value - c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) - c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value - - // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, - // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting - - // Configure Interrupts and Bypass Enable - // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared, - // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips - // can join the I2C bus and all can be controlled by the Arduino as master - writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22); - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt - delay(100); -} - - -// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average -// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. -void calibrateMPU9250(float * dest1, float * dest2) -{ - uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data - uint16_t ii, packet_count, fifo_count; - int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; - - // reset device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device - delay(100); - - // get stable time source; Auto select clock source to be PLL gyroscope reference if ready - // else use the internal oscillator, bits 2:0 = 001 - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); - writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); - delay(200); - -// Configure device for bias calculation - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source - writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP - delay(15); - -// Configure MPU6050 gyro and accelerometer for bias calculation - writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity - - uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec - uint16_t accelsensitivity = 16384; // = 16384 LSB/g - - // Configure FIFO to capture accelerometer and gyro data for bias calculation - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150) - delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes - -// At end of sample accumulation, turn off FIFO sensor read - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO - readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count - fifo_count = ((uint16_t)data[0] << 8) | data[1]; - packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging - - for (ii = 0; ii < packet_count; ii++) { - int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; - readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging - accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO - accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; - accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; - gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; - gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; - gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; - - accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases - accel_bias[1] += (int32_t) accel_temp[1]; - accel_bias[2] += (int32_t) accel_temp[2]; - gyro_bias[0] += (int32_t) gyro_temp[0]; - gyro_bias[1] += (int32_t) gyro_temp[1]; - gyro_bias[2] += (int32_t) gyro_temp[2]; - -} - accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases - accel_bias[1] /= (int32_t) packet_count; - accel_bias[2] /= (int32_t) packet_count; - gyro_bias[0] /= (int32_t) packet_count; - gyro_bias[1] /= (int32_t) packet_count; - gyro_bias[2] /= (int32_t) packet_count; - - if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation - else {accel_bias[2] += (int32_t) accelsensitivity;} - -// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup - 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 - data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases - data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; - data[3] = (-gyro_bias[1]/4) & 0xFF; - data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; - data[5] = (-gyro_bias[2]/4) & 0xFF; - -// Push gyro biases to hardware registers - writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]); - -// Output scaled gyro biases for display in the main program - dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; - dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; - dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; - -// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain -// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold -// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature -// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that -// the accelerometer biases calculated above must be divided by 8. - - int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases - readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values - accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); - accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); - accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - - uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers - uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis - - for(ii = 0; ii < 3; ii++) { - if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit - } - - // Construct total accelerometer bias, including calculated average accelerometer bias from above - accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) - accel_bias_reg[1] -= (accel_bias[1]/8); - accel_bias_reg[2] -= (accel_bias[2]/8); - - data[0] = (accel_bias_reg[0] >> 8) & 0xFF; - data[1] = (accel_bias_reg[0]) & 0xFF; - data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers - data[2] = (accel_bias_reg[1] >> 8) & 0xFF; - data[3] = (accel_bias_reg[1]) & 0xFF; - data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers - data[4] = (accel_bias_reg[2] >> 8) & 0xFF; - data[5] = (accel_bias_reg[2]) & 0xFF; - data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers - -// Apparently this is not working for the acceleration biases in the MPU-9250 -// Are we handling the temperature correction bit properly? -// Push accelerometer biases to hardware registers -/* writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); -*/ -// Output scaled accelerometer biases for display in the main program - dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; - dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; - dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; -} - -// Accelerometer and gyroscope self test; check calibration wrt factory settings -void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass -{ - uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; - uint8_t selfTest[6]; - int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3]; - float factoryTrim[6]; - uint8_t FS = 0; - - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<>1]) & 0x00FF); - else n_rem ^= (unsigned short) (n_prom[cnt>>1]>>8); - for(n_bit = 8; n_bit > 0; n_bit--) - { - if(n_rem & 0x8000) n_rem = (n_rem<<1) ^ 0x3000; - else n_rem = (n_rem<<1); - } - } - n_rem = ((n_rem>>12) & 0x000F); - return (n_rem ^ 0x00); -} - - -// I2C read/write functions for the MPU9250 and AK8963 sensors - - void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) -{ - Wire.beginTransmission(address); // Initialize the Tx buffer - Wire.write(subAddress); // Put slave register address in Tx buffer - Wire.write(data); // Put data in Tx buffer - Wire.endTransmission(); // Send the Tx buffer -} - - uint8_t readByte(uint8_t address, uint8_t subAddress) -{ - uint8_t data; // `data` will store the register data - Wire.beginTransmission(address); // Initialize the Tx buffer - Wire.write(subAddress); // Put slave register address in Tx buffer - Wire.endTransmission(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive -// Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive -// Wire.requestFrom(address, 1); // Read one byte from slave register address - Wire.requestFrom(address, (size_t) 1); // Read one byte from slave register address - data = Wire.read(); // Fill Rx buffer with result - return data; // Return data read from slave register -} - - void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) -{ - Wire.beginTransmission(address); // Initialize the Tx buffer - Wire.write(subAddress); // Put slave register address in Tx buffer - Wire.endTransmission(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive -// Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive - uint8_t i = 0; -// Wire.requestFrom(address, count); // Read bytes from slave register address - Wire.requestFrom(address, (size_t) count); // Read bytes from slave register address - while (Wire.available()) { - dest[i++] = Wire.read(); } // Put read results in the Rx buffer -} - diff --git a/extras/MPU9250_MS5637_AHRS_t3.ino b/extras/MPU9250_MS5637_AHRS_t3.ino deleted file mode 100644 index af83942..0000000 --- a/extras/MPU9250_MS5637_AHRS_t3.ino +++ /dev/null @@ -1,1297 +0,0 @@ -/* MPU9250_MS5637_t3 Basic Example Code - by: Kris Winer - date: April 1, 2014 - license: Beerware - Use this code however you'd like. If you - find it useful you can buy me a beer some time. - - Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, - getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to - allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and - Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1. - - This sketch is intended specifically for the MPU9250+MS5637 Add-on shield for the Teensy 3.1. - It uses SDA/SCL on pins 17/16, respectively, and it uses the Teensy 3.1-specific Wire library i2c_t3.h. - The MS5637 is a simple but high resolution pressure sensor, which can be used in its high resolution - mode but with power consumption of 20 microAmp, or in a lower resolution mode with power consumption of - only 1 microAmp. The choice will depend on the application. - - SDA and SCL should have external pull-up resistors (to 3.3V). - 4K7 resistors are on the MPU9250+MS5637 breakout board. - - Hardware setup: - MPU9250 Breakout --------- Arduino - VDD ---------------------- 3.3V - VDDI --------------------- 3.3V - SDA ----------------------- A4 - SCL ----------------------- A5 - GND ---------------------- GND - - Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library. - 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. - We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file. - We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file. - */ -//#include "Wire.h" -#include -#include -#include -#include - -// Using NOKIA 5110 monochrome 84 x 48 pixel display -// pin 7 - Serial clock out (SCLK) -// pin 6 - Serial data out (DIN) -// pin 5 - Data/Command select (D/C) -// pin 3 - LCD chip select (SCE) -// pin 4 - LCD reset (RST) -Adafruit_PCD8544 display = Adafruit_PCD8544(7, 6, 5, 3, 4); - -// See MS5637-02BA03 Low Voltage Barometric Pressure Sensor Data Sheet -#define MS5637_RESET 0x1E -#define MS5637_CONVERT_D1 0x40 -#define MS5637_CONVERT_D2 0x50 -#define MS5637_ADC_READ 0x00 - -// 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 -// above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map -// -//Magnetometer Registers -#define AK8963_ADDRESS 0x0C -#define WHO_AM_I_AK8963 0x00 // should return 0x48 -#define INFO 0x01 -#define AK8963_ST1 0x02 // data ready status bit 0 -#define AK8963_XOUT_L 0x03 // data -#define AK8963_XOUT_H 0x04 -#define AK8963_YOUT_L 0x05 -#define AK8963_YOUT_H 0x06 -#define AK8963_ZOUT_L 0x07 -#define AK8963_ZOUT_H 0x08 -#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 -#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 -#define AK8963_ASTC 0x0C // Self test control -#define AK8963_I2CDIS 0x0F // I2C disable -#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value -#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value -#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value - -#define SELF_TEST_X_GYRO 0x00 -#define SELF_TEST_Y_GYRO 0x01 -#define SELF_TEST_Z_GYRO 0x02 - -/*#define X_FINE_GAIN 0x03 // [7:0] fine gain -#define Y_FINE_GAIN 0x04 -#define Z_FINE_GAIN 0x05 -#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer -#define XA_OFFSET_L_TC 0x07 -#define YA_OFFSET_H 0x08 -#define YA_OFFSET_L_TC 0x09 -#define ZA_OFFSET_H 0x0A -#define ZA_OFFSET_L_TC 0x0B */ - -#define SELF_TEST_X_ACCEL 0x0D -#define SELF_TEST_Y_ACCEL 0x0E -#define SELF_TEST_Z_ACCEL 0x0F - -#define SELF_TEST_A 0x10 - -#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope -#define XG_OFFSET_L 0x14 -#define YG_OFFSET_H 0x15 -#define YG_OFFSET_L 0x16 -#define ZG_OFFSET_H 0x17 -#define ZG_OFFSET_L 0x18 -#define SMPLRT_DIV 0x19 -#define CONFIG 0x1A -#define GYRO_CONFIG 0x1B -#define ACCEL_CONFIG 0x1C -#define ACCEL_CONFIG2 0x1D -#define LP_ACCEL_ODR 0x1E -#define WOM_THR 0x1F - -#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms -#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] -#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms - -#define FIFO_EN 0x23 -#define I2C_MST_CTRL 0x24 -#define I2C_SLV0_ADDR 0x25 -#define I2C_SLV0_REG 0x26 -#define I2C_SLV0_CTRL 0x27 -#define I2C_SLV1_ADDR 0x28 -#define I2C_SLV1_REG 0x29 -#define I2C_SLV1_CTRL 0x2A -#define I2C_SLV2_ADDR 0x2B -#define I2C_SLV2_REG 0x2C -#define I2C_SLV2_CTRL 0x2D -#define I2C_SLV3_ADDR 0x2E -#define I2C_SLV3_REG 0x2F -#define I2C_SLV3_CTRL 0x30 -#define I2C_SLV4_ADDR 0x31 -#define I2C_SLV4_REG 0x32 -#define I2C_SLV4_DO 0x33 -#define I2C_SLV4_CTRL 0x34 -#define I2C_SLV4_DI 0x35 -#define I2C_MST_STATUS 0x36 -#define INT_PIN_CFG 0x37 -#define INT_ENABLE 0x38 -#define DMP_INT_STATUS 0x39 // Check DMP interrupt -#define INT_STATUS 0x3A -#define ACCEL_XOUT_H 0x3B -#define ACCEL_XOUT_L 0x3C -#define ACCEL_YOUT_H 0x3D -#define ACCEL_YOUT_L 0x3E -#define ACCEL_ZOUT_H 0x3F -#define ACCEL_ZOUT_L 0x40 -#define TEMP_OUT_H 0x41 -#define TEMP_OUT_L 0x42 -#define GYRO_XOUT_H 0x43 -#define GYRO_XOUT_L 0x44 -#define GYRO_YOUT_H 0x45 -#define GYRO_YOUT_L 0x46 -#define GYRO_ZOUT_H 0x47 -#define GYRO_ZOUT_L 0x48 -#define EXT_SENS_DATA_00 0x49 -#define EXT_SENS_DATA_01 0x4A -#define EXT_SENS_DATA_02 0x4B -#define EXT_SENS_DATA_03 0x4C -#define EXT_SENS_DATA_04 0x4D -#define EXT_SENS_DATA_05 0x4E -#define EXT_SENS_DATA_06 0x4F -#define EXT_SENS_DATA_07 0x50 -#define EXT_SENS_DATA_08 0x51 -#define EXT_SENS_DATA_09 0x52 -#define EXT_SENS_DATA_10 0x53 -#define EXT_SENS_DATA_11 0x54 -#define EXT_SENS_DATA_12 0x55 -#define EXT_SENS_DATA_13 0x56 -#define EXT_SENS_DATA_14 0x57 -#define EXT_SENS_DATA_15 0x58 -#define EXT_SENS_DATA_16 0x59 -#define EXT_SENS_DATA_17 0x5A -#define EXT_SENS_DATA_18 0x5B -#define EXT_SENS_DATA_19 0x5C -#define EXT_SENS_DATA_20 0x5D -#define EXT_SENS_DATA_21 0x5E -#define EXT_SENS_DATA_22 0x5F -#define EXT_SENS_DATA_23 0x60 -#define MOT_DETECT_STATUS 0x61 -#define I2C_SLV0_DO 0x63 -#define I2C_SLV1_DO 0x64 -#define I2C_SLV2_DO 0x65 -#define I2C_SLV3_DO 0x66 -#define I2C_MST_DELAY_CTRL 0x67 -#define SIGNAL_PATH_RESET 0x68 -#define MOT_DETECT_CTRL 0x69 -#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP -#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode -#define PWR_MGMT_2 0x6C -#define DMP_BANK 0x6D // Activates a specific bank in the DMP -#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank -#define DMP_REG 0x6F // Register in DMP from which to read or to which to write -#define DMP_REG_1 0x70 -#define DMP_REG_2 0x71 -#define FIFO_COUNTH 0x72 -#define FIFO_COUNTL 0x73 -#define FIFO_R_W 0x74 -#define WHO_AM_I_MPU9250 0x75 // Should return 0x71 -#define XA_OFFSET_H 0x77 -#define XA_OFFSET_L 0x78 -#define YA_OFFSET_H 0x7A -#define YA_OFFSET_L 0x7B -#define ZA_OFFSET_H 0x7D -#define ZA_OFFSET_L 0x7E - -// Using the MPU9250Teensy 3.1 Add-On shield, ADO is set to 0 -// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 -#define ADO 0 -#if ADO -#define MPU9250_ADDRESS 0x69 // Device address when ADO = 1 -#define AK8963_ADDRESS 0x0C // Address of magnetometer -#define MS5637_ADDRESS 0x76 // Address of altimeter -#else -#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0 -#define AK8963_ADDRESS 0x0C // Address of magnetometer -#define MS5637_ADDRESS 0x76 // Address of altimeter -#endif - -#define SerialDebug true // set to true to get Serial output for debugging - -// Set initial input parameters -enum Ascale { - AFS_2G = 0, - AFS_4G, - AFS_8G, - AFS_16G -}; - -enum Gscale { - GFS_250DPS = 0, - GFS_500DPS, - GFS_1000DPS, - GFS_2000DPS -}; - -enum Mscale { - MFS_14BITS = 0, // 0.6 mG per LSB - MFS_16BITS // 0.15 mG per LSB -}; - -#define ADC_256 0x00 // define pressure and temperature conversion rates -#define ADC_512 0x02 -#define ADC_1024 0x04 -#define ADC_2048 0x06 -#define ADC_4096 0x08 -#define ADC_8192 0x0A -#define ADC_D1 0x40 -#define ADC_D2 0x50 - -// Specify sensor full scale -uint8_t OSR = ADC_8192; // set pressure amd temperature oversample rate -uint8_t Gscale = GFS_250DPS; -uint8_t Ascale = AFS_2G; -uint8_t Mscale = MFS_16BITS; // Choose either 14-bit or 16-bit magnetometer resolution -uint8_t Mmode = 0x02; // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read -float aRes, gRes, mRes; // scale resolutions per LSB for the sensors - -// Pin definitions -int intPin = 8; // These can be changed, 2 and 3 are the Arduinos ext int pins -int myLed = 13; - -uint16_t Pcal[8]; // calibration constants from MS5637 PROM registers -unsigned char nCRC; // calculated check sum to ensure PROM integrity -uint32_t D1 = 0, D2 = 0; // raw MS5637 pressure and temperature data -double dT, OFFSET, SENS, T2, OFFSET2, SENS2; // First order and second order corrections for raw S5637 temperature and pressure data -int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output -int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output -int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output -float magCalibration[3] = {0, 0, 0}; // Factory mag calibration and mag bias -float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0}, magScale[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer -int16_t tempCount; // temperature raw count output -float temperature; // Stores the MPU9250 gyro internal chip temperature in degrees Celsius -double Temperature, Pressure; // stores MS5637 pressures sensor pressure and temperature -float SelfTest[6]; // holds results of gyro and accelerometer self test - -// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System) -float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s) -float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) -// There is a tradeoff in the beta parameter between accuracy and response speed. -// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. -// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion. -// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car! -// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec -// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense; -// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. -// In any case, this is the free parameter in the Madgwick filtering and fusion scheme. -float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta -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 -#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 -#define Ki 0.0f - -uint32_t delt_t = 0, count = 0, sumCount = 0; // used to control display output rate -float pitch, yaw, roll; -float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes -uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval -uint32_t Now = 0; // used to calculate integration interval - -float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values -float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion -float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method - - -void setup() -{ -// Wire.begin(); -// TWBR = 12; // 400 kbit/sec I2C speed for Pro Mini - // Setup for Master mode, pins 18/19, external pullups, 400kHz for Teensy 3.1 - Wire.begin(I2C_MASTER, 0x00, I2C_PINS_16_17, I2C_PULLUP_EXT, I2C_RATE_400); - delay(4000); - Serial.begin(38400); - - // Set up the interrupt pin, its set as active high, push-pull - pinMode(intPin, INPUT); - pinMode(myLed, OUTPUT); - digitalWrite(myLed, HIGH); - - display.begin(); // Initialize the display - display.setContrast(40); // Set the contrast - -// Start device display with ID of sensor - display.clearDisplay(); - display.setTextSize(2); - display.setCursor(0,0); display.print("MPU9250"); - display.setTextSize(1); - display.setCursor(0, 20); display.print("9-DOF 16-bit"); - display.setCursor(0, 30); display.print("motion sensor"); - display.setCursor(20,40); display.print("60 ug LSB"); - display.display(); - delay(1000); - -// Set up for data display - display.setTextSize(1); // Set text size to normal, 2 is twice normal etc. - display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen - display.clearDisplay(); // clears the screen and buffer - - I2Cscan();// look for I2C devices on the bus - - // Read the WHO_AM_I register, this is a good test of communication - Serial.println("MPU9250 9-axis motion sensor..."); - byte c = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 - Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x71, HEX); - display.setCursor(20,0); display.print("MPU9250"); - display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(c, HEX); - display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0x71, HEX); - display.display(); - delay(1000); - - if (c == 0x71) // WHO_AM_I should always be 0x68 - { - Serial.println("MPU9250 is online..."); - - MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values - Serial.print("x-axis self test: acceleration trim within : "); Serial.print(SelfTest[0],1); Serial.println("% of factory value"); - Serial.print("y-axis self test: acceleration trim within : "); Serial.print(SelfTest[1],1); Serial.println("% of factory value"); - Serial.print("z-axis self test: acceleration trim within : "); Serial.print(SelfTest[2],1); Serial.println("% of factory value"); - Serial.print("x-axis self test: gyration trim within : "); Serial.print(SelfTest[3],1); Serial.println("% of factory value"); - Serial.print("y-axis self test: gyration trim within : "); Serial.print(SelfTest[4],1); Serial.println("% of factory value"); - Serial.print("z-axis self test: gyration trim within : "); Serial.print(SelfTest[5],1); Serial.println("% of factory value"); - delay(1000); - - // get sensor resolutions, only need to do this once - getAres(); - getGres(); - getMres(); - - Serial.println(" Calibrate gyro and accel"); - accelgyrocalMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers - Serial.println("accel biases (mg)"); Serial.println(1000.*accelBias[0]); Serial.println(1000.*accelBias[1]); Serial.println(1000.*accelBias[2]); - Serial.println("gyro biases (dps)"); Serial.println(gyroBias[0]); Serial.println(gyroBias[1]); Serial.println(gyroBias[2]); - - display.clearDisplay(); - - display.setCursor(0, 0); display.print("MPU9250 bias"); - display.setCursor(0, 8); display.print(" x y z "); - - display.setCursor(0, 16); display.print((int)(1000*accelBias[0])); - display.setCursor(24, 16); display.print((int)(1000*accelBias[1])); - display.setCursor(48, 16); display.print((int)(1000*accelBias[2])); - display.setCursor(72, 16); display.print("mg"); - - display.setCursor(0, 24); display.print(gyroBias[0], 1); - display.setCursor(24, 24); display.print(gyroBias[1], 1); - display.setCursor(48, 24); display.print(gyroBias[2], 1); - display.setCursor(66, 24); display.print("o/s"); - - display.display(); - delay(1000); - - initMPU9250(); - Serial.println("MPU9250 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature - - // Read the WHO_AM_I register of the magnetometer, this is a good test of communication - byte d = readByte(AK8963_ADDRESS, WHO_AM_I_AK8963); // Read WHO_AM_I register for AK8963 - Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" I should be "); Serial.println(0x48, HEX); - display.clearDisplay(); - display.setCursor(20,0); display.print("AK8963"); - display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(d, HEX); - display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0x48, HEX); - display.display(); - delay(1000); - - // Get magnetometer calibration from AK8963 ROM - initAK8963(magCalibration); Serial.println("AK8963 initialized for active data mode...."); // Initialize device for active mode read of magnetometer - - magcalMPU9250(magBias, magScale); - Serial.println("AK8963 mag biases (mG)"); Serial.println(magBias[0]); Serial.println(magBias[1]); Serial.println(magBias[2]); - Serial.println("AK8963 mag scale (mG)"); Serial.println(magScale[0]); Serial.println(magScale[1]); Serial.println(magScale[2]); - delay(2000); // add delay to see results before serial spew of data - - if(SerialDebug) { -// Serial.println("Calibration values: "); - Serial.print("X-Axis sensitivity adjustment value "); Serial.println(magCalibration[0], 2); - Serial.print("Y-Axis sensitivity adjustment value "); Serial.println(magCalibration[1], 2); - Serial.print("Z-Axis sensitivity adjustment value "); Serial.println(magCalibration[2], 2); - } - - display.clearDisplay(); - display.setCursor(20,0); display.print("AK8963"); - display.setCursor(0,10); display.print("ASAX "); display.setCursor(50,10); display.print(magCalibration[0], 2); - display.setCursor(0,20); display.print("ASAY "); display.setCursor(50,20); display.print(magCalibration[1], 2); - display.setCursor(0,30); display.print("ASAZ "); display.setCursor(50,30); display.print(magCalibration[2], 2); - display.display(); - delay(1000); - - // Reset the MS5637 pressure sensor - MS5637Reset(); - delay(100); - Serial.println("MS5637 pressure sensor reset..."); - // Read PROM data from MS5637 pressure sensor - MS5637PromRead(Pcal); - Serial.println("PROM dta read:"); - Serial.print("C0 = "); Serial.println(Pcal[0]); - unsigned char refCRC = Pcal[0] >> 12; - Serial.print("C1 = "); Serial.println(Pcal[1]); - Serial.print("C2 = "); Serial.println(Pcal[2]); - Serial.print("C3 = "); Serial.println(Pcal[3]); - Serial.print("C4 = "); Serial.println(Pcal[4]); - Serial.print("C5 = "); Serial.println(Pcal[5]); - Serial.print("C6 = "); Serial.println(Pcal[6]); - - nCRC = MS5637checkCRC(Pcal); //calculate checksum to ensure integrity of MS5637 calibration data - Serial.print("Checksum = "); Serial.print(nCRC); Serial.print(" , should be "); Serial.println(refCRC); - - display.clearDisplay(); - display.setCursor(20,0); display.print("MS5637"); - display.setCursor(0,10); display.print("CRC is "); display.setCursor(50,10); display.print(nCRC); - display.setCursor(0,20); display.print("Should be "); display.setCursor(50,30); display.print(refCRC); - display.display(); - delay(1000); - - } - else - { - Serial.print("Could not connect to MPU9250: 0x"); - Serial.println(c, HEX); - while(1) ; // Loop forever if communication doesn't happen - } -} - -void loop() -{ - // If intPin goes high, all data registers have new data - if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt - // if (digitalRead(intPin)) { // On interrupt, read data - readAccelData(accelCount); // Read the x/y/z adc values - - // Now we'll calculate the accleration value into actual g's - ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set - ay = (float)accelCount[1]*aRes - accelBias[1]; - az = (float)accelCount[2]*aRes - accelBias[2]; - - readGyroData(gyroCount); // Read the x/y/z adc values - - // Calculate the gyro value into actual degrees per second - gx = (float)gyroCount[0]*gRes; // get actual gyro value, this depends on scale being set - gy = (float)gyroCount[1]*gRes; - gz = (float)gyroCount[2]*gRes; - - readMagData(magCount); // Read the x/y/z adc values - - // Calculate the magnetometer values in milliGauss - // Include factory calibration per data sheet and user environmental corrections - mx = (float)magCount[0]*mRes*magCalibration[0] - magBias[0]; // get actual magnetometer value, this depends on scale being set - my = (float)magCount[1]*mRes*magCalibration[1] - magBias[1]; - mz = (float)magCount[2]*mRes*magCalibration[2] - magBias[2]; - mx *= magScale[0]; - my *= magScale[1]; - mz *= magScale[2]; - } - - Now = micros(); - deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update - lastUpdate = Now; - - sum += deltat; // sum for averaging filter update rate - sumCount++; - - // Sensors x (y)-axis of the accelerometer/gyro is aligned with the y (x)-axis of the magnetometer; - // the magnetometer z-axis (+ down) is misaligned with z-axis (+ up) of accelerometer and gyro! - // We have to make some allowance for this orientation mismatch in feeding the output to the quaternion filter. - // We will assume that +y accel/gyro is North, then x accel/gyro is East. So if we want te quaternions properly aligned - // we need to feed into the madgwick function Ay, Ax, -Az, Gy, Gx, -Gz, Mx, My, and Mz. But because gravity is by convention - // positive down, we need to invert the accel data, so we pass -Ay, -Ax, Az, Gy, Gx, -Gz, Mx, My, and Mz into the Madgwick - // function to get North along the accel +y-axis, East along the accel +x-axis, and Down along the accel -z-axis. - // This orientation choice can be modified to allow any convenient (non-NED) orientation convention. - // This is ok by aircraft orientation standards! - // Pass gyro rate as rad/s - MadgwickQuaternionUpdate(-ay, -ax, az, gy*PI/180.0f, gx*PI/180.0f, -gz*PI/180.0f, mx, my, mz); -// if(passThru)MahonyQuaternionUpdate(-ay, -ax, az, gy*PI/180.0f, gx*PI/180.0f, -gz*PI/180.0f, mx, my, mz); - - // Serial print and/or display at 0.5 s rate independent of data rates - delt_t = millis() - count; - if (delt_t > 500) { // update LCD once per half-second independent of read rate - - if(SerialDebug) { - Serial.print("ax = "); Serial.print((int)1000*ax); - Serial.print(" ay = "); Serial.print((int)1000*ay); - Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg"); - Serial.print("gx = "); Serial.print( gx, 2); - Serial.print(" gy = "); Serial.print( gy, 2); - Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s"); - Serial.print("mx = "); Serial.print( (int)mx ); - Serial.print(" my = "); Serial.print( (int)my ); - Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG"); - - Serial.print("q0 = "); Serial.print(q[0]); - Serial.print(" qx = "); Serial.print(q[1]); - Serial.print(" qy = "); Serial.print(q[2]); - Serial.print(" qz = "); Serial.println(q[3]); - } - tempCount = readTempData(); // Read the gyro adc values - temperature = ((float) tempCount) / 333.87 + 21.0; // Gyro chip temperature in degrees Centigrade - // Print temperature in degrees Centigrade - Serial.print("Gyro temperature is "); Serial.print(temperature, 1); Serial.println(" degrees C"); // Print T values to tenths of s degree C - - D1 = MS5637Read(ADC_D1, OSR); // get raw pressure value - D2 = MS5637Read(ADC_D2, OSR); // get raw temperature value - dT = D2 - Pcal[5]*pow(2,8); // calculate temperature difference from reference - OFFSET = Pcal[2]*pow(2, 17) + dT*Pcal[4]/pow(2,6); - SENS = Pcal[1]*pow(2,16) + dT*Pcal[3]/pow(2,7); - - Temperature = (2000 + (dT*Pcal[6])/pow(2, 23))/100; // First-order Temperature in degrees Centigrade -// -// Second order corrections - if(Temperature > 20) - { - T2 = 5*dT*dT/pow(2, 38); // correction for high temperatures - OFFSET2 = 0; - SENS2 = 0; - } - if(Temperature < 20) // correction for low temperature - { - T2 = 3*dT*dT/pow(2, 33); - OFFSET2 = 61*(100*Temperature - 2000)*(100*Temperature - 2000)/16; - SENS2 = 29*(100*Temperature - 2000)*(100*Temperature - 2000)/16; - } - if(Temperature < -15) // correction for very low temperature - { - OFFSET2 = OFFSET2 + 17*(100*Temperature + 1500)*(100*Temperature + 1500); - SENS2 = SENS2 + 9*(100*Temperature + 1500)*(100*Temperature + 1500); - } - // End of second order corrections - // - Temperature = Temperature - T2/100; - OFFSET = OFFSET - OFFSET2; - SENS = SENS - SENS2; - - Pressure = (((D1*SENS)/pow(2, 21) - OFFSET)/pow(2, 15))/100; // Pressure in mbar or kPa - - const int station_elevation_m = 1050.0*0.3048; // Accurate for the roof on my house; convert from feet to meters - - float baroin = Pressure; // pressure is now in millibars - - // Formula to correct absolute pressure in millbars to "altimeter pressure" in inches of mercury - // comparable to weather report pressure - float part1 = baroin - 0.3; //Part 1 of formula - const float part2 = 0.0000842288; - float part3 = pow(part1, 0.190284); - float part4 = (float)station_elevation_m / part3; - float part5 = (1.0 + (part2 * part4)); - float part6 = pow(part5, 5.2553026); - float altimeter_setting_pressure_mb = part1 * part6; // Output is now in adjusted millibars - baroin = altimeter_setting_pressure_mb * 0.02953; - - float altitude = 145366.45*(1. - pow((Pressure/1013.25), 0.190284)); - - if(SerialDebug) { - Serial.print("Digital temperature value = "); Serial.print( (float)Temperature, 2); Serial.println(" C"); // temperature in degrees Celsius - Serial.print("Digital temperature value = "); Serial.print(9.*(float) Temperature/5. + 32., 2); Serial.println(" F"); // temperature in degrees Fahrenheit - Serial.print("Digital pressure value = "); Serial.print((float) Pressure, 2); Serial.println(" mbar");// pressure in millibar - Serial.print("Altitude = "); Serial.print(altitude, 2); Serial.println(" feet"); - } - - // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. - // In this coordinate system, the positive z-axis is down toward Earth. - // 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. - // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. - // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. - // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. - // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be - // applied in the correct order which for this configuration is yaw, pitch, and then roll. - // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. - 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]); - pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); - 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]); - pitch *= 180.0f / PI; - yaw *= 180.0f / PI; - yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 - roll *= 180.0f / PI; - - if(SerialDebug) { - Serial.print("Yaw, Pitch, Roll: "); - Serial.print(yaw, 2); - Serial.print(", "); - Serial.print(pitch, 2); - Serial.print(", "); - Serial.println(roll, 2); - - Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz"); - } - - display.clearDisplay(); - - display.setCursor(0, 0); display.print(" x y z "); - - display.setCursor(0, 8); display.print((int)(1000*ax)); - display.setCursor(24, 8); display.print((int)(1000*ay)); - display.setCursor(48, 8); display.print((int)(1000*az)); - display.setCursor(72, 8); display.print("mg"); - - display.setCursor(0, 16); display.print((int)(gx)); - display.setCursor(24, 16); display.print((int)(gy)); - display.setCursor(48, 16); display.print((int)(gz)); - display.setCursor(66, 16); display.print("o/s"); - - display.setCursor(0, 24); display.print((int)(mx)); - display.setCursor(24, 24); display.print((int)(my)); - display.setCursor(48, 24); display.print((int)(mz)); - display.setCursor(72, 24); display.print("mG"); - - display.setCursor(0, 32); display.print((int)(yaw)); - display.setCursor(24, 32); display.print((int)(pitch)); - display.setCursor(48, 32); display.print((int)(roll)); - display.setCursor(66, 32); display.print("ypr"); - - // With these settings the filter is updating at a ~145 Hz rate using the Madgwick scheme and - // >200 Hz using the Mahony scheme even though the display refreshes at only 2 Hz. - // The filter update rate is determined mostly by the mathematical steps in the respective algorithms, - // the processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR: - // an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces - // filter update rates of 36 - 145 and ~38 Hz for the Madgwick and Mahony schemes, respectively. - // This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads. - // This filter update rate should be fast enough to maintain accurate platform orientation for - // stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz - // produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors. - // The 3.3 V 8 MHz Pro Mini is doing pretty well! - display.setCursor(0, 40); display.print(altitude, 0); display.print("ft"); - display.setCursor(68, 0); display.print(9.*Temperature/5. + 32., 0); - display.setCursor(42, 40); display.print((float) sumCount / (1000.*sum), 2); display.print("kHz"); - display.display(); - - digitalWrite(myLed, !digitalRead(myLed)); - count = millis(); - sumCount = 0; - sum = 0; - } - -} - -//=================================================================================================================== -//====== Set of useful function to access acceleration. gyroscope, magnetometer, and temperature data -//=================================================================================================================== - -void getMres() { - switch (Mscale) - { - // Possible magnetometer scales (and their register bit settings) are: - // 14 bit resolution (0) and 16 bit resolution (1) - case MFS_14BITS: - mRes = 10.*4912./8190.; // Proper scale to return milliGauss - break; - case MFS_16BITS: - mRes = 10.*4912./32760.0; // Proper scale to return milliGauss - break; - } -} - -void getGres() { - switch (Gscale) - { - // Possible gyro scales (and their register bit settings) are: - // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: - case GFS_250DPS: - gRes = 250.0/32768.0; - break; - case GFS_500DPS: - gRes = 500.0/32768.0; - break; - case GFS_1000DPS: - gRes = 1000.0/32768.0; - break; - case GFS_2000DPS: - gRes = 2000.0/32768.0; - break; - } -} - -void getAres() { - switch (Ascale) - { - // Possible accelerometer scales (and their register bit settings) are: - // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: - case AFS_2G: - aRes = 2.0/32768.0; - break; - case AFS_4G: - aRes = 4.0/32768.0; - break; - case AFS_8G: - aRes = 8.0/32768.0; - break; - case AFS_16G: - aRes = 16.0/32768.0; - break; - } -} - - -void readAccelData(int16_t * destination) -{ - uint8_t rawData[6]; // x/y/z accel register data stored here - readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array - destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; - destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; -} - - -void readGyroData(int16_t * destination) -{ - uint8_t rawData[6]; // x/y/z gyro register data stored here - readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array - destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; - destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; -} - -void readMagData(int16_t * destination) -{ - uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition - if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set - readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array - uint8_t c = rawData[6]; // End data read by reading ST2 register - if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data - destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian - destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; - } - } -} - -int16_t readTempData() -{ - uint8_t rawData[2]; // x/y/z gyro register data stored here - readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array - return ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a 16-bit value -} - -void initAK8963(float * destination) -{ - // First extract the factory calibration for each magnetometer axis - uint8_t rawData[3]; // x/y/z gyro calibration data stored here - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer - delay(10); - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode - delay(10); - readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values - destination[0] = (float)(rawData[0] - 128)/256. + 1.; // Return x-axis sensitivity adjustment values, etc. - destination[1] = (float)(rawData[1] - 128)/256. + 1.; - destination[2] = (float)(rawData[2] - 128)/256. + 1.; - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer - delay(10); - // Configure the magnetometer for continuous read and highest resolution - // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, - // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates - writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR - delay(10); -} - - -void initMPU9250() -{ - // wake up device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors - delay(100); // Wait for all registers to reset - - // get stable time source - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else - delay(200); - - // Configure Gyro and Thermometer - // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively; - // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot - // be higher than 1 / 0.0059 = 170 Hz - // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both - // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x03); - - // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; a rate consistent with the filter update rate - // determined inset in CONFIG above - - // Set gyroscope full scale range - // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 - uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value - // c = c & ~0xE0; // Clear self-test bits [7:5] - c = c & ~0x02; // Clear Fchoice bits [1:0] - c = c & ~0x18; // Clear AFS bits [4:3] - c = c | Gscale << 3; // Set full scale range for the gyro - // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register - - // Set accelerometer full-scale range configuration - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value - // c = c & ~0xE0; // Clear self-test bits [7:5] - c = c & ~0x18; // Clear AFS bits [4:3] - c = c | Ascale << 3; // Set full scale range for the accelerometer - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value - - // Set accelerometer sample rate configuration - // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for - // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value - c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) - c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value - - // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, - // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting - - // Configure Interrupts and Bypass Enable - // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared, - // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips - // can join the I2C bus and all can be controlled by the Arduino as master - writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22); - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt - delay(100); -} - - -// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average -// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. -void accelgyrocalMPU9250(float * dest1, float * dest2) -{ - uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data - uint16_t ii, packet_count, fifo_count; - int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; - - // reset device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device - delay(100); - - // get stable time source; Auto select clock source to be PLL gyroscope reference if ready - // else use the internal oscillator, bits 2:0 = 001 - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); - writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); - delay(200); - -// Configure device for bias calculation - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source - writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP - delay(15); - -// Configure MPU6050 gyro and accelerometer for bias calculation - writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity - - uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec - uint16_t accelsensitivity = 16384; // = 16384 LSB/g - -// Configure FIFO to capture accelerometer and gyro data for bias calculation - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150) - delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes - -// At end of sample accumulation, turn off FIFO sensor read - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO - readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count - fifo_count = ((uint16_t)data[0] << 8) | data[1]; - packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging - - for (ii = 0; ii < packet_count; ii++) { - int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; - readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging - accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO - accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; - accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; - gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; - gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; - gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; - - accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases - accel_bias[1] += (int32_t) accel_temp[1]; - accel_bias[2] += (int32_t) accel_temp[2]; - gyro_bias[0] += (int32_t) gyro_temp[0]; - gyro_bias[1] += (int32_t) gyro_temp[1]; - gyro_bias[2] += (int32_t) gyro_temp[2]; - -} - accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases - accel_bias[1] /= (int32_t) packet_count; - accel_bias[2] /= (int32_t) packet_count; - gyro_bias[0] /= (int32_t) packet_count; - gyro_bias[1] /= (int32_t) packet_count; - gyro_bias[2] /= (int32_t) packet_count; - - if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation - else {accel_bias[2] += (int32_t) accelsensitivity;} - -// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup - 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 - data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases - data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; - data[3] = (-gyro_bias[1]/4) & 0xFF; - data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; - data[5] = (-gyro_bias[2]/4) & 0xFF; - -// Push gyro biases to hardware registers - writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]); - -// Output scaled gyro biases for display in the main program - dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; - dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; - dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; - -// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain -// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold -// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature -// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that -// the accelerometer biases calculated above must be divided by 8. - - int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases - readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values - accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); - accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); - accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - - uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers - uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis - - for(ii = 0; ii < 3; ii++) { - if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit - } - - // Construct total accelerometer bias, including calculated average accelerometer bias from above - accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) - accel_bias_reg[1] -= (accel_bias[1]/8); - accel_bias_reg[2] -= (accel_bias[2]/8); - - data[0] = (accel_bias_reg[0] >> 8) & 0xFF; - data[1] = (accel_bias_reg[0]) & 0xFF; - data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers - data[2] = (accel_bias_reg[1] >> 8) & 0xFF; - data[3] = (accel_bias_reg[1]) & 0xFF; - data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers - data[4] = (accel_bias_reg[2] >> 8) & 0xFF; - data[5] = (accel_bias_reg[2]) & 0xFF; - data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers - -// Apparently this is not working for the acceleration biases in the MPU-9250 -// Are we handling the temperature correction bit properly? -// Push accelerometer biases to hardware registers -/* writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); -*/ -// Output scaled accelerometer biases for display in the main program - dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; - dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; - dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; -} - - -void magcalMPU9250(float * dest1, float * dest2) -{ - uint16_t ii = 0, sample_count = 0; - int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0}; - int16_t mag_max[3] = {0x8000, 0x8000, 0x8000}, mag_min[3] = {0x7FFF, 0x7FFF, 0x7FFF}, mag_temp[3] = {0, 0, 0}; - - Serial.println("Mag Calibration: Wave device in a figure eight until done!"); - delay(4000); - - sample_count = 128; - for(ii = 0; ii < sample_count; ii++) { - readMagData(mag_temp); // Read the mag data - for (int jj = 0; jj < 3; jj++) { - if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj]; - if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj]; - } - if(Mmode == 0x02) delay(135); // at 8 Hz ODR, new mag data is available every 125 ms - if(Mmode == 0x06) delay(12); // at 100 Hz ODR, new mag data is available every 10 ms - } - -// Serial.println("mag x min/max:"); Serial.println(mag_max[0]); Serial.println(mag_min[0]); -// Serial.println("mag y min/max:"); Serial.println(mag_max[1]); Serial.println(mag_min[1]); -// Serial.println("mag z min/max:"); Serial.println(mag_max[2]); Serial.println(mag_min[2]); - - // Get hard iron correction - mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts - mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts - mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts - - dest1[0] = (float) mag_bias[0]*mRes*magCalibration[0]; // save mag biases in G for main program - dest1[1] = (float) mag_bias[1]*mRes*magCalibration[1]; - dest1[2] = (float) mag_bias[2]*mRes*magCalibration[2]; - - // Get soft iron correction estimate - mag_scale[0] = (mag_max[0] - mag_min[0])/2; // get average x axis max chord length in counts - mag_scale[1] = (mag_max[1] - mag_min[1])/2; // get average y axis max chord length in counts - mag_scale[2] = (mag_max[2] - mag_min[2])/2; // get average z axis max chord length in counts - - float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2]; - avg_rad /= 3.0; - - dest2[0] = avg_rad/((float)mag_scale[0]); - dest2[1] = avg_rad/((float)mag_scale[1]); - dest2[2] = avg_rad/((float)mag_scale[2]); - - Serial.println("Mag Calibration done!"); -} - - - -// Accelerometer and gyroscope self test; check calibration wrt factory settings -void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass -{ - uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; - uint8_t selfTest[6]; - int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3]; - float factoryTrim[6]; - uint8_t FS = 0; - - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<>1]) & 0x00FF); - else n_rem ^= (unsigned short) (n_prom[cnt>>1]>>8); - for(n_bit = 8; n_bit > 0; n_bit--) - { - if(n_rem & 0x8000) n_rem = (n_rem<<1) ^ 0x3000; - else n_rem = (n_rem<<1); - } - } - n_rem = ((n_rem>>12) & 0x000F); - return (n_rem ^ 0x00); -} - - -// I2C scan function - -void I2Cscan() -{ -// scan for i2c devices - byte error, address; - int nDevices; - - Serial.println("Scanning..."); - - nDevices = 0; - for(address = 1; address < 127; address++ ) - { - // The i2c_scanner uses the return value of - // the Write.endTransmisstion to see if - // a device did acknowledge to the address. - Wire.beginTransmission(address); - error = Wire.endTransmission(); - - if (error == 0) - { - Serial.print("I2C device found at address 0x"); - if (address<16) - Serial.print("0"); - Serial.print(address,HEX); - Serial.println(" !"); - - nDevices++; - } - else if (error==4) - { - Serial.print("Unknow error at address 0x"); - if (address<16) - Serial.print("0"); - Serial.println(address,HEX); - } - } - if (nDevices == 0) - Serial.println("No I2C devices found\n"); - else - Serial.println("done\n"); - -} - - -// I2C read/write functions for the MPU9250 and AK8963 sensors - - void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) -{ - Wire.beginTransmission(address); // Initialize the Tx buffer - Wire.write(subAddress); // Put slave register address in Tx buffer - Wire.write(data); // Put data in Tx buffer - Wire.endTransmission(); // Send the Tx buffer -} - - uint8_t readByte(uint8_t address, uint8_t subAddress) -{ - uint8_t data; // `data` will store the register data - Wire.beginTransmission(address); // Initialize the Tx buffer - Wire.write(subAddress); // Put slave register address in Tx buffer - Wire.endTransmission(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive -// Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive -// Wire.requestFrom(address, 1); // Read one byte from slave register address - Wire.requestFrom(address, (size_t) 1); // Read one byte from slave register address - data = Wire.read(); // Fill Rx buffer with result - return data; // Return data read from slave register -} - - void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) -{ - Wire.beginTransmission(address); // Initialize the Tx buffer - Wire.write(subAddress); // Put slave register address in Tx buffer - Wire.endTransmission(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive -// Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive - uint8_t i = 0; -// Wire.requestFrom(address, count); // Read bytes from slave register address - Wire.requestFrom(address, (size_t) count); // Read bytes from slave register address - while (Wire.available()) { - dest[i++] = Wire.read(); } // Put read results in the Rx buffer -} - diff --git a/extras/STM32F401/MPU9250.h b/extras/STM32F401/MPU9250.h deleted file mode 100644 index a78fd8b..0000000 --- a/extras/STM32F401/MPU9250.h +++ /dev/null @@ -1,879 +0,0 @@ -#ifndef MPU9250_H -#define MPU9250_H - -#include "mbed.h" -#include "math.h" - -// 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 -// above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map -// -//Magnetometer Registers -#define AK8963_ADDRESS 0x0C<<1 -#define WHO_AM_I_AK8963 0x00 // should return 0x48 -#define INFO 0x01 -#define AK8963_ST1 0x02 // data ready status bit 0 -#define AK8963_XOUT_L 0x03 // data -#define AK8963_XOUT_H 0x04 -#define AK8963_YOUT_L 0x05 -#define AK8963_YOUT_H 0x06 -#define AK8963_ZOUT_L 0x07 -#define AK8963_ZOUT_H 0x08 -#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 -#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 -#define AK8963_ASTC 0x0C // Self test control -#define AK8963_I2CDIS 0x0F // I2C disable -#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value -#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value -#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value - -#define SELF_TEST_X_GYRO 0x00 -#define SELF_TEST_Y_GYRO 0x01 -#define SELF_TEST_Z_GYRO 0x02 - -/*#define X_FINE_GAIN 0x03 // [7:0] fine gain -#define Y_FINE_GAIN 0x04 -#define Z_FINE_GAIN 0x05 -#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer -#define XA_OFFSET_L_TC 0x07 -#define YA_OFFSET_H 0x08 -#define YA_OFFSET_L_TC 0x09 -#define ZA_OFFSET_H 0x0A -#define ZA_OFFSET_L_TC 0x0B */ - -#define SELF_TEST_X_ACCEL 0x0D -#define SELF_TEST_Y_ACCEL 0x0E -#define SELF_TEST_Z_ACCEL 0x0F - -#define SELF_TEST_A 0x10 - -#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope -#define XG_OFFSET_L 0x14 -#define YG_OFFSET_H 0x15 -#define YG_OFFSET_L 0x16 -#define ZG_OFFSET_H 0x17 -#define ZG_OFFSET_L 0x18 -#define SMPLRT_DIV 0x19 -#define CONFIG 0x1A -#define GYRO_CONFIG 0x1B -#define ACCEL_CONFIG 0x1C -#define ACCEL_CONFIG2 0x1D -#define LP_ACCEL_ODR 0x1E -#define WOM_THR 0x1F - -#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms -#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] -#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms - -#define FIFO_EN 0x23 -#define I2C_MST_CTRL 0x24 -#define I2C_SLV0_ADDR 0x25 -#define I2C_SLV0_REG 0x26 -#define I2C_SLV0_CTRL 0x27 -#define I2C_SLV1_ADDR 0x28 -#define I2C_SLV1_REG 0x29 -#define I2C_SLV1_CTRL 0x2A -#define I2C_SLV2_ADDR 0x2B -#define I2C_SLV2_REG 0x2C -#define I2C_SLV2_CTRL 0x2D -#define I2C_SLV3_ADDR 0x2E -#define I2C_SLV3_REG 0x2F -#define I2C_SLV3_CTRL 0x30 -#define I2C_SLV4_ADDR 0x31 -#define I2C_SLV4_REG 0x32 -#define I2C_SLV4_DO 0x33 -#define I2C_SLV4_CTRL 0x34 -#define I2C_SLV4_DI 0x35 -#define I2C_MST_STATUS 0x36 -#define INT_PIN_CFG 0x37 -#define INT_ENABLE 0x38 -#define DMP_INT_STATUS 0x39 // Check DMP interrupt -#define INT_STATUS 0x3A -#define ACCEL_XOUT_H 0x3B -#define ACCEL_XOUT_L 0x3C -#define ACCEL_YOUT_H 0x3D -#define ACCEL_YOUT_L 0x3E -#define ACCEL_ZOUT_H 0x3F -#define ACCEL_ZOUT_L 0x40 -#define TEMP_OUT_H 0x41 -#define TEMP_OUT_L 0x42 -#define GYRO_XOUT_H 0x43 -#define GYRO_XOUT_L 0x44 -#define GYRO_YOUT_H 0x45 -#define GYRO_YOUT_L 0x46 -#define GYRO_ZOUT_H 0x47 -#define GYRO_ZOUT_L 0x48 -#define EXT_SENS_DATA_00 0x49 -#define EXT_SENS_DATA_01 0x4A -#define EXT_SENS_DATA_02 0x4B -#define EXT_SENS_DATA_03 0x4C -#define EXT_SENS_DATA_04 0x4D -#define EXT_SENS_DATA_05 0x4E -#define EXT_SENS_DATA_06 0x4F -#define EXT_SENS_DATA_07 0x50 -#define EXT_SENS_DATA_08 0x51 -#define EXT_SENS_DATA_09 0x52 -#define EXT_SENS_DATA_10 0x53 -#define EXT_SENS_DATA_11 0x54 -#define EXT_SENS_DATA_12 0x55 -#define EXT_SENS_DATA_13 0x56 -#define EXT_SENS_DATA_14 0x57 -#define EXT_SENS_DATA_15 0x58 -#define EXT_SENS_DATA_16 0x59 -#define EXT_SENS_DATA_17 0x5A -#define EXT_SENS_DATA_18 0x5B -#define EXT_SENS_DATA_19 0x5C -#define EXT_SENS_DATA_20 0x5D -#define EXT_SENS_DATA_21 0x5E -#define EXT_SENS_DATA_22 0x5F -#define EXT_SENS_DATA_23 0x60 -#define MOT_DETECT_STATUS 0x61 -#define I2C_SLV0_DO 0x63 -#define I2C_SLV1_DO 0x64 -#define I2C_SLV2_DO 0x65 -#define I2C_SLV3_DO 0x66 -#define I2C_MST_DELAY_CTRL 0x67 -#define SIGNAL_PATH_RESET 0x68 -#define MOT_DETECT_CTRL 0x69 -#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP -#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode -#define PWR_MGMT_2 0x6C -#define DMP_BANK 0x6D // Activates a specific bank in the DMP -#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank -#define DMP_REG 0x6F // Register in DMP from which to read or to which to write -#define DMP_REG_1 0x70 -#define DMP_REG_2 0x71 -#define FIFO_COUNTH 0x72 -#define FIFO_COUNTL 0x73 -#define FIFO_R_W 0x74 -#define WHO_AM_I_MPU9250 0x75 // Should return 0x71 -#define XA_OFFSET_H 0x77 -#define XA_OFFSET_L 0x78 -#define YA_OFFSET_H 0x7A -#define YA_OFFSET_L 0x7B -#define ZA_OFFSET_H 0x7D -#define ZA_OFFSET_L 0x7E - -// Using the MSENSR-9250 breakout board, ADO is set to 0 -// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 -//mbed uses the eight-bit device address, so shift seven-bit addresses left by one! -#define ADO 0 -#if ADO -#define MPU9250_ADDRESS 0x69<<1 // Device address when ADO = 1 -#else -#define MPU9250_ADDRESS 0x68<<1 // Device address when ADO = 0 -#endif - -// Set initial input parameters -enum Ascale { - AFS_2G = 0, - AFS_4G, - AFS_8G, - AFS_16G -}; - -enum Gscale { - GFS_250DPS = 0, - GFS_500DPS, - GFS_1000DPS, - GFS_2000DPS -}; - -enum Mscale { - MFS_14BITS = 0, // 0.6 mG per LSB - MFS_16BITS // 0.15 mG per LSB -}; - -uint8_t Ascale = AFS_2G; // AFS_2G, AFS_4G, AFS_8G, AFS_16G -uint8_t Gscale = GFS_250DPS; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS -uint8_t Mscale = MFS_16BITS; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution -uint8_t Mmode = 0x06; // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR -float aRes, gRes, mRes; // scale resolutions per LSB for the sensors - -//Set up I2C, (SDA,SCL) -I2C i2c(I2C_SDA, I2C_SCL); - -DigitalOut myled(LED1); - -// Pin definitions -int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins - -int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output -int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output -int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output -float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0}; // Factory mag calibration and mag bias -float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer -float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values -int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius -float temperature; -float SelfTest[6]; - -int delt_t = 0; // used to control display output rate -int count = 0; // used to control display output rate - -// parameters for 6 DoF sensor fusion calculations -float PI = 3.14159265358979323846f; -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 -float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta -float GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) -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 -#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 -#define Ki 0.0f - -float pitch, yaw, roll; -float deltat = 0.0f; // integration interval for both filter schemes -int lastUpdate = 0, firstUpdate = 0, Now = 0; // used to calculate integration interval // used to calculate integration interval -float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion -float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method - -class MPU9250 { - - protected: - - public: - //=================================================================================================================== -//====== Set of useful function to access acceleratio, gyroscope, and temperature data -//=================================================================================================================== - - void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) -{ - char data_write[2]; - data_write[0] = subAddress; - data_write[1] = data; - i2c.write(address, data_write, 2, 0); -} - - char readByte(uint8_t address, uint8_t subAddress) -{ - char data[1]; // `data` will store the register data - char data_write[1]; - data_write[0] = subAddress; - i2c.write(address, data_write, 1, 1); // no stop - i2c.read(address, data, 1, 0); - return data[0]; -} - - void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) -{ - char data[14]; - char data_write[1]; - data_write[0] = subAddress; - i2c.write(address, data_write, 1, 1); // no stop - i2c.read(address, data, count, 0); - for(int ii = 0; ii < count; ii++) { - dest[ii] = data[ii]; - } -} - - -void getMres() { - switch (Mscale) - { - // Possible magnetometer scales (and their register bit settings) are: - // 14 bit resolution (0) and 16 bit resolution (1) - case MFS_14BITS: - mRes = 10.0*4219.0/8190.0; // Proper scale to return milliGauss - break; - case MFS_16BITS: - mRes = 10.0*4219.0/32760.0; // Proper scale to return milliGauss - break; - } -} - - -void getGres() { - switch (Gscale) - { - // Possible gyro scales (and their register bit settings) are: - // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: - case GFS_250DPS: - gRes = 250.0/32768.0; - break; - case GFS_500DPS: - gRes = 500.0/32768.0; - break; - case GFS_1000DPS: - gRes = 1000.0/32768.0; - break; - case GFS_2000DPS: - gRes = 2000.0/32768.0; - break; - } -} - - -void getAres() { - switch (Ascale) - { - // Possible accelerometer scales (and their register bit settings) are: - // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: - case AFS_2G: - aRes = 2.0/32768.0; - break; - case AFS_4G: - aRes = 4.0/32768.0; - break; - case AFS_8G: - aRes = 8.0/32768.0; - break; - case AFS_16G: - aRes = 16.0/32768.0; - break; - } -} - - -void readAccelData(int16_t * destination) -{ - uint8_t rawData[6]; // x/y/z accel register data stored here - readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array - destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; - destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; -} - -void readGyroData(int16_t * destination) -{ - uint8_t rawData[6]; // x/y/z gyro register data stored here - readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array - destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; - destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; -} - -void readMagData(int16_t * destination) -{ - uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition - if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set - readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array - uint8_t c = rawData[6]; // End data read by reading ST2 register - if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data - destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value - destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian - destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ; - } - } -} - -int16_t readTempData() -{ - uint8_t rawData[2]; // x/y/z gyro register data stored here - readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array - return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]) ; // Turn the MSB and LSB into a 16-bit value -} - - -void resetMPU9250() { - // reset device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device - wait(0.1); - } - - void initAK8963(float * destination) -{ - // First extract the factory calibration for each magnetometer axis - uint8_t rawData[3]; // x/y/z gyro calibration data stored here - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer - wait(0.01); - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode - wait(0.01); - readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values - destination[0] = (float)(rawData[0] - 128)/256.0f + 1.0f; // Return x-axis sensitivity adjustment values, etc. - destination[1] = (float)(rawData[1] - 128)/256.0f + 1.0f; - destination[2] = (float)(rawData[2] - 128)/256.0f + 1.0f; - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer - wait(0.01); - // Configure the magnetometer for continuous read and highest resolution - // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, - // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates - writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR - wait(0.01); -} - - -void initMPU9250() -{ - // Initialize MPU9250 device - // wake up device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors - wait(0.1); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt - - // get stable time source - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 - - // Configure Gyro and Accelerometer - // Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively; - // DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both - // Maximum delay is 4.9 ms which is just over a 200 Hz maximum rate - writeByte(MPU9250_ADDRESS, CONFIG, 0x03); - - // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; the same rate set in CONFIG above - - // Set gyroscope full scale range - // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 - uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value - // c = c & ~0xE0; // Clear self-test bits [7:5] - c = c & ~0x02; // Clear Fchoice bits [1:0] - c = c & ~0x18; // Clear AFS bits [4:3] - c = c | Gscale << 3; // Set full scale range for the gyro - // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register - - // Set accelerometer full-scale range configuration - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value - // c = c & ~0xE0; // Clear self-test bits [7:5] - c = c & ~0x18; // Clear AFS bits [4:3] - c = c | Ascale << 3; // Set full scale range for the accelerometer - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value - - // Set accelerometer sample rate configuration - // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for - // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value - c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) - c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value - - // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, - // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting - - // Configure Interrupts and Bypass Enable - // Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips - // can join the I2C bus and all can be controlled by the Arduino as master - writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22); - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt -} - -// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average -// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. -void calibrateMPU9250(float * dest1, float * dest2) -{ - uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data - uint16_t ii, packet_count, fifo_count; - int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; - -// reset device, reset all registers, clear gyro and accelerometer bias registers - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device - wait(0.1); - -// get stable time source -// Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); - writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); - wait(0.2); - -// Configure device for bias calculation - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source - writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP - wait(0.015); - -// Configure MPU9250 gyro and accelerometer for bias calculation - writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity - - uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec - uint16_t accelsensitivity = 16384; // = 16384 LSB/g - -// Configure FIFO to capture accelerometer and gyro data for bias calculation - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250) - wait(0.04); // accumulate 40 samples in 80 milliseconds = 480 bytes - -// At end of sample accumulation, turn off FIFO sensor read - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO - readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count - fifo_count = ((uint16_t)data[0] << 8) | data[1]; - packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging - - for (ii = 0; ii < packet_count; ii++) { - int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; - readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging - accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO - accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; - accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; - gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; - gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; - gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; - - accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases - accel_bias[1] += (int32_t) accel_temp[1]; - accel_bias[2] += (int32_t) accel_temp[2]; - gyro_bias[0] += (int32_t) gyro_temp[0]; - gyro_bias[1] += (int32_t) gyro_temp[1]; - gyro_bias[2] += (int32_t) gyro_temp[2]; - -} - accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases - accel_bias[1] /= (int32_t) packet_count; - accel_bias[2] /= (int32_t) packet_count; - gyro_bias[0] /= (int32_t) packet_count; - gyro_bias[1] /= (int32_t) packet_count; - gyro_bias[2] /= (int32_t) packet_count; - - if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation - else {accel_bias[2] += (int32_t) accelsensitivity;} - -// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup - 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 - data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases - data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; - data[3] = (-gyro_bias[1]/4) & 0xFF; - data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; - data[5] = (-gyro_bias[2]/4) & 0xFF; - -/// Push gyro biases to hardware registers -/* writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]); -*/ - dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction - dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; - dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; - -// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain -// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold -// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature -// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that -// the accelerometer biases calculated above must be divided by 8. - - int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases - readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values - accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1]; - readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); - accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1]; - readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); - accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1]; - - uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers - uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis - - for(ii = 0; ii < 3; ii++) { - if(accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit - } - - // Construct total accelerometer bias, including calculated average accelerometer bias from above - accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) - accel_bias_reg[1] -= (accel_bias[1]/8); - accel_bias_reg[2] -= (accel_bias[2]/8); - - data[0] = (accel_bias_reg[0] >> 8) & 0xFF; - data[1] = (accel_bias_reg[0]) & 0xFF; - data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers - data[2] = (accel_bias_reg[1] >> 8) & 0xFF; - data[3] = (accel_bias_reg[1]) & 0xFF; - data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers - data[4] = (accel_bias_reg[2] >> 8) & 0xFF; - data[5] = (accel_bias_reg[2]) & 0xFF; - data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers - -// Apparently this is not working for the acceleration biases in the MPU-9250 -// Are we handling the temperature correction bit properly? -// Push accelerometer biases to hardware registers -/* writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); -*/ -// Output scaled accelerometer biases for manual subtraction in the main program - dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; - dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; - dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; -} - - -// Accelerometer and gyroscope self test; check calibration wrt factory settings -void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass -{ - uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; - uint8_t selfTest[6]; - int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3]; - float factoryTrim[6]; - uint8_t FS = 0; - - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1< 0.0f) - { - eInt[0] += ex; // accumulate integral error - eInt[1] += ey; - eInt[2] += ez; - } - else - { - eInt[0] = 0.0f; // prevent integral wind up - eInt[1] = 0.0f; - eInt[2] = 0.0f; - } - - // Apply feedback terms - gx = gx + Kp * ex + Ki * eInt[0]; - gy = gy + Kp * ey + Ki * eInt[1]; - gz = gz + Kp * ez + Ki * eInt[2]; - - // Integrate rate of change of quaternion - pa = q2; - pb = q3; - pc = q4; - q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat); - q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat); - q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat); - q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat); - - // Normalise quaternion - norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); - norm = 1.0f / norm; - q[0] = q1 * norm; - q[1] = q2 * norm; - q[2] = q3 * norm; - q[3] = q4 * norm; - - } - }; -#endif diff --git a/extras/STM32F401/Readme.md b/extras/STM32F401/Readme.md deleted file mode 100644 index 6381e10..0000000 --- a/extras/STM32F401/Readme.md +++ /dev/null @@ -1,3 +0,0 @@ -This is a translation of the Arduino sketch located in the MPU-9250 repository for the MPU-9250 9-axis motion sensor with 9 DoF sensor fusion using open-source Madgwick and Mahony algorithms and an STM32F401 M4 Cortex microprocessor. - -The programs were compiled using the mbed compiler and achieve sensor fusion filter update rates of 4870 Hz operating the STM32F401 at 84 MHz. diff --git a/extras/STM32F401/main.cpp b/extras/STM32F401/main.cpp deleted file mode 100644 index 306e6f7..0000000 --- a/extras/STM32F401/main.cpp +++ /dev/null @@ -1,228 +0,0 @@ -/* MPU9250 Basic Example Code - by: Kris Winer - date: April 1, 2014 - license: Beerware - Use this code however you'd like. If you - find it useful you can buy me a beer some time. - - Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, - getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to - allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and - Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1. - - SDA and SCL should have external pull-up resistors (to 3.3V). - 10k resistors are on the EMSENSR-9250 breakout board. - - Hardware setup: - MPU9250 Breakout --------- Arduino - VDD ---------------------- 3.3V - VDDI --------------------- 3.3V - SDA ----------------------- A4 - SCL ----------------------- A5 - GND ---------------------- GND - - Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library. - 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. - We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file. - We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file. - */ - -//#include "ST_F401_84MHZ.h" -//F401_init84 myinit(0); -#include "mbed.h" -#include "MPU9250.h" -#include "N5110.h" - -// Using NOKIA 5110 monochrome 84 x 48 pixel display -// pin 9 - Serial clock out (SCLK) -// pin 8 - Serial data out (DIN) -// pin 7 - Data/Command select (D/C) -// pin 5 - LCD chip select (CS) -// pin 6 - LCD reset (RST) -//Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); - -float sum = 0; -uint32_t sumCount = 0; - - MPU9250 mpu9250; - - Timer t; - - Serial pc(USBTX, USBRX); // tx, rx - - // VCC, SCE, RST, D/C, MOSI,S CLK, LED - N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7); - - - -int main() -{ - pc.baud(9600); - - //Set up I2C - i2c.frequency(400000); // use fast (400 kHz) I2C - - pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock); - - t.start(); - - lcd.init(); - lcd.setBrightness(0.05); - - - // Read the WHO_AM_I register, this is a good test of communication - uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 - pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r"); - - if (whoami == 0x71) // WHO_AM_I should always be 0x68 - { - pc.printf("MPU9250 is online...\n\r"); - wait(1); - lcd.clear(); - lcd.printString("MPU9250 OK", 0, 0); - - mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration - mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers - pc.printf("x gyro bias = %f\n\r", gyroBias[0]); - pc.printf("y gyro bias = %f\n\r", gyroBias[1]); - pc.printf("z gyro bias = %f\n\r", gyroBias[2]); - pc.printf("x accel bias = %f\n\r", accelBias[0]); - pc.printf("y accel bias = %f\n\r", accelBias[1]); - pc.printf("z accel bias = %f\n\r", accelBias[2]); - wait(2); - mpu9250.initMPU9250(); - pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature - mpu9250.initAK8963(magCalibration); - pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer - pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1< 10000000.0f) { -// beta = 0.04; // decrease filter gain after stabilized -// zeta = 0.015; // increasey bias drift gain after stabilized - // } - - // Pass gyro rate as rad/s - mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); - // mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); - - // Serial print and/or display at 0.5 s rate independent of data rates - delt_t = t.read_ms() - count; - if (delt_t > 500) { // update LCD once per half-second independent of read rate - - pc.printf("ax = %f", 1000*ax); - pc.printf(" ay = %f", 1000*ay); - pc.printf(" az = %f mg\n\r", 1000*az); - - pc.printf("gx = %f", gx); - pc.printf(" gy = %f", gy); - pc.printf(" gz = %f deg/s\n\r", gz); - - pc.printf("gx = %f", mx); - pc.printf(" gy = %f", my); - pc.printf(" gz = %f mG\n\r", mz); - - tempCount = mpu9250.readTempData(); // Read the adc values - temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade - pc.printf(" temperature = %f C\n\r", temperature); - - pc.printf("q0 = %f\n\r", q[0]); - pc.printf("q1 = %f\n\r", q[1]); - pc.printf("q2 = %f\n\r", q[2]); - pc.printf("q3 = %f\n\r", q[3]); - - lcd.clear(); - lcd.printString("MPU9250", 0, 0); - lcd.printString("x y z", 0, 1); - lcd.setXYAddress(0, 2); lcd.printChar((char)(1000*ax)); - lcd.setXYAddress(20, 2); lcd.printChar((char)(1000*ay)); - lcd.setXYAddress(40, 2); lcd.printChar((char)(1000*az)); lcd.printString("mg", 66, 2); - - - // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. - // In this coordinate system, the positive z-axis is down toward Earth. - // 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. - // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. - // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. - // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. - // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be - // applied in the correct order which for this configuration is yaw, pitch, and then roll. - // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. - 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]); - pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); - 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]); - pitch *= 180.0f / PI; - yaw *= 180.0f / PI; - yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 - roll *= 180.0f / PI; - - pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll); - pc.printf("average rate = %f\n\r", (float) sumCount/sum); - - myled= !myled; - count = t.read_ms(); - sum = 0; - sumCount = 0; -} -} - - } diff --git a/library.properties b/library.properties index d213c83..a97c9c5 100644 --- a/library.properties +++ b/library.properties @@ -2,8 +2,8 @@ name=SparkFun MPU-9250 9 DOF IMU Breakout version=1.0.0 author=SparkFun Electronics maintainer=SparkFun Electronics -sentence=Driver for ST's LSM303C 6-DOF IMU (3-axis accelerometer & 3-axis magnetometer) -paragraph=The LSM303C is a system-in-package featuring a 3D digital linear acceleration sensor and a 3D digital magnetic sensor. The LSM303C has linear acceleration full scales of ±2 g / ±4 g / ±8 g and a magnetic field full scale of ±16 gauss. The LSM303C includes an I2C serial bus interface that supports standard and fast mode (100 kHz and 400 kHz) and a half-duplex subset of the SPI serial interface. +sentence=Driver for InvenSense's MPU-9250 9-DOF IMU (3-axis gyroscope, 3-axis accelerometer & 3-axis magnetometer) +paragraph=The MPU-9250 is a system-in-package featuring acceleration full-scales of ±2 / ±4 / ±8 / ±16 (g), rotational full-scales of ±250 / ±500 / ±1000 / ±2000 (°/sec) and a magnetic field full scale of ±4800 µT. The MPU-9250 includes an I2C serial bus interface that supports speeds up to 400 kHz. category=Sensors url=https://github.com/sparkfun/MPU-9250_Breakout architectures=* From 5ad429e75f0d4e84c22928844722efad6bc7d862 Mon Sep 17 00:00:00 2001 From: Brent Wilkins Date: Tue, 26 Jul 2016 16:33:03 -0600 Subject: [PATCH 09/28] Updated README --- README.md | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index bd82252..8e7c321 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,9 @@ MPU-9250 9 DOF IMU Arduino Library ================================== -![MPU-9250 Breakout](https://cdn.sparkfun.com/r/600-600/assets/learn_tutorials/4/1/8/LSM303C_BOB.jpg) +![MPU-9250 Breakout](https://cdn.sparkfun.com/assets/parts/1/1/3/0/6/13762-00.jpg)](https://cdn.sparkfun.com/assets/parts/1/1/3/0/6/13762-00.jpg) -[*MPU-9250 Breakout (BOB-13303)*](https://www.sparkfun.com/products/13303) +[*MPU-9250 Breakout (SEN-13762)*](https://www.sparkfun.com/products/13762) This is an arduino IDE library to control the MPU-9250. @@ -12,7 +12,7 @@ This has been tested with Arduino Pro Mini. Repository Contents ------------------- -* **/examples** — Example sketches for the library (.ino). Run these from the Arduino IDE. +* **/examples** — Example sketch for the library (.ino). Run this from the Arduino IDE. * **/src** — Source files for the library (.cpp, .h). * **keywords.txt** — Keywords from this library that will be highlighted in the Arduino IDE. * **library.properties** — General library properties for the Arduino package manager. @@ -26,13 +26,15 @@ Documentation -------------- * **[Installing an Arduino Library Guide](https://learn.sparkfun.com/tutorials/installing-an-arduino-library)** — Basic information on how to install an Arduino library. -* **[Product Repository](https://github.com/sparkfun/MPU-9250_6_DOF_IMU_Breakout)** — Main repository (including hardware files) for the MPU-9250 Breakout. +* **[Product Repository](https://github.com/sparkfun/MPU-9250_Breakout)** — Main repository (including hardware files) for the MPU-9250 Breakout. +* **[Datasheet](https://cdn.sparkfun.com/assets/learn_tutorials/5/5/0/MPU9250REV1.0.pdf)** — Datasheet containing part of the product documentation. +* **[Register map](https://cdn.sparkfun.com/assets/learn_tutorials/5/5/0/MPU-9250-Register-Map.pdf)** — Register map containing the rest of the product documentation. * **[Hookup Guide](https://learn.sparkfun.com/tutorials/MPU-9250-hookup-guide)** — Basic hookup guide for the MPU-9250 Breakout. Products that use this Library --------------------------------- -* [BOB-13303](https://www.sparkfun.com/products/13339) — MPU-9250 Breakout board +* [SEN-13762](https://www.sparkfun.com/products/13762) — MPU-9250 Breakout board Version History --------------- From 5543784164aa47a83951e0420f334a7cc7ba1d59 Mon Sep 17 00:00:00 2001 From: Brent Wilkins Date: Fri, 2 Sep 2016 08:55:46 -0600 Subject: [PATCH 10/28] added missing myIMUs --- .../MPU9250BasicAHRS/MPU9250BasicAHRS.ino | 81 ++++++++++++++----- 1 file changed, 59 insertions(+), 22 deletions(-) diff --git a/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino b/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino index dd4e643..27792e2 100644 --- a/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino +++ b/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino @@ -125,9 +125,9 @@ void setup() display.setCursor(0, 0); display.print("MPU9250 bias"); display.setCursor(0, 8); display.print(" x y z "); - display.setCursor(0, 16); display.print((int)(1000*accelBias[0])); - display.setCursor(24, 16); display.print((int)(1000*accelBias[1])); - display.setCursor(48, 16); display.print((int)(1000*accelBias[2])); + display.setCursor(0, 16); display.print((int)(1000*myIMU.accelBias[0])); + display.setCursor(24, 16); display.print((int)(1000*myIMU.accelBias[1])); + display.setCursor(48, 16); display.print((int)(1000*myIMU.accelBias[2])); display.setCursor(72, 16); display.print("mg"); display.setCursor(0, 24); display.print(myIMU.gyroBias[0], 1); @@ -162,29 +162,62 @@ void setup() #endif // LCD // Get magnetometer calibration from AK8963 ROM - myIMU.initAK8963(myIMU.magCalibration); + myIMU.initAK8963(myIMU.factoryMagCalibration); // Initialize device for active mode read of magnetometer Serial.println("AK8963 initialized for active data mode...."); if (SerialDebug) { // Serial.println("Calibration values: "); Serial.print("X-Axis sensitivity adjustment value "); - Serial.println(myIMU.magCalibration[0], 2); + Serial.println(myIMU.factoryMagCalibration[0], 2); Serial.print("Y-Axis sensitivity adjustment value "); - Serial.println(myIMU.magCalibration[1], 2); + Serial.println(myIMU.factoryMagCalibration[1], 2); Serial.print("Z-Axis sensitivity adjustment value "); - Serial.println(myIMU.magCalibration[2], 2); + Serial.println(myIMU.factoryMagCalibration[2], 2); } #ifdef LCD display.clearDisplay(); display.setCursor(20,0); display.print("AK8963"); display.setCursor(0,10); display.print("ASAX "); display.setCursor(50,10); - display.print(myIMU.magCalibration[0], 2); + display.print(myIMU.factoryMagCalibration[0], 2); display.setCursor(0,20); display.print("ASAY "); display.setCursor(50,20); - display.print(myIMU.magCalibration[1], 2); + display.print(myIMU.factoryMagCalibration[1], 2); display.setCursor(0,30); display.print("ASAZ "); display.setCursor(50,30); - display.print(myIMU.magCalibration[2], 2); + display.print(myIMU.factoryMagCalibration[2], 2); + display.display(); + delay(1000); +#endif // LCD + + myIMU.magCalMPU9250(myIMU.magBias, myIMU.magScale); + Serial.println("AK8963 mag biases (mG)"); Serial.println(myIMU.magBias[0]); + Serial.println(myIMU.magBias[1]); Serial.println(myIMU.magBias[2]); + Serial.println("AK8963 mag scale (mG)"); Serial.println(myIMU.magScale[0]); + Serial.println(myIMU.magScale[1]); Serial.println(myIMU.magScale[2]); + delay(2000); // Add delay to see results before serial spew of data + + if(SerialDebug) + { + // Serial.println("Calibration values: "); + Serial.println("Magnetometer:"); + Serial.print("X-Axis sensitivity adjustment value "); +// TODO: change this from factory to calculated values + Serial.println(myIMU.factoryMagCalibration[0], 2); + Serial.print("Y-Axis sensitivity adjustment value "); + Serial.println(myIMU.factoryMagCalibration[1], 2); + Serial.print("Z-Axis sensitivity adjustment value "); + Serial.println(myIMU.factoryMagCalibration[2], 2); + } + +#ifdef LCD + display.clearDisplay(); + display.setCursor(20,0); display.print("AK8963"); + display.setCursor(0,10); display.print("ASAX "); display.setCursor(50,10); + display.print(myIMU.factoryMagCalibration[0], 2); + display.setCursor(0,20); display.print("ASAY "); display.setCursor(50,20); + display.print(myIMU.factoryMagCalibration[1], 2); + display.setCursor(0,30); display.print("ASAZ "); display.setCursor(50,30); + display.print(myIMU.factoryMagCalibration[2], 2); display.display(); delay(1000); #endif // LCD @@ -208,9 +241,9 @@ void loop() // Now we'll calculate the accleration value into actual g's // This depends on scale being set - myIMU.ax = (float)myIMU.accelCount[0]*myIMU.aRes; // - accelBias[0]; - myIMU.ay = (float)myIMU.accelCount[1]*myIMU.aRes; // - accelBias[1]; - myIMU.az = (float)myIMU.accelCount[2]*myIMU.aRes; // - accelBias[2]; + myIMU.ax = (float)myIMU.accelCount[0]*myIMU.aRes; // - myIMU.accelBias[0]; + myIMU.ay = (float)myIMU.accelCount[1]*myIMU.aRes; // - myIMU.accelBias[1]; + myIMU.az = (float)myIMU.accelCount[2]*myIMU.aRes; // - myIMU.accelBias[2]; myIMU.readGyroData(myIMU.gyroCount); // Read the x/y/z adc values myIMU.getGres(); @@ -223,24 +256,28 @@ void loop() myIMU.readMagData(myIMU.magCount); // Read the x/y/z adc values myIMU.getMres(); +// TODO: This needs to be fixed. No need to hard code it anymore. // User environmental x-axis correction in milliGauss, should be // automatically calculated - myIMU.magbias[0] = +470.; + myIMU.magBias[0] = +470.; // User environmental x-axis correction in milliGauss TODO axis?? - myIMU.magbias[1] = +120.; + myIMU.magBias[1] = +120.; // User environmental x-axis correction in milliGauss - myIMU.magbias[2] = +125.; + myIMU.magBias[2] = +125.; // Calculate the magnetometer values in milliGauss // Include factory calibration per data sheet and user environmental // corrections // Get actual magnetometer value, this depends on scale being set - myIMU.mx = (float)myIMU.magCount[0]*myIMU.mRes*myIMU.magCalibration[0] - - myIMU.magbias[0]; - myIMU.my = (float)myIMU.magCount[1]*myIMU.mRes*myIMU.magCalibration[1] - - myIMU.magbias[1]; - myIMU.mz = (float)myIMU.magCount[2]*myIMU.mRes*myIMU.magCalibration[2] - - myIMU.magbias[2]; + myIMU.mx = + (float)myIMU.magCount[0] * myIMU.mRes*myIMU.factoryMagCalibration[0] - + myIMU.magBias[0]; + myIMU.my = + (float)myIMU.magCount[1] * myIMU.mRes*myIMU.factoryMagCalibration[1] - + myIMU.magBias[1]; + myIMU.mz = + (float)myIMU.magCount[2] * myIMU.mRes*myIMU.factoryMagCalibration[2] - + myIMU.magBias[2]; } // if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) // Must be called before updating quaternions! From ed3eafc84f0c657c506abd1aa8fc60e1513789c7 Mon Sep 17 00:00:00 2001 From: Eric Wieser Date: Thu, 3 Nov 2016 14:37:47 +0000 Subject: [PATCH 11/28] Correct use of FS parameter This bug was previously fixed upstream in 066ca8297684d41b3e327c7ad143e110bcee5fda. See https://cdn.sparkfun.com/assets/learn_tutorials/5/5/0/MPU-9250-Register-Map.pdf#page=7 for verification that this is correct --- src/MPU9250.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/MPU9250.cpp b/src/MPU9250.cpp index c57fd57..f4edabc 100644 --- a/src/MPU9250.cpp +++ b/src/MPU9250.cpp @@ -362,13 +362,13 @@ void MPU9250::MPU9250SelfTest(float * destination) // Should return percent devi uint8_t selfTest[6]; int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3]; float factoryTrim[6]; - uint8_t FS = 0; + uint8_t FS = GFS_250DPS; writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1< Date: Tue, 3 Jan 2017 10:04:49 -0700 Subject: [PATCH 12/28] Updated links to product image Product photos were updated on SparkFun.com, so they needed to be changed here too. --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 8e7c321..1608145 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ MPU-9250 9 DOF IMU Arduino Library ================================== -![MPU-9250 Breakout](https://cdn.sparkfun.com/assets/parts/1/1/3/0/6/13762-00.jpg)](https://cdn.sparkfun.com/assets/parts/1/1/3/0/6/13762-00.jpg) +[![MPU-9250 Breakout](https://cdn.sparkfun.com/assets/parts/1/1/3/0/6/13762-00a.jpg)](https://cdn.sparkfun.com/assets/parts/1/1/3/0/6/13762-00a.jpg) [*MPU-9250 Breakout (SEN-13762)*](https://www.sparkfun.com/products/13762) From 77f89160ff4f7419f2534c45ad9867a4384dddf1 Mon Sep 17 00:00:00 2001 From: Brent Wilkins Date: Wed, 4 Jan 2017 15:36:08 -0700 Subject: [PATCH 13/28] Added magnetometer calibration routine and cleaned up some things --- .../MPU9250BasicAHRS/MPU9250BasicAHRS.ino | 162 ++-- src/MPU9250.cpp | 754 ++++++++++++------ src/MPU9250.h | 20 +- 3 files changed, 589 insertions(+), 347 deletions(-) diff --git a/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino b/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino index 27792e2..e0bc9b6 100644 --- a/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino +++ b/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino @@ -97,7 +97,7 @@ void setup() delay(1000); #endif // LCD - if (c == 0x71) // WHO_AM_I should always be 0x68 + if (c == 0x71) // WHO_AM_I should always be 0x71 { Serial.println("MPU9250 is online..."); @@ -147,8 +147,11 @@ void setup() // Read the WHO_AM_I register of the magnetometer, this is a good test of // communication byte d = myIMU.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963); - Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX); - Serial.print(" I should be "); Serial.println(0x48, HEX); + Serial.print("AK8963 "); + Serial.print("I AM "); + Serial.print(d, HEX); + Serial.print(" I should be "); + Serial.println(0x48, HEX); #ifdef LCD display.clearDisplay(); @@ -165,43 +168,54 @@ void setup() myIMU.initAK8963(myIMU.factoryMagCalibration); // Initialize device for active mode read of magnetometer Serial.println("AK8963 initialized for active data mode...."); + if (SerialDebug) { // Serial.println("Calibration values: "); - Serial.print("X-Axis sensitivity adjustment value "); + Serial.print("X-Axis factory sensitivity adjustment value "); Serial.println(myIMU.factoryMagCalibration[0], 2); - Serial.print("Y-Axis sensitivity adjustment value "); + Serial.print("Y-Axis factory sensitivity adjustment value "); Serial.println(myIMU.factoryMagCalibration[1], 2); - Serial.print("Z-Axis sensitivity adjustment value "); + Serial.print("Z-Axis factory sensitivity adjustment value "); Serial.println(myIMU.factoryMagCalibration[2], 2); } #ifdef LCD display.clearDisplay(); - display.setCursor(20,0); display.print("AK8963"); - display.setCursor(0,10); display.print("ASAX "); display.setCursor(50,10); - display.print(myIMU.factoryMagCalibration[0], 2); - display.setCursor(0,20); display.print("ASAY "); display.setCursor(50,20); - display.print(myIMU.factoryMagCalibration[1], 2); - display.setCursor(0,30); display.print("ASAZ "); display.setCursor(50,30); - display.print(myIMU.factoryMagCalibration[2], 2); + display.setCursor(20,0); display.print("AK8963"); + display.setCursor(0,10); display.print("ASAX "); + display.setCursor(50,10); display.print(myIMU.factoryMagCalibration[0], 2); + display.setCursor(0,20); display.print("ASAY "); + display.setCursor(50,20); display.print(myIMU.factoryMagCalibration[1], 2); + display.setCursor(0,30); display.print("ASAZ "); + display.setCursor(50,30); display.print(myIMU.factoryMagCalibration[2], 2); display.display(); delay(1000); #endif // LCD + // Get sensor resolutions, only need to do this once + myIMU.getAres(); + myIMU.getGres(); + myIMU.getMres(); + + // The next call delays for 4 seconds, and then records about 15 seconds of + // data to calculate bias and scale. myIMU.magCalMPU9250(myIMU.magBias, myIMU.magScale); - Serial.println("AK8963 mag biases (mG)"); Serial.println(myIMU.magBias[0]); - Serial.println(myIMU.magBias[1]); Serial.println(myIMU.magBias[2]); - Serial.println("AK8963 mag scale (mG)"); Serial.println(myIMU.magScale[0]); - Serial.println(myIMU.magScale[1]); Serial.println(myIMU.magScale[2]); + Serial.println("AK8963 mag biases (mG)"); + Serial.println(myIMU.magBias[0]); + Serial.println(myIMU.magBias[1]); + Serial.println(myIMU.magBias[2]); + + Serial.println("AK8963 mag scale (mG)"); + Serial.println(myIMU.magScale[0]); + Serial.println(myIMU.magScale[1]); + Serial.println(myIMU.magScale[2]); delay(2000); // Add delay to see results before serial spew of data - + if(SerialDebug) { - // Serial.println("Calibration values: "); Serial.println("Magnetometer:"); Serial.print("X-Axis sensitivity adjustment value "); -// TODO: change this from factory to calculated values Serial.println(myIMU.factoryMagCalibration[0], 2); Serial.print("Y-Axis sensitivity adjustment value "); Serial.println(myIMU.factoryMagCalibration[1], 2); @@ -235,49 +249,35 @@ void loop() // If intPin goes high, all data registers have new data // On interrupt, check if data ready interrupt if (myIMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) - { + { myIMU.readAccelData(myIMU.accelCount); // Read the x/y/z adc values - myIMU.getAres(); // Now we'll calculate the accleration value into actual g's // This depends on scale being set - myIMU.ax = (float)myIMU.accelCount[0]*myIMU.aRes; // - myIMU.accelBias[0]; - myIMU.ay = (float)myIMU.accelCount[1]*myIMU.aRes; // - myIMU.accelBias[1]; - myIMU.az = (float)myIMU.accelCount[2]*myIMU.aRes; // - myIMU.accelBias[2]; + myIMU.ax = (float)myIMU.accelCount[0] * myIMU.aRes; // - myIMU.accelBias[0]; + myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes; // - myIMU.accelBias[1]; + myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes; // - myIMU.accelBias[2]; myIMU.readGyroData(myIMU.gyroCount); // Read the x/y/z adc values - myIMU.getGres(); // Calculate the gyro value into actual degrees per second // This depends on scale being set - myIMU.gx = (float)myIMU.gyroCount[0]*myIMU.gRes; - myIMU.gy = (float)myIMU.gyroCount[1]*myIMU.gRes; - myIMU.gz = (float)myIMU.gyroCount[2]*myIMU.gRes; + myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes; + myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes; + myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes; myIMU.readMagData(myIMU.magCount); // Read the x/y/z adc values - myIMU.getMres(); -// TODO: This needs to be fixed. No need to hard code it anymore. - // User environmental x-axis correction in milliGauss, should be - // automatically calculated - myIMU.magBias[0] = +470.; - // User environmental x-axis correction in milliGauss TODO axis?? - myIMU.magBias[1] = +120.; - // User environmental x-axis correction in milliGauss - myIMU.magBias[2] = +125.; // Calculate the magnetometer values in milliGauss // Include factory calibration per data sheet and user environmental // corrections // Get actual magnetometer value, this depends on scale being set - myIMU.mx = - (float)myIMU.magCount[0] * myIMU.mRes*myIMU.factoryMagCalibration[0] - - myIMU.magBias[0]; - myIMU.my = - (float)myIMU.magCount[1] * myIMU.mRes*myIMU.factoryMagCalibration[1] - - myIMU.magBias[1]; - myIMU.mz = - (float)myIMU.magCount[2] * myIMU.mRes*myIMU.factoryMagCalibration[2] - - myIMU.magBias[2]; + myIMU.mx = (float)myIMU.magCount[0] * myIMU.mRes + * myIMU.factoryMagCalibration[0] - myIMU.magBias[0]; + myIMU.my = (float)myIMU.magCount[1] * myIMU.mRes + * myIMU.factoryMagCalibration[1] - myIMU.magBias[1]; + myIMU.mz = (float)myIMU.magCount[2] * myIMU.mRes + * myIMU.factoryMagCalibration[2] - myIMU.magBias[2]; } // if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) // Must be called before updating quaternions! @@ -291,9 +291,8 @@ void loop() // along the x-axis just like in the LSM9DS0 sensor. This rotation can be // modified to allow any convenient orientation convention. This is ok by // aircraft orientation standards! Pass gyro rate as rad/s -// MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); - MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx*DEG_TO_RAD, - myIMU.gy*DEG_TO_RAD, myIMU.gz*DEG_TO_RAD, myIMU.my, + MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx * DEG_TO_RAD, + myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD, myIMU.my, myIMU.mx, myIMU.mz, myIMU.deltat); if (!AHRS) @@ -304,11 +303,11 @@ void loop() if(SerialDebug) { // Print acceleration values in milligs! - Serial.print("X-acceleration: "); Serial.print(1000*myIMU.ax); + Serial.print("X-acceleration: "); Serial.print(1000 * myIMU.ax); Serial.print(" mg "); - Serial.print("Y-acceleration: "); Serial.print(1000*myIMU.ay); + Serial.print("Y-acceleration: "); Serial.print(1000 * myIMU.ay); Serial.print(" mg "); - Serial.print("Z-acceleration: "); Serial.print(1000*myIMU.az); + Serial.print("Z-acceleration: "); Serial.print(1000 * myIMU.az); Serial.println(" mg "); // Print gyro values in degree/sec @@ -340,9 +339,9 @@ void loop() display.setCursor(0, 0); display.print("MPU9250/AK8963"); display.setCursor(0, 8); display.print(" x y z "); - display.setCursor(0, 16); display.print((int)(1000*myIMU.ax)); - display.setCursor(24, 16); display.print((int)(1000*myIMU.ay)); - display.setCursor(48, 16); display.print((int)(1000*myIMU.az)); + display.setCursor(0, 16); display.print((int)(1000 * myIMU.ax)); + display.setCursor(24, 16); display.print((int)(1000 * myIMU.ay)); + display.setCursor(48, 16); display.print((int)(1000 * myIMU.az)); display.setCursor(72, 16); display.print("mg"); display.setCursor(0, 24); display.print((int)(myIMU.gx)); @@ -375,22 +374,22 @@ void loop() { if(SerialDebug) { - Serial.print("ax = "); Serial.print((int)1000*myIMU.ax); - Serial.print(" ay = "); Serial.print((int)1000*myIMU.ay); - Serial.print(" az = "); Serial.print((int)1000*myIMU.az); + Serial.print("ax = "); Serial.print((int)1000 * myIMU.ax); + Serial.print(" ay = "); Serial.print((int)1000 * myIMU.ay); + Serial.print(" az = "); Serial.print((int)1000 * myIMU.az); Serial.println(" mg"); - Serial.print("gx = "); Serial.print( myIMU.gx, 2); - Serial.print(" gy = "); Serial.print( myIMU.gy, 2); - Serial.print(" gz = "); Serial.print( myIMU.gz, 2); + Serial.print("gx = "); Serial.print(myIMU.gx, 2); + Serial.print(" gy = "); Serial.print(myIMU.gy, 2); + Serial.print(" gz = "); Serial.print(myIMU.gz, 2); Serial.println(" deg/s"); - Serial.print("mx = "); Serial.print( (int)myIMU.mx ); - Serial.print(" my = "); Serial.print( (int)myIMU.my ); - Serial.print(" mz = "); Serial.print( (int)myIMU.mz ); + Serial.print("mx = "); Serial.print((int)myIMU.mx); + Serial.print(" my = "); Serial.print((int)myIMU.my); + Serial.print(" mz = "); Serial.print((int)myIMU.mz); Serial.println(" mG"); - Serial.print("q0 = "); Serial.print(*getQ()); + Serial.print("q0 = "); Serial.print(*getQ()); Serial.print(" qx = "); Serial.print(*(getQ() + 1)); Serial.print(" qy = "); Serial.print(*(getQ() + 2)); Serial.print(" qz = "); Serial.println(*(getQ() + 3)); @@ -412,21 +411,24 @@ void loop() // For more see // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles // which has additional links. - myIMU.yaw = atan2(2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ() * - *(getQ()+3)), *getQ() * *getQ() + *(getQ()+1) * *(getQ()+1) - - *(getQ()+2) * *(getQ()+2) - *(getQ()+3) * *(getQ()+3)); - myIMU.pitch = -asin(2.0f * (*(getQ()+1) * *(getQ()+3) - *getQ() * - *(getQ()+2))); - myIMU.roll = atan2(2.0f * (*getQ() * *(getQ()+1) + *(getQ()+2) * - *(getQ()+3)), *getQ() * *getQ() - *(getQ()+1) * *(getQ()+1) - - *(getQ()+2) * *(getQ()+2) + *(getQ()+3) * *(getQ()+3)); + myIMU.yaw = atan2(2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ() + * *(getQ()+3)), *getQ() * *getQ() + *(getQ()+1) + * *(getQ()+1) - *(getQ()+2) * *(getQ()+2) - *(getQ()+3) + * *(getQ()+3)); + myIMU.pitch = -asin(2.0f * (*(getQ()+1) * *(getQ()+3) - *getQ() + * *(getQ()+2))); + myIMU.roll = atan2(2.0f * (*getQ() * *(getQ()+1) + *(getQ()+2) + * *(getQ()+3)), *getQ() * *getQ() - *(getQ()+1) + * *(getQ()+1) - *(getQ()+2) * *(getQ()+2) + *(getQ()+3) + * *(getQ()+3)); myIMU.pitch *= RAD_TO_DEG; myIMU.yaw *= RAD_TO_DEG; + // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is // 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19 // - http://www.ngdc.noaa.gov/geomag-web/#declination - myIMU.yaw -= 8.5; - myIMU.roll *= RAD_TO_DEG; + myIMU.yaw -= 8.5; + myIMU.roll *= RAD_TO_DEG; if(SerialDebug) { @@ -438,7 +440,7 @@ void loop() Serial.println(myIMU.roll, 2); Serial.print("rate = "); - Serial.print((float)myIMU.sumCount/myIMU.sum, 2); + Serial.print((float)myIMU.sumCount / myIMU.sum, 2); Serial.println(" Hz"); } @@ -447,9 +449,9 @@ void loop() display.setCursor(0, 0); display.print(" x y z "); - display.setCursor(0, 8); display.print((int)(1000*myIMU.ax)); - display.setCursor(24, 8); display.print((int)(1000*myIMU.ay)); - display.setCursor(48, 8); display.print((int)(1000*myIMU.az)); + display.setCursor(0, 8); display.print((int)(1000 * myIMU.ax)); + display.setCursor(24, 8); display.print((int)(1000 * myIMU.ay)); + display.setCursor(48, 8); display.print((int)(1000 * myIMU.az)); display.setCursor(72, 8); display.print("mg"); display.setCursor(0, 16); display.print((int)(myIMU.gx)); diff --git a/src/MPU9250.cpp b/src/MPU9250.cpp index c57fd57..9cb7d4f 100644 --- a/src/MPU9250.cpp +++ b/src/MPU9250.cpp @@ -5,59 +5,64 @@ //====== and temperature data //============================================================================== -void MPU9250::getMres() { +void MPU9250::getMres() +{ switch (Mscale) { - // Possible magnetometer scales (and their register bit settings) are: - // 14 bit resolution (0) and 16 bit resolution (1) + // Possible magnetometer scales (and their register bit settings) are: + // 14 bit resolution (0) and 16 bit resolution (1) case MFS_14BITS: - mRes = 10.*4912./8190.; // Proper scale to return milliGauss - break; + mRes = 10.0f * 4912.0f / 8190.0f; // Proper scale to return milliGauss + break; case MFS_16BITS: - mRes = 10.*4912./32760.0; // Proper scale to return milliGauss - break; + mRes = 10.0f * 4912.0f / 32760.0f; // Proper scale to return milliGauss + break; } } -void MPU9250::getGres() { +void MPU9250::getGres() +{ switch (Gscale) { - // Possible gyro scales (and their register bit settings) are: - // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: + // Possible gyro scales (and their register bit settings) are: + // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). + // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that + // 2-bit value: case GFS_250DPS: - gRes = 250.0/32768.0; - break; + gRes = 250.0f / 32768.0f; + break; case GFS_500DPS: - gRes = 500.0/32768.0; - break; + gRes = 500.0f / 32768.0f; + break; case GFS_1000DPS: - gRes = 1000.0/32768.0; - break; + gRes = 1000.0f / 32768.0f; + break; case GFS_2000DPS: - gRes = 2000.0/32768.0; - break; + gRes = 2000.0f / 32768.0f; + break; } } -void MPU9250::getAres() { +void MPU9250::getAres() +{ switch (Ascale) { - // Possible accelerometer scales (and their register bit settings) are: - // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: + // Possible accelerometer scales (and their register bit settings) are: + // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). + // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that + // 2-bit value: case AFS_2G: - aRes = 2.0/32768.0; - break; + aRes = 2.0f / 32768.0f; + break; case AFS_4G: - aRes = 4.0/32768.0; - break; + aRes = 4.0f / 32768.0f; + break; case AFS_8G: - aRes = 8.0/32768.0; - break; + aRes = 8.0f / 32768.0f; + break; case AFS_16G: - aRes = 16.0/32768.0; - break; + aRes = 16.0f / 32768.0f; + break; } } @@ -65,39 +70,45 @@ void MPU9250::getAres() { void MPU9250::readAccelData(int16_t * destination) { uint8_t rawData[6]; // x/y/z accel register data stored here - readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array - destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; - destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; + // Read the six raw data registers into data array + readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); + + // Turn the MSB and LSB into a signed 16-bit value + destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; + destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; + destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; } void MPU9250::readGyroData(int16_t * destination) { uint8_t rawData[6]; // x/y/z gyro register data stored here - readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array - destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value - destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; - destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; + // Read the six raw data registers sequentially into data array + readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); + + // Turn the MSB and LSB into a signed 16-bit value + destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; + destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; + destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; } void MPU9250::readMagData(int16_t * destination) { - // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of - // data acquisition + // x/y/z gyro register data, ST2 register stored here, must read ST2 at end + // of data acquisition uint8_t rawData[7]; // Wait for magnetometer data ready bit to be set - if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) + if (readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // Read the six raw data and ST2 registers sequentially into data array readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); uint8_t c = rawData[6]; // End data read by reading ST2 register // Check if magnetic sensor overflow set, if not then report data - if(!(c & 0x08)) + if (!(c & 0x08)) { // Turn the MSB and LSB into a signed 16-bit value destination[0] = ((int16_t)rawData[1] << 8) | rawData[0]; - // Data stored as little Endian + // Data stored as little Endian destination[1] = ((int16_t)rawData[3] << 8) | rawData[2]; destination[2] = ((int16_t)rawData[5] << 8) | rawData[4]; } @@ -106,16 +117,19 @@ void MPU9250::readMagData(int16_t * destination) int16_t MPU9250::readTempData() { - uint8_t rawData[2]; // x/y/z gyro register data stored here - readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array - return ((int16_t)rawData[0] << 8) | rawData[1]; // Turn the MSB and LSB into a 16-bit value + uint8_t rawData[2]; // x/y/z gyro register data stored here + // Read the two raw data registers sequentially into data array + readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); + // Turn the MSB and LSB into a 16-bit value + return ((int16_t)rawData[0] << 8) | rawData[1]; } // Calculate the time the last update took for use in the quaternion filters +// TODO: This doesn't really belong in this class. void MPU9250::updateTime() { Now = micros(); - + // Set integration time by time elapsed since last filter update deltat = ((Now - lastUpdate) / 1000000.0f); lastUpdate = Now; @@ -128,79 +142,106 @@ void MPU9250::initAK8963(float * destination) { // First extract the factory calibration for each magnetometer axis uint8_t rawData[3]; // x/y/z gyro calibration data stored here - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer + writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer delay(10); writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode delay(10); - readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values - destination[0] = (float)(rawData[0] - 128)/256. + 1.; // Return x-axis sensitivity adjustment values, etc. - destination[1] = (float)(rawData[1] - 128)/256. + 1.; - destination[2] = (float)(rawData[2] - 128)/256. + 1.; - writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer + + // Read the x-, y-, and z-axis calibration values + readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); + + // Return x-axis sensitivity adjustment values, etc. + destination[0] = (float)(rawData[0] - 128)/256. + 1.; + destination[1] = (float)(rawData[1] - 128)/256. + 1.; + destination[2] = (float)(rawData[2] - 128)/256. + 1.; + writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer delay(10); - // Configure the magnetometer for continuous read and highest resolution - // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, - // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates - writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR + + // Configure the magnetometer for continuous read and highest resolution. + // Set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL + // register, and enable continuous mode data acquisition Mmode (bits [3:0]), + // 0010 for 8 Hz and 0110 for 100 Hz sample rates. + + // Set magnetometer data resolution and sample ODR + writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); delay(10); } void MPU9250::initMPU9250() -{ - // wake up device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors - delay(100); // Wait for all registers to reset - - // get stable time source - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else - delay(200); - - // Configure Gyro and Thermometer - // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively; - // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot - // be higher than 1 / 0.0059 = 170 Hz - // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both - // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x03); - - // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; a rate consistent with the filter update rate - // determined inset in CONFIG above - - // Set gyroscope full scale range - // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 - uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value - // c = c & ~0xE0; // Clear self-test bits [7:5] - c = c & ~0x02; // Clear Fchoice bits [1:0] +{ + // wake up device + // Clear sleep mode bit (6), enable all sensors + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); + delay(100); // Wait for all registers to reset + + // Get stable time source + // Auto select clock source to be PLL gyroscope reference if ready else + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); + delay(200); + + // Configure Gyro and Thermometer + // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, + // respectively; + // minimum delay time for this setting is 5.9 ms, which means sensor fusion + // update rates cannot be higher than 1 / 0.0059 = 170 Hz + // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both + // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), + // 8 kHz, or 1 kHz + writeByte(MPU9250_ADDRESS, CONFIG, 0x03); + + // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) + // Use a 200 Hz rate; a rate consistent with the filter update rate + // determined inset in CONFIG above. + writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); + + // Set gyroscope full scale range + // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are + // left-shifted into positions 4:3 + + // get current GYRO_CONFIG register value + uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); + // c = c & ~0xE0; // Clear self-test bits [7:5] + c = c & ~0x02; // Clear Fchoice bits [1:0] c = c & ~0x18; // Clear AFS bits [4:3] c = c | Gscale << 3; // Set full scale range for the gyro - // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register - - // Set accelerometer full-scale range configuration - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value - // c = c & ~0xE0; // Clear self-test bits [7:5] + // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of + // GYRO_CONFIG + // c =| 0x00; + // Write new GYRO_CONFIG value to register + writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); + + // Set accelerometer full-scale range configuration + // Get current ACCEL_CONFIG register value + c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); + // c = c & ~0xE0; // Clear self-test bits [7:5] c = c & ~0x18; // Clear AFS bits [4:3] - c = c | Ascale << 3; // Set full scale range for the accelerometer - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value - - // Set accelerometer sample rate configuration - // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for - // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value - c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) + c = c | Ascale << 3; // Set full scale range for the accelerometer + // Write new ACCEL_CONFIG register value + writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); + + // Set accelerometer sample rate configuration + // It is possible to get a 4 kHz sample rate from the accelerometer by + // choosing 1 for accel_fchoice_b bit [3]; in this case the bandwidth is + // 1.13 kHz + // Get current ACCEL_CONFIG2 register value + c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); + c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value - // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, - // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting + // Write new ACCEL_CONFIG2 register value + writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); + // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, + // but all these rates are further reduced by a factor of 5 to 200 Hz because + // of the SMPLRT_DIV setting // Configure Interrupts and Bypass Enable - // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared, - // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips - // can join the I2C bus and all can be controlled by the Arduino as master - writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22); - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt - delay(100); + // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH + // until interrupt cleared, clear on read of INT_STATUS, and enable + // I2C_BYPASS_EN so additional chips can join the I2C bus and all can be + // controlled by the Arduino as master. + writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22); + // Enable data ready (bit 0) interrupt + writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); + delay(100); } @@ -208,139 +249,191 @@ void MPU9250::initMPU9250() // initialization. It calculates the average of the at-rest readings and then // loads the resulting offsets into accelerometer and gyro bias registers. void MPU9250::calibrateMPU9250(float * gyroBias, float * accelBias) -{ +{ uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data uint16_t ii, packet_count, fifo_count; int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; - + // reset device // Write a one to bit 7 reset bit; toggle reset device writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); delay(100); - - // get stable time source; Auto select clock source to be PLL gyroscope - // reference if ready else use the internal oscillator, bits 2:0 = 001 - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); + + // get stable time source; Auto select clock source to be PLL gyroscope + // reference if ready else use the internal oscillator, bits 2:0 = 001 + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); - delay(200); + delay(200); // Configure device for bias calculation - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source - writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP + // Disable all interrupts + writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); + // Disable FIFO + writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); + // Turn on internal clock source + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); + // Disable I2C master + writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); + // Disable FIFO and I2C master modes + writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); + // Reset FIFO and DMP + writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); delay(15); - -// Configure MPU6050 gyro and accelerometer for bias calculation - writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity - - uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec - uint16_t accelsensitivity = 16384; // = 16384 LSB/g - // Configure FIFO to capture accelerometer and gyro data for bias calculation - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150) - delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes + // Configure MPU6050 gyro and accelerometer for bias calculation + // Set low-pass filter to 188 Hz + writeByte(MPU9250_ADDRESS, CONFIG, 0x01); + // Set sample rate to 1 kHz + writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); + // Set gyro full-scale to 250 degrees per second, maximum sensitivity + writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); + // Set accelerometer full-scale to 2 g, maximum sensitivity + writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); -// At end of sample accumulation, turn off FIFO sensor read - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO - readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count + uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec + uint16_t accelsensitivity = 16384; // = 16384 LSB/g + + // Configure FIFO to capture accelerometer and gyro data for bias calculation + writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO + // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in + // MPU-9150) + writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); + delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes + + // At end of sample accumulation, turn off FIFO sensor read + // Disable gyro and accelerometer sensors for FIFO + writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); + // Read FIFO sample count + readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); fifo_count = ((uint16_t)data[0] << 8) | data[1]; - packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging - + // How many sets of full gyro and accelerometer data for averaging + packet_count = fifo_count/12; + for (ii = 0; ii < packet_count; ii++) { int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; - readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging - accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ); // Form signed 16-bit integer for each sample in FIFO + // Read data for averaging + readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); + // Form signed 16-bit integer for each sample in FIFO + accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ); accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ); accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ); gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ); gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ); gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]); - - accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases + + // Sum individual signed 16-bit biases to get accumulated signed 32-bit + // biases. + accel_bias[0] += (int32_t) accel_temp[0]; accel_bias[1] += (int32_t) accel_temp[1]; accel_bias[2] += (int32_t) accel_temp[2]; gyro_bias[0] += (int32_t) gyro_temp[0]; gyro_bias[1] += (int32_t) gyro_temp[1]; gyro_bias[2] += (int32_t) gyro_temp[2]; } - accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases + // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases + accel_bias[0] /= (int32_t) packet_count; accel_bias[1] /= (int32_t) packet_count; accel_bias[2] /= (int32_t) packet_count; gyro_bias[0] /= (int32_t) packet_count; gyro_bias[1] /= (int32_t) packet_count; gyro_bias[2] /= (int32_t) packet_count; - - if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation - else {accel_bias[2] += (int32_t) accelsensitivity;} - -// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup - 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 - data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases + + // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases + if (accel_bias[2] > 0L) + { + accel_bias[2] -= (int32_t) accelsensitivity; + } + else + { + accel_bias[2] += (int32_t) accelsensitivity; + } + + // Construct the gyro biases for push to the hardware gyro bias registers, + // which are reset to zero upon device startup. + // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input + // format. + data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; + // Biases are additive, so change sign on calculated average gyro biases + data[1] = (-gyro_bias[0]/4) & 0xFF; data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; data[3] = (-gyro_bias[1]/4) & 0xFF; data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; data[5] = (-gyro_bias[2]/4) & 0xFF; - -// Push gyro biases to hardware registers + + // Push gyro biases to hardware registers writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]); writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]); writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]); writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]); writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]); writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]); - -// Output scaled gyro biases for display in the main program - gyroBias[0] = (float) gyro_bias[0]/(float) gyrosensitivity; + + // Output scaled gyro biases for display in the main program + gyroBias[0] = (float) gyro_bias[0]/(float) gyrosensitivity; gyroBias[1] = (float) gyro_bias[1]/(float) gyrosensitivity; gyroBias[2] = (float) gyro_bias[2]/(float) gyrosensitivity; -// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain -// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold -// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature -// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that -// the accelerometer biases calculated above must be divided by 8. - - int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases - readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values + // Construct the accelerometer biases for push to the hardware accelerometer + // bias registers. These registers contain factory trim values which must be + // added to the calculated accelerometer biases; on boot up these registers + // will hold non-zero values. In addition, bit 0 of the lower byte must be + // preserved since it is used for temperature compensation calculations. + // Accelerometer bias registers expect bias input as 2048 LSB per g, so that + // the accelerometer biases calculated above must be divided by 8. + + // A place to hold the factory accelerometer trim biases + int32_t accel_bias_reg[3] = {0, 0, 0}; + // Read factory accelerometer trim values + readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]); readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]); readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - - uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers - uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis - - for(ii = 0; ii < 3; ii++) { - if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit + + // Define mask for temperature compensation bit 0 of lower byte of + // accelerometer bias registers + uint32_t mask = 1uL; + // Define array to hold mask bit for each accelerometer bias axis + uint8_t mask_bit[3] = {0, 0, 0}; + + for (ii = 0; ii < 3; ii++) + { + // If temperature compensation bit is set, record that fact in mask_bit + if ((accel_bias_reg[ii] & mask)) + { + mask_bit[ii] = 0x01; + } } - - // Construct total accelerometer bias, including calculated average accelerometer bias from above - accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) + + // Construct total accelerometer bias, including calculated average + // accelerometer bias from above + // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g + // (16 g full scale) + accel_bias_reg[0] -= (accel_bias[0]/8); accel_bias_reg[1] -= (accel_bias[1]/8); accel_bias_reg[2] -= (accel_bias[2]/8); - + data[0] = (accel_bias_reg[0] >> 8) & 0xFF; data[1] = (accel_bias_reg[0]) & 0xFF; - data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers + // preserve temperature compensation bit when writing back to accelerometer + // bias registers + data[1] = data[1] | mask_bit[0]; data[2] = (accel_bias_reg[1] >> 8) & 0xFF; data[3] = (accel_bias_reg[1]) & 0xFF; - data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers + // Preserve temperature compensation bit when writing back to accelerometer + // bias registers + data[3] = data[3] | mask_bit[1]; data[4] = (accel_bias_reg[2] >> 8) & 0xFF; data[5] = (accel_bias_reg[2]) & 0xFF; - data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers - -// Apparently this is not working for the acceleration biases in the MPU-9250 -// Are we handling the temperature correction bit properly? -// Push accelerometer biases to hardware registers + // Preserve temperature compensation bit when writing back to accelerometer + // bias registers + data[5] = data[5] | mask_bit[2]; + + // Apparently this is not working for the acceleration biases in the MPU-9250 + // Are we handling the temperature correction bit properly? + // Push accelerometer biases to hardware registers writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]); writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]); writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]); @@ -348,99 +441,230 @@ void MPU9250::calibrateMPU9250(float * gyroBias, float * accelBias) writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]); writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); -// Output scaled accelerometer biases for display in the main program - accelBias[0] = (float)accel_bias[0]/(float)accelsensitivity; - accelBias[1] = (float)accel_bias[1]/(float)accelsensitivity; - accelBias[2] = (float)accel_bias[2]/(float)accelsensitivity; + // Output scaled accelerometer biases for display in the main program + accelBias[0] = (float)accel_bias[0]/(float)accelsensitivity; + accelBias[1] = (float)accel_bias[1]/(float)accelsensitivity; + accelBias[2] = (float)accel_bias[2]/(float)accelsensitivity; } - + // Accelerometer and gyroscope self test; check calibration wrt factory settings -void MPU9250::MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass +// Should return percent deviation from factory trim values, +/- 14 or less +// deviation is a pass. +void MPU9250::MPU9250SelfTest(float * destination) { uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; uint8_t selfTest[6]; - int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3]; + int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0}; float factoryTrim[6]; uint8_t FS = 0; - - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1< mag_max[jj]) + { + mag_max[jj] = mag_temp[jj]; + } + if (mag_temp[jj] < mag_min[jj]) + { + mag_min[jj] = mag_temp[jj]; + } + } + + if (Mmode == M_8HZ) + { + delay(135); // At 8 Hz ODR, new mag data is available every 125 ms + } + if (Mmode == M_100HZ) + { + delay(12); // At 100 Hz ODR, new mag data is available every 10 ms + } + } + + // Serial.println("mag x min/max:"); Serial.println(mag_max[0]); Serial.println(mag_min[0]); + // Serial.println("mag y min/max:"); Serial.println(mag_max[1]); Serial.println(mag_min[1]); + // Serial.println("mag z min/max:"); Serial.println(mag_max[2]); Serial.println(mag_min[2]); + + // Get hard iron correction + // Get 'average' x mag bias in counts + mag_bias[0] = (mag_max[0] + mag_min[0]) / 2; + // Get 'average' y mag bias in counts + mag_bias[1] = (mag_max[1] + mag_min[1]) / 2; + // Get 'average' z mag bias in counts + mag_bias[2] = (mag_max[2] + mag_min[2]) / 2; + + // Save mag biases in G for main program + bias_dest[0] = (float)mag_bias[0] * mRes * factoryMagCalibration[0]; + bias_dest[1] = (float)mag_bias[1] * mRes * factoryMagCalibration[1]; + bias_dest[2] = (float)mag_bias[2] * mRes * factoryMagCalibration[2]; + + // Get soft iron correction estimate + // Get average x axis max chord length in counts + mag_scale[0] = (mag_max[0] - mag_min[0]) / 2; + // Get average y axis max chord length in counts + mag_scale[1] = (mag_max[1] - mag_min[1]) / 2; + // Get average z axis max chord length in counts + mag_scale[2] = (mag_max[2] - mag_min[2]) / 2; + + float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2]; + avg_rad /= 3.0; + + scale_dest[0] = avg_rad / ((float)mag_scale[0]); + scale_dest[1] = avg_rad / ((float)mag_scale[1]); + scale_dest[2] = avg_rad / ((float)mag_scale[2]); + + Serial.println("Mag Calibration done!"); +} + // Wire.h read and write protocols void MPU9250::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) { @@ -452,23 +676,29 @@ void MPU9250::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) uint8_t MPU9250::readByte(uint8_t address, uint8_t subAddress) { - uint8_t data; // `data` will store the register data - Wire.beginTransmission(address); // Initialize the Tx buffer - Wire.write(subAddress); // Put slave register address in Tx buffer - Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive - Wire.requestFrom(address, (uint8_t) 1); // Read one byte from slave register address - data = Wire.read(); // Fill Rx buffer with result - return data; // Return data read from slave register + uint8_t data; // `data` will store the register data + Wire.beginTransmission(address); // Initialize the Tx buffer + Wire.write(subAddress); // Put slave register address in Tx buffer + Wire.endTransmission(false); // Send the Tx buffer, but send a restart + // to keep connection alive + Wire.requestFrom(address, (uint8_t) 1); // Read one byte from slave + // register address + data = Wire.read(); // Fill Rx buffer with result + return data; // Return data read from slave register } void MPU9250::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) -{ - Wire.beginTransmission(address); // Initialize the Tx buffer - Wire.write(subAddress); // Put slave register address in Tx buffer - Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive +{ + Wire.beginTransmission(address); // Initialize the Tx buffer + Wire.write(subAddress); // Put slave register address in Tx buffer + Wire.endTransmission(false); // Send the Tx buffer, but send a restart + // to keep connection alive uint8_t i = 0; - Wire.requestFrom(address, count); // Read bytes from slave register address - while (Wire.available()) { - dest[i++] = Wire.read(); } // Put read results in the Rx buffer + Wire.requestFrom(address, count); // Read bytes from slave register address + while (Wire.available()) + { + // Put read results in the Rx buffer + dest[i++] = Wire.read(); + } } diff --git a/src/MPU9250.h b/src/MPU9250.h index 579e315..3430765 100644 --- a/src/MPU9250.h +++ b/src/MPU9250.h @@ -200,13 +200,19 @@ class MPU9250 MFS_16BITS // 0.15 mG per LSB }; + enum M_MODE { + M_8HZ = 0x02, // 8 Hz update + M_100HZ = 0x06 // 100 Hz continuous magnetometer + }; + + // TODO: Add setter methods for this hard coded stuff // Specify sensor full scale uint8_t Gscale = GFS_250DPS; uint8_t Ascale = AFS_2G; // Choose either 14-bit or 16-bit magnetometer resolution uint8_t Mscale = MFS_16BITS; // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read - uint8_t Mmode = 0x02; + uint8_t Mmode = M_8HZ; public: float pitch, yaw, roll; @@ -226,13 +232,16 @@ class MPU9250 // Variables to hold latest sensor data values float ax, ay, az, gx, gy, gz, mx, my, mz; // Factory mag calibration and mag bias - float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0}; - // Bias corrections for gyro and accelerometer - float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; + float factoryMagCalibration[3] = {0, 0, 0}, factoryMagBias[3] = {0, 0, 0}; + // Bias corrections for gyro, accelerometer, and magnetometer + float gyroBias[3] = {0, 0, 0}, + accelBias[3] = {0, 0, 0}, + magBias[3] = {0, 0, 0}, + magScale[3] = {0, 0, 0}; float SelfTest[6]; // Stores the 16-bit signed accelerometer sensor output int16_t accelCount[3]; - + public: void getMres(); void getGres(); @@ -246,6 +255,7 @@ class MPU9250 void initMPU9250(); void calibrateMPU9250(float * gyroBias, float * accelBias); void MPU9250SelfTest(float * destination); + void magCalMPU9250(float * dest1, float * dest2); void writeByte(uint8_t, uint8_t, uint8_t); uint8_t readByte(uint8_t, uint8_t); void readBytes(uint8_t, uint8_t, uint8_t, uint8_t *); From 018dd45cfb6db1f19e29c96fd3d67d5bb286a9a4 Mon Sep 17 00:00:00 2001 From: Brent Wilkins Date: Tue, 10 Jan 2017 13:47:02 -0700 Subject: [PATCH 14/28] Added SPI capabilities. WARNING: Not reading from AK8963 over SPI yet. --- .../MPU9250BasicAHRS_I2C.ino} | 52 +- .../MPU9250BasicAHRS_SPI.ino | 508 ++++++++++++++++++ src/MPU9250.cpp | 163 +++++- src/MPU9250.h | 23 +- 4 files changed, 697 insertions(+), 49 deletions(-) rename examples/{MPU9250BasicAHRS/MPU9250BasicAHRS.ino => MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino} (93%) create mode 100644 examples/MPU9250BasicAHRS_SPI/MPU9250BasicAHRS_SPI.ino diff --git a/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino b/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino similarity index 93% rename from examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino rename to examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino index e0bc9b6..5174fc1 100644 --- a/examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino +++ b/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino @@ -84,8 +84,10 @@ void setup() // Read the WHO_AM_I register, this is a good test of communication byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); - Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); - Serial.print(" I should be "); Serial.println(0x71, HEX); + Serial.print(F("MPU9250 I AM 0x")); + Serial.print(c, HEX); + Serial.print(F(" I should be 0x")); + Serial.println(0x71, HEX); #ifdef LCD display.setCursor(20,0); display.print("MPU9250"); @@ -99,22 +101,22 @@ void setup() if (c == 0x71) // WHO_AM_I should always be 0x71 { - Serial.println("MPU9250 is online..."); + Serial.println(F("MPU9250 is online...")); // Start by performing self test and reporting values - myIMU.MPU9250SelfTest(myIMU.SelfTest); - Serial.print("x-axis self test: acceleration trim within : "); - Serial.print(myIMU.SelfTest[0],1); Serial.println("% of factory value"); - Serial.print("y-axis self test: acceleration trim within : "); - Serial.print(myIMU.SelfTest[1],1); Serial.println("% of factory value"); - Serial.print("z-axis self test: acceleration trim within : "); - Serial.print(myIMU.SelfTest[2],1); Serial.println("% of factory value"); - Serial.print("x-axis self test: gyration trim within : "); - Serial.print(myIMU.SelfTest[3],1); Serial.println("% of factory value"); - Serial.print("y-axis self test: gyration trim within : "); - Serial.print(myIMU.SelfTest[4],1); Serial.println("% of factory value"); - Serial.print("z-axis self test: gyration trim within : "); - Serial.print(myIMU.SelfTest[5],1); Serial.println("% of factory value"); + myIMU.MPU9250SelfTest(myIMU.selfTest); + Serial.print(F("x-axis self test: acceleration trim within : ")); + Serial.print(myIMU.selfTest[0],1); Serial.println("% of factory value"); + Serial.print(F("y-axis self test: acceleration trim within : ")); + Serial.print(myIMU.selfTest[1],1); Serial.println("% of factory value"); + Serial.print(F("z-axis self test: acceleration trim within : ")); + Serial.print(myIMU.selfTest[2],1); Serial.println("% of factory value"); + Serial.print(F("x-axis self test: gyration trim within : ")); + Serial.print(myIMU.selfTest[3],1); Serial.println("% of factory value"); + Serial.print(F("y-axis self test: gyration trim within : ")); + Serial.print(myIMU.selfTest[4],1); Serial.println("% of factory value"); + Serial.print(F("z-axis self test: gyration trim within : ")); + Serial.print(myIMU.selfTest[5],1); Serial.println("% of factory value"); // Calibrate gyro and accelerometers, load biases in bias registers myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias); @@ -148,9 +150,9 @@ void setup() // communication byte d = myIMU.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963); Serial.print("AK8963 "); - Serial.print("I AM "); + Serial.print("I AM 0x"); Serial.print(d, HEX); - Serial.print(" I should be "); + Serial.print(" I should be 0x"); Serial.println(0x48, HEX); #ifdef LCD @@ -164,6 +166,14 @@ void setup() delay(1000); #endif // LCD + if (d != 0x48) + { + // Communication failed, stop here + Serial.println(F("Communication failed, abort!")); + Serial.flush(); + abort(); + } + // Get magnetometer calibration from AK8963 ROM myIMU.initAK8963(myIMU.factoryMagCalibration); // Initialize device for active mode read of magnetometer @@ -240,7 +250,11 @@ void setup() { Serial.print("Could not connect to MPU9250: 0x"); Serial.println(c, HEX); - while(1) ; // Loop forever if communication doesn't happen + + // Communication failed, stop here + Serial.println(F("Communication failed, abort!")); + Serial.flush(); + abort(); } } diff --git a/examples/MPU9250BasicAHRS_SPI/MPU9250BasicAHRS_SPI.ino b/examples/MPU9250BasicAHRS_SPI/MPU9250BasicAHRS_SPI.ino new file mode 100644 index 0000000..c666167 --- /dev/null +++ b/examples/MPU9250BasicAHRS_SPI/MPU9250BasicAHRS_SPI.ino @@ -0,0 +1,508 @@ +/* MPU9250 Basic Example Code + by: Kris Winer + date: April 1, 2014 + license: Beerware - Use this code however you'd like. If you + find it useful you can buy me a beer some time. + Modified by Brent Wilkins January 9, 2016 + + Demonstrate basic MPU-9250 functionality including parameterizing the register + addresses, initializing the sensor, getting properly scaled accelerometer, + gyroscope, and magnetometer data out. Added display functions to allow display + to on breadboard monitor. Addition of 9 DoF sensor fusion using open source + Madgwick and Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini. + + SDA and SCL should have external pull-up resistors (to 3.3V). + 10k resistors are on the EMSENSR-9250 breakout board. + + Hardware setup: + MPU9250 Breakout --------- Arduino + VDD ---------------------- 3.3V + VDDI --------------------- 3.3V + SDA ----------------------- A4 + SCL ----------------------- A5 + GND ---------------------- GND + */ + +#include "quaternionFilters.h" +#include "MPU9250.h" + +#ifdef LCD +#include +#include + +// Using NOKIA 5110 monochrome 84 x 48 pixel display +// pin 9 - Serial clock out (SCLK) +// pin 8 - Serial data out (DIN) +// pin 7 - Data/Command select (D/C) +// pin 5 - LCD chip select (CS) +// pin 6 - LCD reset (RST) +Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); +#endif // LCD + +#define AHRS true // Set to false for basic data read +#define SerialDebug true // Set to true to get Serial output for debugging + +MPU9250 myIMU = MPU9250(2); // Using digital pin to for chip select in demo + +void setup() +{ + Serial.begin(38400); + +Serial.println("Testing MPU9250::readBytes..."); +byte fc = 0; +myIMU.readBytes(MPU9250_ADDRESS, WHO_AM_I_MPU9250, 1, &fc); +Serial.println(fc, HEX); + +byte fd = 0; +myIMU.readBytes(AK8963_ADDRESS, WHO_AM_I_AK8963, 1, &fd); +Serial.println(fd, HEX); + +#ifdef LCD + display.begin(); // Initialize the display + display.setContrast(58); // Set the contrast + + // Start device display with ID of sensor + display.clearDisplay(); + display.setTextSize(2); + display.setCursor(0,0); display.print("MPU9250"); + display.setTextSize(1); + display.setCursor(0, 20); display.print("9-DOF 16-bit"); + display.setCursor(0, 30); display.print("motion sensor"); + display.setCursor(20,40); display.print("60 ug LSB"); + display.display(); + delay(1000); + + // Set up for data display + display.setTextSize(1); // Set text size to normal, 2 is twice normal etc. + display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen + display.clearDisplay(); // Clears the screen and buffer +#endif // LCD + + // Read the WHO_AM_I register, this is a good test of communication + byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); + Serial.print(F("MPU9250 I AM 0x")); + Serial.print(c, HEX); + Serial.print(F(" I should be 0x")); + Serial.println(0x71, HEX); + +#ifdef LCD + display.setCursor(20,0); display.print("MPU9250"); + display.setCursor(0,10); display.print("I AM"); + display.setCursor(0,20); display.print(c, HEX); + display.setCursor(0,30); display.print("I Should Be"); + display.setCursor(0,40); display.print(0x71, HEX); + display.display(); + delay(1000); +#endif // LCD + + if (c == 0x71) // WHO_AM_I should always be 0x71 + { + Serial.println(F("MPU9250 is online...")); + + // Start by performing self test and reporting values + myIMU.MPU9250SelfTest(myIMU.selfTest); + Serial.print(F("x-axis self test: acceleration trim within : ")); + Serial.print(myIMU.selfTest[0],1); Serial.println("% of factory value"); + Serial.print(F("y-axis self test: acceleration trim within : ")); + Serial.print(myIMU.selfTest[1],1); Serial.println("% of factory value"); + Serial.print(F("z-axis self test: acceleration trim within : ")); + Serial.print(myIMU.selfTest[2],1); Serial.println("% of factory value"); + Serial.print(F("x-axis self test: gyration trim within : ")); + Serial.print(myIMU.selfTest[3],1); Serial.println("% of factory value"); + Serial.print(F("y-axis self test: gyration trim within : ")); + Serial.print(myIMU.selfTest[4],1); Serial.println("% of factory value"); + Serial.print(F("z-axis self test: gyration trim within : ")); + Serial.print(myIMU.selfTest[5],1); Serial.println("% of factory value"); + + // Calibrate gyro and accelerometers, load biases in bias registers + myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias); + +#ifdef LCD + display.clearDisplay(); + + display.setCursor(0, 0); display.print("MPU9250 bias"); + display.setCursor(0, 8); display.print(" x y z "); + + display.setCursor(0, 16); display.print((int)(1000*myIMU.accelBias[0])); + display.setCursor(24, 16); display.print((int)(1000*myIMU.accelBias[1])); + display.setCursor(48, 16); display.print((int)(1000*myIMU.accelBias[2])); + display.setCursor(72, 16); display.print("mg"); + + display.setCursor(0, 24); display.print(myIMU.gyroBias[0], 1); + display.setCursor(24, 24); display.print(myIMU.gyroBias[1], 1); + display.setCursor(48, 24); display.print(myIMU.gyroBias[2], 1); + display.setCursor(66, 24); display.print("o/s"); + + display.display(); + delay(1000); +#endif // LCD + + myIMU.initMPU9250(); + // Initialize device for active mode read of acclerometer, gyroscope, and + // temperature + Serial.println("MPU9250 initialized for active data mode...."); + + // Read the WHO_AM_I register of the magnetometer, this is a good test of + // communication + byte d = myIMU.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963); + Serial.print("AK8963 "); + Serial.print("I AM 0x"); + Serial.print(d, HEX); + Serial.print(" I should be 0x"); + Serial.println(0x48, HEX); + +#ifdef LCD + display.clearDisplay(); + display.setCursor(20,0); display.print("AK8963"); + display.setCursor(0,10); display.print("I AM"); + display.setCursor(0,20); display.print(d, HEX); + display.setCursor(0,30); display.print("I Should Be"); + display.setCursor(0,40); display.print(0x48, HEX); + display.display(); + delay(1000); +#endif // LCD + + if (d != 0x48) + { + // Communication failed, stop here + Serial.println(F("Communication failed, abort!")); + Serial.flush(); + abort(); + } + + // Get magnetometer calibration from AK8963 ROM + myIMU.initAK8963(myIMU.factoryMagCalibration); + // Initialize device for active mode read of magnetometer + Serial.println("AK8963 initialized for active data mode...."); + + if (SerialDebug) + { + // Serial.println("Calibration values: "); + Serial.print("X-Axis factory sensitivity adjustment value "); + Serial.println(myIMU.factoryMagCalibration[0], 2); + Serial.print("Y-Axis factory sensitivity adjustment value "); + Serial.println(myIMU.factoryMagCalibration[1], 2); + Serial.print("Z-Axis factory sensitivity adjustment value "); + Serial.println(myIMU.factoryMagCalibration[2], 2); + } + +#ifdef LCD + display.clearDisplay(); + display.setCursor(20,0); display.print("AK8963"); + display.setCursor(0,10); display.print("ASAX "); + display.setCursor(50,10); display.print(myIMU.factoryMagCalibration[0], 2); + display.setCursor(0,20); display.print("ASAY "); + display.setCursor(50,20); display.print(myIMU.factoryMagCalibration[1], 2); + display.setCursor(0,30); display.print("ASAZ "); + display.setCursor(50,30); display.print(myIMU.factoryMagCalibration[2], 2); + display.display(); + delay(1000); +#endif // LCD + + // Get sensor resolutions, only need to do this once + myIMU.getAres(); + myIMU.getGres(); + myIMU.getMres(); + + // The next call delays for 4 seconds, and then records about 15 seconds of + // data to calculate bias and scale. + myIMU.magCalMPU9250(myIMU.magBias, myIMU.magScale); + Serial.println("AK8963 mag biases (mG)"); + Serial.println(myIMU.magBias[0]); + Serial.println(myIMU.magBias[1]); + Serial.println(myIMU.magBias[2]); + + Serial.println("AK8963 mag scale (mG)"); + Serial.println(myIMU.magScale[0]); + Serial.println(myIMU.magScale[1]); + Serial.println(myIMU.magScale[2]); + delay(2000); // Add delay to see results before serial spew of data + + if(SerialDebug) + { + Serial.println("Magnetometer:"); + Serial.print("X-Axis sensitivity adjustment value "); + Serial.println(myIMU.factoryMagCalibration[0], 2); + Serial.print("Y-Axis sensitivity adjustment value "); + Serial.println(myIMU.factoryMagCalibration[1], 2); + Serial.print("Z-Axis sensitivity adjustment value "); + Serial.println(myIMU.factoryMagCalibration[2], 2); + } + +#ifdef LCD + display.clearDisplay(); + display.setCursor(20,0); display.print("AK8963"); + display.setCursor(0,10); display.print("ASAX "); display.setCursor(50,10); + display.print(myIMU.factoryMagCalibration[0], 2); + display.setCursor(0,20); display.print("ASAY "); display.setCursor(50,20); + display.print(myIMU.factoryMagCalibration[1], 2); + display.setCursor(0,30); display.print("ASAZ "); display.setCursor(50,30); + display.print(myIMU.factoryMagCalibration[2], 2); + display.display(); + delay(1000); +#endif // LCD + } // if (c == 0x71) + else + { + Serial.print("Could not connect to MPU9250: 0x"); + Serial.println(c, HEX); + + // Communication failed, stop here + Serial.println(F("Communication failed, abort!")); + Serial.flush(); + abort(); + } +} + +void loop() +{ +// TODO: Fix these comments not interrupt pin used + // If intPin goes high, all data registers have new data + // On interrupt, check if data ready interrupt + if (myIMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) + { + myIMU.readAccelData(myIMU.accelCount); // Read the x/y/z adc values + + // Now we'll calculate the accleration value into actual g's + // This depends on scale being set + myIMU.ax = (float)myIMU.accelCount[0] * myIMU.aRes; // - myIMU.accelBias[0]; + myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes; // - myIMU.accelBias[1]; + myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes; // - myIMU.accelBias[2]; + + myIMU.readGyroData(myIMU.gyroCount); // Read the x/y/z adc values + + // Calculate the gyro value into actual degrees per second + // This depends on scale being set + myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes; + myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes; + myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes; + + myIMU.readMagData(myIMU.magCount); // Read the x/y/z adc values + + // Calculate the magnetometer values in milliGauss + // Include factory calibration per data sheet and user environmental + // corrections + // Get actual magnetometer value, this depends on scale being set + myIMU.mx = (float)myIMU.magCount[0] * myIMU.mRes + * myIMU.factoryMagCalibration[0] - myIMU.magBias[0]; + myIMU.my = (float)myIMU.magCount[1] * myIMU.mRes + * myIMU.factoryMagCalibration[1] - myIMU.magBias[1]; + myIMU.mz = (float)myIMU.magCount[2] * myIMU.mRes + * myIMU.factoryMagCalibration[2] - myIMU.magBias[2]; + } // if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) + + // Must be called before updating quaternions! + myIMU.updateTime(); + + // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of + // the magnetometer; the magnetometer z-axis (+ down) is opposite to z-axis + // (+ up) of accelerometer and gyro! We have to make some allowance for this + // orientationmismatch in feeding the output to the quaternion filter. For the + // MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward + // along the x-axis just like in the LSM9DS0 sensor. This rotation can be + // modified to allow any convenient orientation convention. This is ok by + // aircraft orientation standards! Pass gyro rate as rad/s + MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx * DEG_TO_RAD, + myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD, myIMU.my, + myIMU.mx, myIMU.mz, myIMU.deltat); + + if (!AHRS) + { + myIMU.delt_t = millis() - myIMU.count; + if (myIMU.delt_t > 500) + { + if(SerialDebug) + { + // Print acceleration values in milligs! + Serial.print("X-acceleration: "); Serial.print(1000 * myIMU.ax); + Serial.print(" mg "); + Serial.print("Y-acceleration: "); Serial.print(1000 * myIMU.ay); + Serial.print(" mg "); + Serial.print("Z-acceleration: "); Serial.print(1000 * myIMU.az); + Serial.println(" mg "); + + // Print gyro values in degree/sec + Serial.print("X-gyro rate: "); Serial.print(myIMU.gx, 3); + Serial.print(" degrees/sec "); + Serial.print("Y-gyro rate: "); Serial.print(myIMU.gy, 3); + Serial.print(" degrees/sec "); + Serial.print("Z-gyro rate: "); Serial.print(myIMU.gz, 3); + Serial.println(" degrees/sec"); + + // Print mag values in degree/sec + Serial.print("X-mag field: "); Serial.print(myIMU.mx); + Serial.print(" mG "); + Serial.print("Y-mag field: "); Serial.print(myIMU.my); + Serial.print(" mG "); + Serial.print("Z-mag field: "); Serial.print(myIMU.mz); + Serial.println(" mG"); + + myIMU.tempCount = myIMU.readTempData(); // Read the adc values + // Temperature in degrees Centigrade + myIMU.temperature = ((float) myIMU.tempCount) / 333.87 + 21.0; + // Print temperature in degrees Centigrade + Serial.print("Temperature is "); Serial.print(myIMU.temperature, 1); + Serial.println(" degrees C"); + } + +#ifdef LCD + display.clearDisplay(); + display.setCursor(0, 0); display.print("MPU9250/AK8963"); + display.setCursor(0, 8); display.print(" x y z "); + + display.setCursor(0, 16); display.print((int)(1000 * myIMU.ax)); + display.setCursor(24, 16); display.print((int)(1000 * myIMU.ay)); + display.setCursor(48, 16); display.print((int)(1000 * myIMU.az)); + display.setCursor(72, 16); display.print("mg"); + + display.setCursor(0, 24); display.print((int)(myIMU.gx)); + display.setCursor(24, 24); display.print((int)(myIMU.gy)); + display.setCursor(48, 24); display.print((int)(myIMU.gz)); + display.setCursor(66, 24); display.print("o/s"); + + display.setCursor(0, 32); display.print((int)(myIMU.mx)); + display.setCursor(24, 32); display.print((int)(myIMU.my)); + display.setCursor(48, 32); display.print((int)(myIMU.mz)); + display.setCursor(72, 32); display.print("mG"); + + display.setCursor(0, 40); display.print("Gyro T "); + display.setCursor(50, 40); display.print(myIMU.temperature, 1); + display.print(" C"); + display.display(); +#endif // LCD + + myIMU.count = millis(); + } // if (myIMU.delt_t > 500) + } // if (!AHRS) + else + { + // Serial print and/or display at 0.5 s rate independent of data rates + myIMU.delt_t = millis() - myIMU.count; + + // update LCD once per half-second independent of read rate + if (myIMU.delt_t > 500) + { + if(SerialDebug) + { + Serial.print("ax = "); Serial.print((int)1000 * myIMU.ax); + Serial.print(" ay = "); Serial.print((int)1000 * myIMU.ay); + Serial.print(" az = "); Serial.print((int)1000 * myIMU.az); + Serial.println(" mg"); + + Serial.print("gx = "); Serial.print(myIMU.gx, 2); + Serial.print(" gy = "); Serial.print(myIMU.gy, 2); + Serial.print(" gz = "); Serial.print(myIMU.gz, 2); + Serial.println(" deg/s"); + + Serial.print("mx = "); Serial.print((int)myIMU.mx); + Serial.print(" my = "); Serial.print((int)myIMU.my); + Serial.print(" mz = "); Serial.print((int)myIMU.mz); + Serial.println(" mG"); + + Serial.print("q0 = "); Serial.print(*getQ()); + Serial.print(" qx = "); Serial.print(*(getQ() + 1)); + Serial.print(" qy = "); Serial.print(*(getQ() + 2)); + Serial.print(" qz = "); Serial.println(*(getQ() + 3)); + } + +// Define output variables from updated quaternion---these are Tait-Bryan +// angles, commonly used in aircraft orientation. In this coordinate system, +// the positive z-axis is down toward Earth. 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. +// Pitch is angle between sensor x-axis and Earth ground plane, toward the +// Earth is positive, up toward the sky is negative. Roll is angle between +// sensor y-axis and Earth ground plane, y-axis up is positive roll. These +// arise from the definition of the homogeneous rotation matrix constructed +// from quaternions. Tait-Bryan angles as well as Euler angles are +// non-commutative; that is, the get the correct orientation the rotations +// must be applied in the correct order which for this configuration is yaw, +// pitch, and then roll. +// For more see +// http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles +// which has additional links. + myIMU.yaw = atan2(2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ() + * *(getQ()+3)), *getQ() * *getQ() + *(getQ()+1) + * *(getQ()+1) - *(getQ()+2) * *(getQ()+2) - *(getQ()+3) + * *(getQ()+3)); + myIMU.pitch = -asin(2.0f * (*(getQ()+1) * *(getQ()+3) - *getQ() + * *(getQ()+2))); + myIMU.roll = atan2(2.0f * (*getQ() * *(getQ()+1) + *(getQ()+2) + * *(getQ()+3)), *getQ() * *getQ() - *(getQ()+1) + * *(getQ()+1) - *(getQ()+2) * *(getQ()+2) + *(getQ()+3) + * *(getQ()+3)); + myIMU.pitch *= RAD_TO_DEG; + myIMU.yaw *= RAD_TO_DEG; + + // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is + // 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19 + // - http://www.ngdc.noaa.gov/geomag-web/#declination + myIMU.yaw -= 8.5; + myIMU.roll *= RAD_TO_DEG; + + if(SerialDebug) + { + Serial.print("Yaw, Pitch, Roll: "); + Serial.print(myIMU.yaw, 2); + Serial.print(", "); + Serial.print(myIMU.pitch, 2); + Serial.print(", "); + Serial.println(myIMU.roll, 2); + + Serial.print("rate = "); + Serial.print((float)myIMU.sumCount / myIMU.sum, 2); + Serial.println(" Hz"); + } + +#ifdef LCD + display.clearDisplay(); + + display.setCursor(0, 0); display.print(" x y z "); + + display.setCursor(0, 8); display.print((int)(1000 * myIMU.ax)); + display.setCursor(24, 8); display.print((int)(1000 * myIMU.ay)); + display.setCursor(48, 8); display.print((int)(1000 * myIMU.az)); + display.setCursor(72, 8); display.print("mg"); + + display.setCursor(0, 16); display.print((int)(myIMU.gx)); + display.setCursor(24, 16); display.print((int)(myIMU.gy)); + display.setCursor(48, 16); display.print((int)(myIMU.gz)); + display.setCursor(66, 16); display.print("o/s"); + + display.setCursor(0, 24); display.print((int)(myIMU.mx)); + display.setCursor(24, 24); display.print((int)(myIMU.my)); + display.setCursor(48, 24); display.print((int)(myIMU.mz)); + display.setCursor(72, 24); display.print("mG"); + + display.setCursor(0, 32); display.print((int)(myIMU.yaw)); + display.setCursor(24, 32); display.print((int)(myIMU.pitch)); + display.setCursor(48, 32); display.print((int)(myIMU.roll)); + display.setCursor(66, 32); display.print("ypr"); + + // With these settings the filter is updating at a ~145 Hz rate using the + // Madgwick scheme and >200 Hz using the Mahony scheme even though the + // display refreshes at only 2 Hz. The filter update rate is determined + // mostly by the mathematical steps in the respective algorithms, the + // processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR: + // an ODR of 10 Hz for the magnetometer produce the above rates, maximum + // magnetometer ODR of 100 Hz produces filter update rates of 36 - 145 and + // ~38 Hz for the Madgwick and Mahony schemes, respectively. This is + // presumably because the magnetometer read takes longer than the gyro or + // accelerometer reads. This filter update rate should be fast enough to + // maintain accurate platform orientation for stabilization control of a + // fast-moving robot or quadcopter. Compare to the update rate of 200 Hz + // produced by the on-board Digital Motion Processor of Invensense's MPU6050 + // 6 DoF and MPU9150 9DoF sensors. The 3.3 V 8 MHz Pro Mini is doing pretty + // well! + display.setCursor(0, 40); display.print("rt: "); + display.print((float) myIMU.sumCount / myIMU.sum, 2); + display.print(" Hz"); + display.display(); +#endif // LCD + + myIMU.count = millis(); + myIMU.sumCount = 0; + myIMU.sum = 0; + } // if (myIMU.delt_t > 500) + } // if (AHRS) +} diff --git a/src/MPU9250.cpp b/src/MPU9250.cpp index 9cb7d4f..e190c26 100644 --- a/src/MPU9250.cpp +++ b/src/MPU9250.cpp @@ -5,6 +5,27 @@ //====== and temperature data //============================================================================== +MPU9250::MPU9250(int8_t cspin /*=NOT_SPI*/) // Uses I2C communication by default +{ + // Use hardware SPI communication + // If used with sparkfun breakout board + // https://www.sparkfun.com/products/13762 , change the pre-soldered JP2 to + // enable SPI (solder middle and left instead of middle and right) pads are + // very small and re-soldering can be very tricky. I2C highly recommended. + if ((cspin > NOT_SPI) && (cspin < NUM_DIGITAL_PINS)) + { + _csPin = cspin; + SPI.begin(); + pinMode(_csPin, OUTPUT); + digitalWrite(_csPin, HIGH); + } + else + { + _csPin = NOT_SPI; + Wire.begin(); + } +} + void MPU9250::getMres() { switch (Mscale) @@ -473,6 +494,8 @@ void MPU9250::MPU9250SelfTest(float * destination) // Get average current values of gyro and acclerometer for (int ii = 0; ii < 200; ii++) { +Serial.print("BHW::ii = "); +Serial.println(ii); // Read the six raw data registers into data array readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Turn the MSB and LSB into a signed 16-bit value @@ -588,8 +611,9 @@ void MPU9250::magCalMPU9250(float * bias_dest, float * scale_dest) // Make sure resolution has been calculated getMres(); - Serial.println("Mag Calibration: Wave device in a figure 8 until done!"); - Serial.println(" 4 seconds to get ready followed by 15 seconds of sampling)"); + Serial.println(F("Mag Calibration: Wave device in a figure 8 until done!")); + Serial.println( + F(" 4 seconds to get ready followed by 15 seconds of sampling)")); delay(4000); // shoot for ~fifteen seconds of mag data @@ -662,43 +686,130 @@ void MPU9250::magCalMPU9250(float * bias_dest, float * scale_dest) scale_dest[1] = avg_rad / ((float)mag_scale[1]); scale_dest[2] = avg_rad / ((float)mag_scale[2]); - Serial.println("Mag Calibration done!"); + Serial.println(F("Mag Calibration done!")); } // Wire.h read and write protocols -void MPU9250::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) +void MPU9250::writeByte(uint8_t deviceAddress, uint8_t registerAddress, + uint8_t data) +{ + if (_csPin != NOT_SPI) + { + writeByteSPI(registerAddress, data); + } + else + { + writeByteWire(deviceAddress,registerAddress, data); + } +} + +void MPU9250::writeByteSPI(uint8_t registerAddress, uint8_t data) { - Wire.beginTransmission(address); // Initialize the Tx buffer - Wire.write(subAddress); // Put slave register address in Tx buffer + SPI.beginTransaction(SPISettings(SPI_DATA_RATE, MSBFIRST, SPI_MODE0)); + digitalWrite(_csPin, LOW); + SPI.transfer(registerAddress & ~0x80); // set first bit to 0 + SPI.transfer(data); + digitalWrite(_csPin, HIGH); + SPI.endTransaction(); +} + +void MPU9250::writeByteWire(uint8_t deviceAddress, uint8_t registerAddress, + uint8_t data) +{ + Wire.beginTransmission(deviceAddress); // Initialize the Tx buffer + Wire.write(registerAddress); // Put slave register address in Tx buffer Wire.write(data); // Put data in Tx buffer Wire.endTransmission(); // Send the Tx buffer } -uint8_t MPU9250::readByte(uint8_t address, uint8_t subAddress) +uint8_t MPU9250::readByte(uint8_t deviceAddress, uint8_t registerAddress) +{ + if (_csPin != NOT_SPI) + { + return readByteSPI(registerAddress); + } + else + { + return readByteWire(deviceAddress, registerAddress); + } +} + +uint8_t MPU9250::readByteWire(uint8_t deviceAddress, uint8_t registerAddress) { uint8_t data; // `data` will store the register data - Wire.beginTransmission(address); // Initialize the Tx buffer - Wire.write(subAddress); // Put slave register address in Tx buffer - Wire.endTransmission(false); // Send the Tx buffer, but send a restart - // to keep connection alive - Wire.requestFrom(address, (uint8_t) 1); // Read one byte from slave - // register address - data = Wire.read(); // Fill Rx buffer with result - return data; // Return data read from slave register + + // Initialize the Tx buffer + Wire.beginTransmission(deviceAddress); + // Put slave register address in Tx buffer + Wire.write(registerAddress); + // Send the Tx buffer, but send a restart to keep connection alive + Wire.endTransmission(false); + // Read one byte from slave register address + Wire.requestFrom(deviceAddress, (uint8_t) 1); + // Fill Rx buffer with result + data = Wire.read(); + // Return data read from slave register + return data; } -void MPU9250::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, - uint8_t * dest) +uint8_t MPU9250::readByteSPI(uint8_t registerAddress) { - Wire.beginTransmission(address); // Initialize the Tx buffer - Wire.write(subAddress); // Put slave register address in Tx buffer - Wire.endTransmission(false); // Send the Tx buffer, but send a restart - // to keep connection alive - uint8_t i = 0; - Wire.requestFrom(address, count); // Read bytes from slave register address - while (Wire.available()) + uint8_t value; + SPI.beginTransaction(SPISettings(SPI_DATA_RATE, MSBFIRST, SPI_MODE0)); + digitalWrite(_csPin, LOW); + SPI.transfer(registerAddress | 0x80); // set first bit to 1 + value = SPI.transfer(0xFF); + digitalWrite(_csPin, HIGH); + SPI.endTransaction(); + return value; +} + +void MPU9250::readBytes(uint8_t deviceAddress, uint8_t registerAddress, + uint8_t count, uint8_t * dest) +{ + if (_csPin == NOT_SPI) // Read via I2C + { + // Initialize the Tx buffer + Wire.beginTransmission(deviceAddress); + // Put slave register address in Tx buffer + Wire.write(registerAddress); + // Send the Tx buffer, but send a restart to keep connection alive + Wire.endTransmission(false); +Serial.print("BHW:: read bytes: 0x"); + uint8_t i = 0; + // Read bytes from slave register address + Wire.requestFrom(deviceAddress, count); + while (Wire.available()) + { + // Put read results in the Rx buffer + dest[i++] = Wire.read(); +Serial.print(dest[i-1], HEX); + } +Serial.print('\n'); + } + else // Read using SPI { - // Put read results in the Rx buffer - dest[i++] = Wire.read(); + SPI.beginTransaction(SPISettings(SPI_DATA_RATE, MSBFIRST, SPI_MODE0)); + digitalWrite(_csPin, LOW); + SPI.transfer(registerAddress | 0x80); // set first bit to 1 +Serial.print("BHW:: read bytes: "); + SPI.transfer(dest, count); + /* + uint8_t i = 0; + while (count > 0) + { + dest[i++] = SPI.transfer(0xFF); +Serial.print(dest[i-1], HEX); + count--; + } + */ + for (int i = 0; i < count; i++) + { + Serial.print(" 0x"); + Serial.print(dest[i], HEX); + } +Serial.print('\n'); + digitalWrite(_csPin, HIGH); + SPI.endTransaction(); // Done after de-asserting chip select to free bus } } diff --git a/src/MPU9250.h b/src/MPU9250.h index 3430765..ca18586 100644 --- a/src/MPU9250.h +++ b/src/MPU9250.h @@ -18,7 +18,7 @@ //Magnetometer Registers #define AK8963_ADDRESS 0x0C -#define WHO_AM_I_AK8963 0x00 // should return 0x48 +#define WHO_AM_I_AK8963 0x00 // (AKA WIA) should return 0x48 #define INFO 0x01 #define AK8963_ST1 0x02 // data ready status bit 0 #define AK8963_XOUT_L 0x03 // data @@ -177,11 +177,15 @@ #define AK8963_ADDRESS 0x0C // Address of magnetometer #endif // AD0 +#define NOT_SPI -1 +#define SPI_DATA_RATE 1000000 // 1MHz is the max speed of the MPU-9250 + class MPU9250 { protected: // Set initial input parameters - enum Ascale { + enum Ascale + { AFS_2G = 0, AFS_4G, AFS_8G, @@ -211,9 +215,18 @@ class MPU9250 uint8_t Ascale = AFS_2G; // Choose either 14-bit or 16-bit magnetometer resolution uint8_t Mscale = MFS_16BITS; + // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read uint8_t Mmode = M_8HZ; + // SPI chip select pin + int8_t _csPin; + + void writeByteWire(uint8_t, uint8_t, uint8_t); + void writeByteSPI(uint8_t, uint8_t); + uint8_t readByteSPI(uint8_t subAddress); + uint8_t readByteWire(uint8_t address, uint8_t subAddress); + public: float pitch, yaw, roll; float temperature; // Stores the real internal chip temperature in Celsius @@ -238,11 +251,12 @@ class MPU9250 accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0}, magScale[3] = {0, 0, 0}; - float SelfTest[6]; + float selfTest[6]; // Stores the 16-bit signed accelerometer sensor output int16_t accelCount[3]; - public: + // Public method declarations + MPU9250(int8_t csPin=NOT_SPI); void getMres(); void getGres(); void getAres(); @@ -259,6 +273,7 @@ class MPU9250 void writeByte(uint8_t, uint8_t, uint8_t); uint8_t readByte(uint8_t, uint8_t); void readBytes(uint8_t, uint8_t, uint8_t, uint8_t *); + bool isInI2cMode() { return _csPin == -1; } }; // class MPU9250 #endif // _MPU9250_H_ From 76045e8384091b505071738216e63cdc313d8e61 Mon Sep 17 00:00:00 2001 From: Brent Wilkins Date: Fri, 3 Feb 2017 15:35:31 -0700 Subject: [PATCH 15/28] Added a sketch to debug SPI. Seems to work as of this commit --- examples/MPU9250_Debug/MPU9250_Debug.ino | 238 +++++++++++++++++++++++ 1 file changed, 238 insertions(+) create mode 100644 examples/MPU9250_Debug/MPU9250_Debug.ino diff --git a/examples/MPU9250_Debug/MPU9250_Debug.ino b/examples/MPU9250_Debug/MPU9250_Debug.ino new file mode 100644 index 0000000..c294940 --- /dev/null +++ b/examples/MPU9250_Debug/MPU9250_Debug.ino @@ -0,0 +1,238 @@ +#include + +const int CS_PIN = 2; + +#define READ_FLAG 0x80 +#define SPI_DATA_RATE 100000 // 1 MHz max +#define SPI_MODE SPI_MODE3 + +#define SERIAL_DEBUG true + +void setup() +{ + pinMode(CS_PIN, OUTPUT); + digitalWrite(CS_PIN, HIGH); + SPI.begin(); + Serial.begin(38400); + + kickHardware(); + + ak8963Init(); + //resetAndWake(); + + ak8963WhoAmI(); + + checkSPI(); +} // end setup() + +void loop() { +} + +// Read the WHOAMI (WIA) register of the AK8963 +uint8_t ak8963WhoAmI() +{ + uint8_t response; + // Set the I2C slave addres of AK8963 and set for read + response = writeByteSPI(0x25, 0x0C|READ_FLAG); + // I2C slave 0 register address from where to begin data transfer + response = writeByteSPI(0x26, 0x00); + // Enable 1-byte reads on slave 0 + response = writeByteSPI(0x27, 0x81); + delayMicroseconds(1); + // Read WIA register + response = writeByteSPI(0x49|READ_FLAG, 0x00); + + return response; +} + +// Initialize the AK8963 +void ak8963Init() +{ + uint8_t ret; + + // Reset registers to defaults, bit auto clears + ret = writeByteSPI(0x6B, 0x80); +#ifdef SERIAL_DEBUG + Serial.print("write(0x6B, 0x80): 0x"); + Serial.println(ret, HEX); +#endif + + // Auto select the best available clock source + ret = writeByteSPI(0x6B, 0x01); +#ifdef SERIAL_DEBUG + Serial.print("write(0x6B, 0x01): 0x"); + Serial.println(ret, HEX); +#endif + + // Enable X,Y, & Z axes of accel and gyro + ret = writeByteSPI(0x6C, 0x00); +#ifdef SERIAL_DEBUG + Serial.print("write(0x6C, 0x00): 0x"); + Serial.println(ret, HEX); +#endif + + // Config disable FSYNC pin, set gyro/temp bandwidth to 184/188 Hz + ret = writeByteSPI(0x1A, 0x01); +#ifdef SERIAL_DEBUG + Serial.print("write(0x1A, 0x01): 0x"); + Serial.println(ret, HEX); +#endif + + // Self tests off, gyro set to +/-2000 dps FS + ret = writeByteSPI(0x1B, 0x18); +#ifdef SERIAL_DEBUG + Serial.print("write(0x1B, 0x18): 0x"); + Serial.println(ret, HEX); +#endif + + // Self test off, accel set to +/- 8g FS + ret = writeByteSPI(0x1C, 0x08); +#ifdef SERIAL_DEBUG + Serial.print("write(0x1C, 0x08): 0x"); + Serial.println(ret, HEX); +#endif + + // Bypass DLPF and set accel bandwidth to 184 Hz + ret = writeByteSPI(0x1D, 0x09); +#ifdef SERIAL_DEBUG + Serial.print("write(0x1D, 0x09): 0x"); + Serial.println(ret, HEX); +#endif + + // Configure INT pin (active high / push-pull / latch until read) + ret = writeByteSPI(0x37, 0x30); +#ifdef SERIAL_DEBUG + Serial.print("write(0x37, 0x30): 0x"); + Serial.println(ret, HEX); +#endif + + // Enable I2C master mode + // TODO Why not do this 11-100 ms after power up? + ret = writeByteSPI(0x6A, 0x20); +#ifdef SERIAL_DEBUG + Serial.print("write(0x6A, 0x20): 0x"); + Serial.println(ret, HEX); +#endif + + // Disable multi-master and set I2C master clock to 400 kHz + //https://developer.mbed.org/users/kylongmu/code/MPU9250_SPI/ calls says + // enabled multi-master... TODO Find out why + ret = writeByteSPI(0x24, 0x0D); +#ifdef SERIAL_DEBUG + Serial.print("write(0x24, 0x0D): 0x"); + Serial.println(ret, HEX); +#endif + + // Set to write to slave address 0x0C + // TODO: Verify this address + ret = writeByteSPI(0x25, 0x0C); +#ifdef SERIAL_DEBUG + Serial.print("write(0x25, 0x0C): 0x"); + Serial.println(ret, HEX); +#endif + + + // Point save 0 register at AK8963's control 2 (soft reset) register + ret = writeByteSPI(0x26, 0x0B); +#ifdef SERIAL_DEBUG + Serial.print("write(0x26, 0x0B): 0x"); + Serial.println(ret, HEX); +#endif + + // Send 0x01 to AK8963 via slave 0 to trigger a soft restart + ret = writeByteSPI(0x63, 0x01); +#ifdef SERIAL_DEBUG + Serial.print("write(0x63, 0x01): 0x"); + Serial.println(ret, HEX); +#endif + + // Enable simple 1-byte I2C reads from slave 0 + ret = writeByteSPI(0x27, 0x81); +#ifdef SERIAL_DEBUG + Serial.print("write(0x27, 0x81): 0x"); + Serial.println(ret, HEX); +#endif + + // Point save 0 register at AK8963's control 1 (mode) register + ret = writeByteSPI(0x26, 0x0A); +#ifdef SERIAL_DEBUG + Serial.print("write(0x26, 0x0A): 0x"); + Serial.println(ret, HEX); +#endif + + // 16-bit continuous measurement mode 1 + ret = writeByteSPI(0x63, 0x12); +#ifdef SERIAL_DEBUG + Serial.print("write(0x63, 0x12): 0x"); + Serial.println(ret, HEX); +#endif + + // Enable simple 1-byte I2C reads from slave 0 + ret = writeByteSPI(0x27, 0x81); +#ifdef SERIAL_DEBUG + Serial.print("write(0x27, 0x81): 0x"); + Serial.println(ret, HEX); +#endif +} + +void checkSPI() +{ + uint8_t temp = SPSR; + if(temp) + { + Serial.print("SPSR – SPI Status Register: 0b"); + Serial.println(temp); + } +} + + +// Reset and wake the MPU-9250 +void resetAndWake() +{ +#ifdef SERIAL_DEBUG + Serial.println("Resetting and waking MPU-9250"); +#endif +#ifdef SERIAL_DEBUG + Serial.println("Resetting (0x80 => 0x6B)"); +#endif + writeByteSPI(0x80, 0x6B); // Reset + delay(100); +#ifdef SERIAL_DEBUG + Serial.println("Waking (0x00 => 0x6B)"); +#endif + writeByteSPI(0x00, 0x6B); // Wake +} + + +// Write a null byte w/o CS assertion to get SPI hardware to idle high (mode 3) +void kickHardware() +{ + SPI.beginTransaction(SPISettings(SPI_DATA_RATE, MSBFIRST, SPI_MODE)); + SPI.transfer(0x00); // Send null byte + SPI.endTransaction(); +} + + +// Function to write a byte on SPI bus. +uint8_t writeByteSPI(uint8_t registerAddress, uint8_t writeData) +{ + digitalWrite(CS_PIN, LOW); + SPI.beginTransaction(SPISettings(SPI_DATA_RATE, MSBFIRST, SPI_MODE)); + SPI.transfer(registerAddress); + uint8_t returnVal = SPI.transfer(writeData); + SPI.endTransaction(); + digitalWrite(CS_PIN, HIGH); + delayMicroseconds(50); // Not used initially, adding during debug +#ifdef SERIAL_DEBUG + Serial.print("BHW::writeByteSPI slave returned: 0x"); + Serial.println(returnVal, HEX); +#endif + return returnVal; +} + + +// Function to read a byte from SPI bus. +uint8_t readByteSPI(uint8_t registerAddress) +{ + return writeByteSPI(registerAddress | READ_FLAG, 0xFF /*0xFF is arbitrary*/); +} From 113e836e989699913660c60bacda12da4a919c91 Mon Sep 17 00:00:00 2001 From: Brent Wilkins Date: Tue, 7 Feb 2017 14:35:04 -0700 Subject: [PATCH 16/28] Added in working magnetometer config and WHOAMI reading. Saving unfinished work as is. --- .../MPU9250BasicAHRS_SPI.ino | 60 ++-- examples/MPU9250_Debug/MPU9250_Debug.ino | 2 +- src/MPU9250.cpp | 274 ++++++++++++++---- src/MPU9250.h | 26 +- 4 files changed, 274 insertions(+), 88 deletions(-) diff --git a/examples/MPU9250BasicAHRS_SPI/MPU9250BasicAHRS_SPI.ino b/examples/MPU9250BasicAHRS_SPI/MPU9250BasicAHRS_SPI.ino index c666167..d00f181 100644 --- a/examples/MPU9250BasicAHRS_SPI/MPU9250BasicAHRS_SPI.ino +++ b/examples/MPU9250BasicAHRS_SPI/MPU9250BasicAHRS_SPI.ino @@ -40,19 +40,31 @@ Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); #endif // LCD #define AHRS true // Set to false for basic data read -#define SerialDebug true // Set to true to get Serial output for debugging +#define SERIAL_DEBUG true // Set to true to get Serial output for debugging MPU9250 myIMU = MPU9250(2); // Using digital pin to for chip select in demo void setup() { Serial.begin(38400); + myIMU.begin(); + + myIMU.ak8963WhoAmI_SPI(); Serial.println("Testing MPU9250::readBytes..."); byte fc = 0; -myIMU.readBytes(MPU9250_ADDRESS, WHO_AM_I_MPU9250, 1, &fc); +Serial.println( myIMU.readBytes(MPU9250_ADDRESS, WHO_AM_I_MPU9250, 1, &fc) ); Serial.println(fc, HEX); - +// TODO BHW +Serial.flush(); +abort(); + +/* +// Set slave address of AK8963 and set AK8963 for read +myIMU.writeByteSPI(I2C_SLV0_ADDR, AK8963_ADDRESS | READ_FLAG); +// Set address to start read from +myIMU.writeByteSPI(I2C_SLV0_REG, WHO_AM_I_AK8963); +*/ byte fd = 0; myIMU.readBytes(AK8963_ADDRESS, WHO_AM_I_AK8963, 1, &fd); Serial.println(fd, HEX); @@ -84,6 +96,9 @@ Serial.println(fd, HEX); Serial.print(c, HEX); Serial.print(F(" I should be 0x")); Serial.println(0x71, HEX); +// TODO BHW +Serial.flush(); +abort(); #ifdef LCD display.setCursor(20,0); display.print("MPU9250"); @@ -175,8 +190,7 @@ Serial.println(fd, HEX); // Initialize device for active mode read of magnetometer Serial.println("AK8963 initialized for active data mode...."); - if (SerialDebug) - { +#ifdef SERIAL_DEBUG // Serial.println("Calibration values: "); Serial.print("X-Axis factory sensitivity adjustment value "); Serial.println(myIMU.factoryMagCalibration[0], 2); @@ -184,7 +198,7 @@ Serial.println(fd, HEX); Serial.println(myIMU.factoryMagCalibration[1], 2); Serial.print("Z-Axis factory sensitivity adjustment value "); Serial.println(myIMU.factoryMagCalibration[2], 2); - } +#endif // SERIAL_DEBUG #ifdef LCD display.clearDisplay(); @@ -218,16 +232,15 @@ Serial.println(fd, HEX); Serial.println(myIMU.magScale[2]); delay(2000); // Add delay to see results before serial spew of data - if(SerialDebug) - { - Serial.println("Magnetometer:"); - Serial.print("X-Axis sensitivity adjustment value "); - Serial.println(myIMU.factoryMagCalibration[0], 2); - Serial.print("Y-Axis sensitivity adjustment value "); - Serial.println(myIMU.factoryMagCalibration[1], 2); - Serial.print("Z-Axis sensitivity adjustment value "); - Serial.println(myIMU.factoryMagCalibration[2], 2); - } +#ifdef SERIAL_DEBUG + Serial.println("Magnetometer:"); + Serial.print("X-Axis sensitivity adjustment value "); + Serial.println(myIMU.factoryMagCalibration[0], 2); + Serial.print("Y-Axis sensitivity adjustment value "); + Serial.println(myIMU.factoryMagCalibration[1], 2); + Serial.print("Z-Axis sensitivity adjustment value "); + Serial.println(myIMU.factoryMagCalibration[2], 2); +#endif // SERIAL_DEBUG #ifdef LCD display.clearDisplay(); @@ -311,8 +324,7 @@ void loop() myIMU.delt_t = millis() - myIMU.count; if (myIMU.delt_t > 500) { - if(SerialDebug) - { +#ifdef SERIAL_DEBUG // Print acceleration values in milligs! Serial.print("X-acceleration: "); Serial.print(1000 * myIMU.ax); Serial.print(" mg "); @@ -343,7 +355,7 @@ void loop() // Print temperature in degrees Centigrade Serial.print("Temperature is "); Serial.print(myIMU.temperature, 1); Serial.println(" degrees C"); - } +#endif // SERIAL_DEBUG #ifdef LCD display.clearDisplay(); @@ -382,8 +394,7 @@ void loop() // update LCD once per half-second independent of read rate if (myIMU.delt_t > 500) { - if(SerialDebug) - { +#ifdef SERIAL_DEBUG Serial.print("ax = "); Serial.print((int)1000 * myIMU.ax); Serial.print(" ay = "); Serial.print((int)1000 * myIMU.ay); Serial.print(" az = "); Serial.print((int)1000 * myIMU.az); @@ -403,7 +414,7 @@ void loop() Serial.print(" qx = "); Serial.print(*(getQ() + 1)); Serial.print(" qy = "); Serial.print(*(getQ() + 2)); Serial.print(" qz = "); Serial.println(*(getQ() + 3)); - } +#endif // SERIAL_DEBUG // Define output variables from updated quaternion---these are Tait-Bryan // angles, commonly used in aircraft orientation. In this coordinate system, @@ -440,8 +451,7 @@ void loop() myIMU.yaw -= 8.5; myIMU.roll *= RAD_TO_DEG; - if(SerialDebug) - { +#ifdef SERIAL_DEBUG Serial.print("Yaw, Pitch, Roll: "); Serial.print(myIMU.yaw, 2); Serial.print(", "); @@ -452,7 +462,7 @@ void loop() Serial.print("rate = "); Serial.print((float)myIMU.sumCount / myIMU.sum, 2); Serial.println(" Hz"); - } +#endif // SERIAL_DEBUG #ifdef LCD display.clearDisplay(); diff --git a/examples/MPU9250_Debug/MPU9250_Debug.ino b/examples/MPU9250_Debug/MPU9250_Debug.ino index c294940..3ba20e6 100644 --- a/examples/MPU9250_Debug/MPU9250_Debug.ino +++ b/examples/MPU9250_Debug/MPU9250_Debug.ino @@ -20,6 +20,7 @@ void setup() ak8963Init(); //resetAndWake(); + Serial.println("Calling ak8963WhoAmI()"); ak8963WhoAmI(); checkSPI(); @@ -222,7 +223,6 @@ uint8_t writeByteSPI(uint8_t registerAddress, uint8_t writeData) uint8_t returnVal = SPI.transfer(writeData); SPI.endTransaction(); digitalWrite(CS_PIN, HIGH); - delayMicroseconds(50); // Not used initially, adding during debug #ifdef SERIAL_DEBUG Serial.print("BHW::writeByteSPI slave returned: 0x"); Serial.println(returnVal, HEX); diff --git a/src/MPU9250.cpp b/src/MPU9250.cpp index e190c26..976c78b 100644 --- a/src/MPU9250.cpp +++ b/src/MPU9250.cpp @@ -17,7 +17,7 @@ MPU9250::MPU9250(int8_t cspin /*=NOT_SPI*/) // Uses I2C communication by default _csPin = cspin; SPI.begin(); pinMode(_csPin, OUTPUT); - digitalWrite(_csPin, HIGH); + deselect(); } else { @@ -163,6 +163,7 @@ void MPU9250::initAK8963(float * destination) { // First extract the factory calibration for each magnetometer axis uint8_t rawData[3]; // x/y/z gyro calibration data stored here + // TODO: Test this!! Likely doesn't work writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer delay(10); writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode @@ -277,7 +278,7 @@ void MPU9250::calibrateMPU9250(float * gyroBias, float * accelBias) // reset device // Write a one to bit 7 reset bit; toggle reset device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, READ_FLAG); delay(100); // get stable time source; Auto select clock source to be PLL gyroscope @@ -690,38 +691,51 @@ void MPU9250::magCalMPU9250(float * bias_dest, float * scale_dest) } // Wire.h read and write protocols -void MPU9250::writeByte(uint8_t deviceAddress, uint8_t registerAddress, +uint8_t MPU9250::writeByte(uint8_t deviceAddress, uint8_t registerAddress, uint8_t data) { if (_csPin != NOT_SPI) { - writeByteSPI(registerAddress, data); + return writeByteSPI(registerAddress, data); } else { - writeByteWire(deviceAddress,registerAddress, data); + return writeByteWire(deviceAddress,registerAddress, data); } } -void MPU9250::writeByteSPI(uint8_t registerAddress, uint8_t data) +uint8_t MPU9250::writeByteSPI(uint8_t registerAddress, uint8_t writeData) { - SPI.beginTransaction(SPISettings(SPI_DATA_RATE, MSBFIRST, SPI_MODE0)); - digitalWrite(_csPin, LOW); - SPI.transfer(registerAddress & ~0x80); // set first bit to 0 - SPI.transfer(data); - digitalWrite(_csPin, HIGH); + uint8_t returnVal; + + SPI.beginTransaction(SPISettings(SPI_DATA_RATE, MSBFIRST, SPI_MODE)); + select(); + + SPI.transfer(registerAddress); + returnVal = SPI.transfer(writeData); + + deselect(); SPI.endTransaction(); +#ifdef SERIAL_DEBUG + Serial.print("MPU9250::writeByteSPI slave returned: 0x"); + Serial.println(returnVal, HEX); +#endif + return returnVal; } -void MPU9250::writeByteWire(uint8_t deviceAddress, uint8_t registerAddress, +uint8_t MPU9250::writeByteWire(uint8_t deviceAddress, uint8_t registerAddress, uint8_t data) { Wire.beginTransmission(deviceAddress); // Initialize the Tx buffer Wire.write(registerAddress); // Put slave register address in Tx buffer Wire.write(data); // Put data in Tx buffer Wire.endTransmission(); // Send the Tx buffer + // TODO: Fix this to return something meaningful + return NULL; } +// Read a byte from given register on device. Calls necessary SPI or I2C +// implementation. This was configured in the constructor. uint8_t MPU9250::readByte(uint8_t deviceAddress, uint8_t registerAddress) { if (_csPin != NOT_SPI) @@ -734,6 +748,7 @@ uint8_t MPU9250::readByte(uint8_t deviceAddress, uint8_t registerAddress) } } +// Read a byte from the given register address from device using I2C uint8_t MPU9250::readByteWire(uint8_t deviceAddress, uint8_t registerAddress) { uint8_t data; // `data` will store the register data @@ -752,64 +767,209 @@ uint8_t MPU9250::readByteWire(uint8_t deviceAddress, uint8_t registerAddress) return data; } +// Read a byte from the given register address using SPI uint8_t MPU9250::readByteSPI(uint8_t registerAddress) { - uint8_t value; - SPI.beginTransaction(SPISettings(SPI_DATA_RATE, MSBFIRST, SPI_MODE0)); + return writeByteSPI(registerAddress | READ_FLAG, 0xFF /*0xFF is arbitrary*/); +} + +// Read 1 or more bytes from given register and device using I2C +uint8_t MPU9250::readBytesWire(uint8_t deviceAddress, uint8_t registerAddress, + uint8_t count, uint8_t * dest) +{ + // Initialize the Tx buffer + Wire.beginTransmission(deviceAddress); + // Put slave register address in Tx buffer + Wire.write(registerAddress); + // Send the Tx buffer, but send a restart to keep connection alive + Wire.endTransmission(false); + + uint8_t i = 0; + // Read bytes from slave register address + Wire.requestFrom(deviceAddress, count); + while (Wire.available()) + { + // Put read results in the Rx buffer + dest[i++] = Wire.read(); + } + + return i; // Return number of bytes written +} + +// Select slave IC by asserting CS pin +void MPU9250::select() +{ digitalWrite(_csPin, LOW); - SPI.transfer(registerAddress | 0x80); // set first bit to 1 - value = SPI.transfer(0xFF); +} + +// Select slave IC by deasserting CS pin +void MPU9250::deselect() +{ digitalWrite(_csPin, HIGH); +} + +uint8_t MPU9250::readBytesSPI(uint8_t registerAddress, uint8_t count, + uint8_t * dest) +{ + SPI.beginTransaction(SPISettings(SPI_DATA_RATE, MSBFIRST, SPI_MODE)); + select(); + + SPI.transfer(registerAddress | READ_FLAG); + + uint8_t i; + + for (i = 0; i < count; i++) + { + dest[i] = SPI.transfer(0x00); +#ifdef SERIAL_DEBUG + Serial.print("readBytesSPI::Read byte: 0x"); + Serial.println(dest[i], HEX); +#endif + } + SPI.endTransaction(); - return value; + deselect(); + + delayMicroseconds(50); + + return i; // Return number of bytes written + + /* +#ifdef SERIAL_DEBUG + Serial.print("MPU9250::writeByteSPI slave returned: 0x"); + Serial.println(returnVal, HEX); +#endif + return returnVal; + */ + + /* + // Set slave address of AK8963 and set AK8963 for read + writeByteSPI(I2C_SLV0_ADDR, AK8963_ADDRESS | READ_FLAG); + +Serial.print("\nBHW::I2C_SLV0_ADDR set to: 0x"); +Serial.println(readByte(MPU9250_ADDRESS, I2C_SLV0_ADDR), HEX); + + // Set address to start read from + writeByteSPI(I2C_SLV0_REG, registerAddress); + // Read bytes from magnetometer + // +Serial.print("\nBHW::I2C_SLV0_CTRL gets 0x"); +Serial.println(READ_FLAG | count, HEX); + + // Read count bytes from registerAddress via I2C_SLV0 + Serial.print("BHW::readBytesSPI: return value test: "); + Serial.println(writeByteSPI(I2C_SLV0_CTRL, READ_FLAG | count)); + */ } -void MPU9250::readBytes(uint8_t deviceAddress, uint8_t registerAddress, +uint8_t MPU9250::readBytes(uint8_t deviceAddress, uint8_t registerAddress, uint8_t count, uint8_t * dest) { if (_csPin == NOT_SPI) // Read via I2C { - // Initialize the Tx buffer - Wire.beginTransmission(deviceAddress); - // Put slave register address in Tx buffer - Wire.write(registerAddress); - // Send the Tx buffer, but send a restart to keep connection alive - Wire.endTransmission(false); -Serial.print("BHW:: read bytes: 0x"); - uint8_t i = 0; - // Read bytes from slave register address - Wire.requestFrom(deviceAddress, count); - while (Wire.available()) - { - // Put read results in the Rx buffer - dest[i++] = Wire.read(); -Serial.print(dest[i-1], HEX); - } -Serial.print('\n'); + return readBytesWire(deviceAddress, registerAddress, count, dest); } else // Read using SPI { - SPI.beginTransaction(SPISettings(SPI_DATA_RATE, MSBFIRST, SPI_MODE0)); - digitalWrite(_csPin, LOW); - SPI.transfer(registerAddress | 0x80); // set first bit to 1 -Serial.print("BHW:: read bytes: "); - SPI.transfer(dest, count); - /* - uint8_t i = 0; - while (count > 0) - { - dest[i++] = SPI.transfer(0xFF); -Serial.print(dest[i-1], HEX); - count--; - } - */ - for (int i = 0; i < count; i++) - { - Serial.print(" 0x"); - Serial.print(dest[i], HEX); - } -Serial.print('\n'); - digitalWrite(_csPin, HIGH); - SPI.endTransaction(); // Done after de-asserting chip select to free bus + return readBytesSPI(registerAddress, count, dest); } } + +bool MPU9250::magInit() +{ + // Reset registers to defaults, bit auto clears + writeByteSPI(0x6B, 0x80); + // Auto select the best available clock source + writeByteSPI(0x6B, 0x01); + // Enable X,Y, & Z axes of accel and gyro + writeByteSPI(0x6C, 0x00); + // Config disable FSYNC pin, set gyro/temp bandwidth to 184/188 Hz + writeByteSPI(0x1A, 0x01); + // Self tests off, gyro set to +/-2000 dps FS + writeByteSPI(0x1B, 0x18); + // Self test off, accel set to +/- 8g FS + writeByteSPI(0x1C, 0x08); + // Bypass DLPF and set accel bandwidth to 184 Hz + writeByteSPI(0x1D, 0x09); + // Configure INT pin (active high / push-pull / latch until read) + writeByteSPI(0x37, 0x30); + // Enable I2C master mode + // TODO Why not do this 11-100 ms after power up? + writeByteSPI(0x6A, 0x20); + // Disable multi-master and set I2C master clock to 400 kHz + //https://developer.mbed.org/users/kylongmu/code/MPU9250_SPI/ calls says + // enabled multi-master... TODO Find out why + writeByteSPI(0x24, 0x0D); + // Set to write to slave address 0x0C + writeByteSPI(0x25, 0x0C); + // Point save 0 register at AK8963's control 2 (soft reset) register + writeByteSPI(0x26, 0x0B); + // Send 0x01 to AK8963 via slave 0 to trigger a soft restart + writeByteSPI(0x63, 0x01); + // Enable simple 1-byte I2C reads from slave 0 + writeByteSPI(0x27, 0x81); + // Point save 0 register at AK8963's control 1 (mode) register + writeByteSPI(0x26, 0x0A); + // 16-bit continuous measurement mode 1 + writeByteSPI(0x63, 0x12); + // Enable simple 1-byte I2C reads from slave 0 + writeByteSPI(0x27, 0x81); + + // TODO: Remove this code + uint8_t ret = ak8963WhoAmI_SPI(); +#ifdef SERIAL_DEBUG + Serial.print("MPU9250::magInit to return "); + Serial.println((ret == 0x48) ? "true" : "false"); +#endif + return ret == 0x48; +} + +// Write a null byte w/o CS assertion to get SPI hardware to idle high (mode 3) +void MPU9250::kickHardware() +{ + SPI.beginTransaction(SPISettings(SPI_DATA_RATE, MSBFIRST, SPI_MODE)); + SPI.transfer(0x00); // Send null byte + SPI.endTransaction(); +} + +bool MPU9250::begin() +{ + kickHardware(); + return magInit(); +} + +// Read the WHOAMI (WIA) register of the AK8963 +// TODO: This method has side effects +uint8_t MPU9250::ak8963WhoAmI_SPI() +{ + uint8_t response, oldSlaveAddress, oldSlaveRegister, oldSlaveConfig; + // Save state + oldSlaveAddress = readByteSPI(I2C_SLV0_ADDR); + oldSlaveRegister = readByteSPI(I2C_SLV0_REG); + oldSlaveConfig = readByteSPI(I2C_SLV0_CTRL); +#ifdef SERIAL_DEBUG + Serial.print("Old slave address: 0x"); + Serial.println(oldSlaveAddress, HEX); + Serial.print("Old slave register: 0x"); + Serial.println(oldSlaveRegister, HEX); + Serial.print("Old slave config: 0x"); + Serial.println(oldSlaveConfig, HEX); +#endif + + // Set the I2C slave addres of AK8963 and set for read + response = writeByteSPI(I2C_SLV0_ADDR, AK8963_ADDRESS|READ_FLAG); + // I2C slave 0 register address from where to begin data transfer + response = writeByteSPI(I2C_SLV0_REG, 0x00); + // Enable 1-byte reads on slave 0 + response = writeByteSPI(I2C_SLV0_CTRL, 0x81); + delayMicroseconds(1); + // Read WIA register + response = writeByteSPI(WHO_AM_I_AK8963|READ_FLAG, 0x00); + + // Restore state + writeByteSPI(I2C_SLV0_ADDR, oldSlaveAddress); + writeByteSPI(I2C_SLV0_REG, oldSlaveRegister); + writeByteSPI(I2C_SLV0_CTRL, oldSlaveConfig); + + return response; +} diff --git a/src/MPU9250.h b/src/MPU9250.h index ca18586..3b204ed 100644 --- a/src/MPU9250.h +++ b/src/MPU9250.h @@ -11,6 +11,8 @@ #include #include +#define SERIAL_DEBUG true + // 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 above // document; the MPU9250 and MPU9150 are virtually identical but the latter has @@ -18,7 +20,7 @@ //Magnetometer Registers #define AK8963_ADDRESS 0x0C -#define WHO_AM_I_AK8963 0x00 // (AKA WIA) should return 0x48 +#define WHO_AM_I_AK8963 0x49 // (AKA WIA) should return 0x48 #define INFO 0x01 #define AK8963_ST1 0x02 // data ready status bit 0 #define AK8963_XOUT_L 0x03 // data @@ -177,8 +179,11 @@ #define AK8963_ADDRESS 0x0C // Address of magnetometer #endif // AD0 +#define READ_FLAG 0x80 #define NOT_SPI -1 #define SPI_DATA_RATE 1000000 // 1MHz is the max speed of the MPU-9250 +//#define SPI_DATA_RATE 1000000 // 1MHz is the max speed of the MPU-9250 +#define SPI_MODE SPI_MODE3 class MPU9250 { @@ -222,10 +227,17 @@ class MPU9250 // SPI chip select pin int8_t _csPin; - void writeByteWire(uint8_t, uint8_t, uint8_t); - void writeByteSPI(uint8_t, uint8_t); + uint8_t writeByteWire(uint8_t, uint8_t, uint8_t); + uint8_t writeByteSPI(uint8_t, uint8_t); uint8_t readByteSPI(uint8_t subAddress); uint8_t readByteWire(uint8_t address, uint8_t subAddress); + bool magInit(); + void kickHardware(); + void select(); + void deselect(); +// TODO: Remove this next line +public: + uint8_t ak8963WhoAmI_SPI(); public: float pitch, yaw, roll; @@ -270,10 +282,14 @@ class MPU9250 void calibrateMPU9250(float * gyroBias, float * accelBias); void MPU9250SelfTest(float * destination); void magCalMPU9250(float * dest1, float * dest2); - void writeByte(uint8_t, uint8_t, uint8_t); + uint8_t writeByte(uint8_t, uint8_t, uint8_t); uint8_t readByte(uint8_t, uint8_t); - void readBytes(uint8_t, uint8_t, uint8_t, uint8_t *); + uint8_t readBytes(uint8_t, uint8_t, uint8_t, uint8_t *); + // TODO: make SPI/Wire private + uint8_t readBytesSPI(uint8_t, uint8_t, uint8_t *); + uint8_t readBytesWire(uint8_t, uint8_t, uint8_t, uint8_t *); bool isInI2cMode() { return _csPin == -1; } + bool begin(); }; // class MPU9250 #endif // _MPU9250_H_ From 69eb8325d492f1e2229e8c4c987b65177fc332d0 Mon Sep 17 00:00:00 2001 From: Ben Anderson Date: Mon, 27 Feb 2017 17:09:07 +0000 Subject: [PATCH 17/28] Update MPU9250.h Changed WHO_AM_I_AK8963 from 0x49 to 0x00 - otherwise initialisation fails --- src/MPU9250.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/MPU9250.h b/src/MPU9250.h index 3b204ed..3169318 100644 --- a/src/MPU9250.h +++ b/src/MPU9250.h @@ -20,7 +20,7 @@ //Magnetometer Registers #define AK8963_ADDRESS 0x0C -#define WHO_AM_I_AK8963 0x49 // (AKA WIA) should return 0x48 +#define WHO_AM_I_AK8963 0x00 // (AKA WIA) should return 0x48 #define INFO 0x01 #define AK8963_ST1 0x02 // data ready status bit 0 #define AK8963_XOUT_L 0x03 // data From 01bfca8cdedd9945f16f0bdb107152b95321c015 Mon Sep 17 00:00:00 2001 From: Owen L - SFE Date: Wed, 1 Aug 2018 09:38:53 -0600 Subject: [PATCH 18/28] Fixed addresses - Changed magnetometer WAI register to 0x00 as per datasheet and pull request - Removed preprocessor logic to set up the defined MPU9250_ADDRESS, instead allowing the user to define that value as one of the two options MPU9250_ADDRESS_AD0 or MPU9250_ADDRESS_AD1 --- src/MPU9250.h | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/MPU9250.h b/src/MPU9250.h index 3b204ed..8af8b87 100644 --- a/src/MPU9250.h +++ b/src/MPU9250.h @@ -19,8 +19,7 @@ // a different register map //Magnetometer Registers -#define AK8963_ADDRESS 0x0C -#define WHO_AM_I_AK8963 0x49 // (AKA WIA) should return 0x48 +#define WHO_AM_I_AK8963 0x00 // (AKA WIA) should return 0x48 #define INFO 0x01 #define AK8963_ST1 0x02 // data ready status bit 0 #define AK8963_XOUT_L 0x03 // data @@ -171,13 +170,12 @@ // Using the MPU-9250 breakout board, ADO is set to 0 // Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 -#define ADO 0 -#if ADO -#define MPU9250_ADDRESS 0x69 // Device address when ADO = 1 -#else -#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0 +// The previous preprocessor directives were sensitive to the location that the user defined AD1 +// Now simply define MPU9250_ADDRESS as one of the two following depending on your application +#define MPU9250_ADDRESS_AD1 0x69 // Device address when ADO = 1 +#define MPU9250_ADDRESS_AD0 0x68 // Device address when ADO = 0 #define AK8963_ADDRESS 0x0C // Address of magnetometer -#endif // AD0 + #define READ_FLAG 0x80 #define NOT_SPI -1 From 4fbe1256d36ec48ddc43f6dcf1e381b062ffa862 Mon Sep 17 00:00:00 2001 From: Owen L - SFE Date: Wed, 1 Aug 2018 10:12:08 -0600 Subject: [PATCH 19/28] Added support for additional SPI or I2C ports --- .../MPU9250BasicAHRS_I2C.ino | 9 +- src/MPU9250.cpp | 227 +++++++++--------- src/MPU9250.h | 7 +- 3 files changed, 131 insertions(+), 112 deletions(-) diff --git a/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino b/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino index 5174fc1..72fea75 100644 --- a/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino +++ b/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino @@ -27,6 +27,8 @@ #include "quaternionFilters.h" #include "MPU9250.h" + + #ifdef LCD #include #include @@ -47,7 +49,12 @@ Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins int myLed = 13; // Set up pin 13 led for toggling -MPU9250 myIMU; +#define I2C_clock 400000 +#define I2C_port Wire +#define MPU9250_ADDRESS MPU9250_ADDRESS_AD0 // Use either this line or the next to select which I2C address your device is using +//#define MPU9250_ADDRESS MPU9250_ADDRESS_AD1 + +MPU9250 myIMU(MPU9250_ADDRESS, I2C_port, I2C_clock); void setup() { diff --git a/src/MPU9250.cpp b/src/MPU9250.cpp index 976c78b..988a675 100644 --- a/src/MPU9250.cpp +++ b/src/MPU9250.cpp @@ -5,25 +5,32 @@ //====== and temperature data //============================================================================== -MPU9250::MPU9250(int8_t cspin /*=NOT_SPI*/) // Uses I2C communication by default +MPU9250::MPU9250( int8_t csPin, SPIClass &spiInterface ) { - // Use hardware SPI communication - // If used with sparkfun breakout board - // https://www.sparkfun.com/products/13762 , change the pre-soldered JP2 to - // enable SPI (solder middle and left instead of middle and right) pads are - // very small and re-soldering can be very tricky. I2C highly recommended. - if ((cspin > NOT_SPI) && (cspin < NUM_DIGITAL_PINS)) - { - _csPin = cspin; - SPI.begin(); + // Use hardware SPI communication + // If used with sparkfun breakout board + // https://www.sparkfun.com/products/13762 , change the pre-soldered JP2 to + // enable SPI (solder middle and left instead of middle and right) pads are + // very small and re-soldering can be very tricky. I2C highly recommended. + + _csPin = csPin; + _spi = &spiInterface; + _wire = NULL; + + _spi->begin(); pinMode(_csPin, OUTPUT); deselect(); - } - else - { - _csPin = NOT_SPI; - Wire.begin(); - } +} +MPU9250::MPU9250( uint8_t address, TwoWire &wirePort, uint32_t clock_frequency ) +{ + _I2Caddr = address; + _wire = &wirePort; + _spi = NULL; + + _csPin = NOT_SPI; // Used to tell the library that the sensor is using I2C + + _wire->begin(); + _wire->setClock(clock_frequency); } void MPU9250::getMres() @@ -92,7 +99,7 @@ void MPU9250::readAccelData(int16_t * destination) { uint8_t rawData[6]; // x/y/z accel register data stored here // Read the six raw data registers into data array - readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); + readBytes(_I2Caddr, ACCEL_XOUT_H, 6, &rawData[0]); // Turn the MSB and LSB into a signed 16-bit value destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; @@ -105,7 +112,7 @@ void MPU9250::readGyroData(int16_t * destination) { uint8_t rawData[6]; // x/y/z gyro register data stored here // Read the six raw data registers sequentially into data array - readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); + readBytes(_I2Caddr, GYRO_XOUT_H, 6, &rawData[0]); // Turn the MSB and LSB into a signed 16-bit value destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; @@ -140,7 +147,7 @@ int16_t MPU9250::readTempData() { uint8_t rawData[2]; // x/y/z gyro register data stored here // Read the two raw data registers sequentially into data array - readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); + readBytes(_I2Caddr, TEMP_OUT_H, 2, &rawData[0]); // Turn the MSB and LSB into a 16-bit value return ((int16_t)rawData[0] << 8) | rawData[1]; } @@ -193,12 +200,12 @@ void MPU9250::initMPU9250() { // wake up device // Clear sleep mode bit (6), enable all sensors - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); + writeByte(_I2Caddr, PWR_MGMT_1, 0x00); delay(100); // Wait for all registers to reset // Get stable time source // Auto select clock source to be PLL gyroscope reference if ready else - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); + writeByte(_I2Caddr, PWR_MGMT_1, 0x01); delay(200); // Configure Gyro and Thermometer @@ -209,19 +216,19 @@ void MPU9250::initMPU9250() // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), // 8 kHz, or 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x03); + writeByte(_I2Caddr, CONFIG, 0x03); // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) // Use a 200 Hz rate; a rate consistent with the filter update rate // determined inset in CONFIG above. - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); + writeByte(_I2Caddr, SMPLRT_DIV, 0x04); // Set gyroscope full scale range // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are // left-shifted into positions 4:3 // get current GYRO_CONFIG register value - uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); + uint8_t c = readByte(_I2Caddr, GYRO_CONFIG); // c = c & ~0xE0; // Clear self-test bits [7:5] c = c & ~0x02; // Clear Fchoice bits [1:0] c = c & ~0x18; // Clear AFS bits [4:3] @@ -230,27 +237,27 @@ void MPU9250::initMPU9250() // GYRO_CONFIG // c =| 0x00; // Write new GYRO_CONFIG value to register - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); + writeByte(_I2Caddr, GYRO_CONFIG, c ); // Set accelerometer full-scale range configuration // Get current ACCEL_CONFIG register value - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); + c = readByte(_I2Caddr, ACCEL_CONFIG); // c = c & ~0xE0; // Clear self-test bits [7:5] c = c & ~0x18; // Clear AFS bits [4:3] c = c | Ascale << 3; // Set full scale range for the accelerometer // Write new ACCEL_CONFIG register value - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); + writeByte(_I2Caddr, ACCEL_CONFIG, c); // Set accelerometer sample rate configuration // It is possible to get a 4 kHz sample rate from the accelerometer by // choosing 1 for accel_fchoice_b bit [3]; in this case the bandwidth is // 1.13 kHz // Get current ACCEL_CONFIG2 register value - c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); + c = readByte(_I2Caddr, ACCEL_CONFIG2); c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz // Write new ACCEL_CONFIG2 register value - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); + writeByte(_I2Caddr, ACCEL_CONFIG2, c); // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, // but all these rates are further reduced by a factor of 5 to 200 Hz because // of the SMPLRT_DIV setting @@ -260,9 +267,9 @@ void MPU9250::initMPU9250() // until interrupt cleared, clear on read of INT_STATUS, and enable // I2C_BYPASS_EN so additional chips can join the I2C bus and all can be // controlled by the Arduino as master. - writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22); + writeByte(_I2Caddr, INT_PIN_CFG, 0x22); // Enable data ready (bit 0) interrupt - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); + writeByte(_I2Caddr, INT_ENABLE, 0x01); delay(100); } @@ -278,55 +285,55 @@ void MPU9250::calibrateMPU9250(float * gyroBias, float * accelBias) // reset device // Write a one to bit 7 reset bit; toggle reset device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, READ_FLAG); + writeByte(_I2Caddr, PWR_MGMT_1, READ_FLAG); delay(100); // get stable time source; Auto select clock source to be PLL gyroscope // reference if ready else use the internal oscillator, bits 2:0 = 001 - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); - writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); + writeByte(_I2Caddr, PWR_MGMT_1, 0x01); + writeByte(_I2Caddr, PWR_MGMT_2, 0x00); delay(200); // Configure device for bias calculation // Disable all interrupts - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); + writeByte(_I2Caddr, INT_ENABLE, 0x00); // Disable FIFO - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); + writeByte(_I2Caddr, FIFO_EN, 0x00); // Turn on internal clock source - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); + writeByte(_I2Caddr, PWR_MGMT_1, 0x00); // Disable I2C master - writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); + writeByte(_I2Caddr, I2C_MST_CTRL, 0x00); // Disable FIFO and I2C master modes - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); + writeByte(_I2Caddr, USER_CTRL, 0x00); // Reset FIFO and DMP - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); + writeByte(_I2Caddr, USER_CTRL, 0x0C); delay(15); // Configure MPU6050 gyro and accelerometer for bias calculation // Set low-pass filter to 188 Hz - writeByte(MPU9250_ADDRESS, CONFIG, 0x01); + writeByte(_I2Caddr, CONFIG, 0x01); // Set sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); + writeByte(_I2Caddr, SMPLRT_DIV, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); + writeByte(_I2Caddr, GYRO_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); + writeByte(_I2Caddr, ACCEL_CONFIG, 0x00); uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec uint16_t accelsensitivity = 16384; // = 16384 LSB/g // Configure FIFO to capture accelerometer and gyro data for bias calculation - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO + writeByte(_I2Caddr, USER_CTRL, 0x40); // Enable FIFO // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in // MPU-9150) - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); + writeByte(_I2Caddr, FIFO_EN, 0x78); delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes // At end of sample accumulation, turn off FIFO sensor read // Disable gyro and accelerometer sensors for FIFO - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); + writeByte(_I2Caddr, FIFO_EN, 0x00); // Read FIFO sample count - readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); + readBytes(_I2Caddr, FIFO_COUNTH, 2, &data[0]); fifo_count = ((uint16_t)data[0] << 8) | data[1]; // How many sets of full gyro and accelerometer data for averaging packet_count = fifo_count/12; @@ -335,7 +342,7 @@ void MPU9250::calibrateMPU9250(float * gyroBias, float * accelBias) { int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; // Read data for averaging - readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); + readBytes(_I2Caddr, FIFO_R_W, 12, &data[0]); // Form signed 16-bit integer for each sample in FIFO accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ); accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ); @@ -384,12 +391,12 @@ void MPU9250::calibrateMPU9250(float * gyroBias, float * accelBias) data[5] = (-gyro_bias[2]/4) & 0xFF; // Push gyro biases to hardware registers - writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]); + writeByte(_I2Caddr, XG_OFFSET_H, data[0]); + writeByte(_I2Caddr, XG_OFFSET_L, data[1]); + writeByte(_I2Caddr, YG_OFFSET_H, data[2]); + writeByte(_I2Caddr, YG_OFFSET_L, data[3]); + writeByte(_I2Caddr, ZG_OFFSET_H, data[4]); + writeByte(_I2Caddr, ZG_OFFSET_L, data[5]); // Output scaled gyro biases for display in the main program gyroBias[0] = (float) gyro_bias[0]/(float) gyrosensitivity; @@ -407,11 +414,11 @@ void MPU9250::calibrateMPU9250(float * gyroBias, float * accelBias) // A place to hold the factory accelerometer trim biases int32_t accel_bias_reg[3] = {0, 0, 0}; // Read factory accelerometer trim values - readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); + readBytes(_I2Caddr, XA_OFFSET_H, 2, &data[0]); accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); + readBytes(_I2Caddr, YA_OFFSET_H, 2, &data[0]); accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); + readBytes(_I2Caddr, ZA_OFFSET_H, 2, &data[0]); accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]); // Define mask for temperature compensation bit 0 of lower byte of @@ -456,12 +463,12 @@ void MPU9250::calibrateMPU9250(float * gyroBias, float * accelBias) // Apparently this is not working for the acceleration biases in the MPU-9250 // Are we handling the temperature correction bit properly? // Push accelerometer biases to hardware registers - writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); + writeByte(_I2Caddr, XA_OFFSET_H, data[0]); + writeByte(_I2Caddr, XA_OFFSET_L, data[1]); + writeByte(_I2Caddr, YA_OFFSET_H, data[2]); + writeByte(_I2Caddr, YA_OFFSET_L, data[3]); + writeByte(_I2Caddr, ZA_OFFSET_H, data[4]); + writeByte(_I2Caddr, ZA_OFFSET_L, data[5]); // Output scaled accelerometer biases for display in the main program accelBias[0] = (float)accel_bias[0]/(float)accelsensitivity; @@ -482,15 +489,15 @@ void MPU9250::MPU9250SelfTest(float * destination) uint8_t FS = 0; // Set gyro sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); + writeByte(_I2Caddr, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz - writeByte(MPU9250_ADDRESS, CONFIG, 0x02); + writeByte(_I2Caddr, CONFIG, 0x02); // Set full scale range for the gyro to 250 dps - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<beginTransmission(deviceAddress); // Initialize the Tx buffer + _wire->write(registerAddress); // Put slave register address in Tx buffer + _wire->write(data); // Put data in Tx buffer + _wire->endTransmission(); // Send the Tx buffer // TODO: Fix this to return something meaningful return NULL; } @@ -754,15 +761,15 @@ uint8_t MPU9250::readByteWire(uint8_t deviceAddress, uint8_t registerAddress) uint8_t data; // `data` will store the register data // Initialize the Tx buffer - Wire.beginTransmission(deviceAddress); + _wire->beginTransmission(deviceAddress); // Put slave register address in Tx buffer - Wire.write(registerAddress); + _wire->write(registerAddress); // Send the Tx buffer, but send a restart to keep connection alive - Wire.endTransmission(false); + _wire->endTransmission(false); // Read one byte from slave register address - Wire.requestFrom(deviceAddress, (uint8_t) 1); + _wire->requestFrom(deviceAddress, (uint8_t) 1); // Fill Rx buffer with result - data = Wire.read(); + data = _wire->read(); // Return data read from slave register return data; } @@ -778,19 +785,19 @@ uint8_t MPU9250::readBytesWire(uint8_t deviceAddress, uint8_t registerAddress, uint8_t count, uint8_t * dest) { // Initialize the Tx buffer - Wire.beginTransmission(deviceAddress); + _wire->beginTransmission(deviceAddress); // Put slave register address in Tx buffer - Wire.write(registerAddress); + _wire->write(registerAddress); // Send the Tx buffer, but send a restart to keep connection alive - Wire.endTransmission(false); + _wire->endTransmission(false); uint8_t i = 0; // Read bytes from slave register address - Wire.requestFrom(deviceAddress, count); - while (Wire.available()) + _wire->requestFrom(deviceAddress, count); + while (_wire->available()) { // Put read results in the Rx buffer - dest[i++] = Wire.read(); + dest[i++] = _wire->read(); } return i; // Return number of bytes written @@ -811,23 +818,23 @@ void MPU9250::deselect() uint8_t MPU9250::readBytesSPI(uint8_t registerAddress, uint8_t count, uint8_t * dest) { - SPI.beginTransaction(SPISettings(SPI_DATA_RATE, MSBFIRST, SPI_MODE)); + _spi->beginTransaction(SPISettings(SPI_DATA_RATE, MSBFIRST, SPI_MODE)); select(); - SPI.transfer(registerAddress | READ_FLAG); + _spi->transfer(registerAddress | READ_FLAG); uint8_t i; for (i = 0; i < count; i++) { - dest[i] = SPI.transfer(0x00); + dest[i] = _spi->transfer(0x00); #ifdef SERIAL_DEBUG Serial.print("readBytesSPI::Read byte: 0x"); Serial.println(dest[i], HEX); #endif } - SPI.endTransaction(); + _spi->endTransaction(); deselect(); delayMicroseconds(50); @@ -847,7 +854,7 @@ uint8_t MPU9250::readBytesSPI(uint8_t registerAddress, uint8_t count, writeByteSPI(I2C_SLV0_ADDR, AK8963_ADDRESS | READ_FLAG); Serial.print("\nBHW::I2C_SLV0_ADDR set to: 0x"); -Serial.println(readByte(MPU9250_ADDRESS, I2C_SLV0_ADDR), HEX); +Serial.println(readByte(_I2Caddr, I2C_SLV0_ADDR), HEX); // Set address to start read from writeByteSPI(I2C_SLV0_REG, registerAddress); @@ -927,9 +934,9 @@ bool MPU9250::magInit() // Write a null byte w/o CS assertion to get SPI hardware to idle high (mode 3) void MPU9250::kickHardware() { - SPI.beginTransaction(SPISettings(SPI_DATA_RATE, MSBFIRST, SPI_MODE)); - SPI.transfer(0x00); // Send null byte - SPI.endTransaction(); + _spi->beginTransaction(SPISettings(SPI_DATA_RATE, MSBFIRST, SPI_MODE)); + _spi->transfer(0x00); // Send null byte + _spi->endTransaction(); } bool MPU9250::begin() diff --git a/src/MPU9250.h b/src/MPU9250.h index 8af8b87..4023555 100644 --- a/src/MPU9250.h +++ b/src/MPU9250.h @@ -212,6 +212,10 @@ class MPU9250 M_100HZ = 0x06 // 100 Hz continuous magnetometer }; + uint8_t _I2Caddr = MPU9250_ADDRESS_AD0; // Use AD0 by default + SPIClass * _spi; // Allows for use of different SPI ports + TwoWire * _wire; // Allows for use of various I2C ports + // TODO: Add setter methods for this hard coded stuff // Specify sensor full scale uint8_t Gscale = GFS_250DPS; @@ -266,7 +270,8 @@ class MPU9250 int16_t accelCount[3]; // Public method declarations - MPU9250(int8_t csPin=NOT_SPI); + MPU9250( int8_t _csPin, SPIClass &spiInterface = SPI); + MPU9250( uint8_t address = MPU9250_ADDRESS_AD0, TwoWire &wirePort = Wire, uint32_t clock_frequency = 100000 ); void getMres(); void getGres(); void getAres(); From 92f6d455931a979d9e1f0790aac71b2f0b926ec7 Mon Sep 17 00:00:00 2001 From: Owen L - SFE Date: Wed, 1 Aug 2018 10:34:33 -0600 Subject: [PATCH 20/28] Updated examples --- .../MPU9250BasicAHRS_I2C.ino | 6 ++-- .../MPU9250BasicAHRS_SPI.ino | 12 +++++++- src/MPU9250.cpp | 28 +++++++++++-------- src/MPU9250.h | 16 +++++++---- 4 files changed, 41 insertions(+), 21 deletions(-) diff --git a/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino b/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino index 72fea75..3c3c20e 100644 --- a/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino +++ b/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino @@ -49,12 +49,12 @@ Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins int myLed = 13; // Set up pin 13 led for toggling -#define I2C_clock 400000 -#define I2C_port Wire +#define I2Cclock 400000 +#define I2Cport Wire #define MPU9250_ADDRESS MPU9250_ADDRESS_AD0 // Use either this line or the next to select which I2C address your device is using //#define MPU9250_ADDRESS MPU9250_ADDRESS_AD1 -MPU9250 myIMU(MPU9250_ADDRESS, I2C_port, I2C_clock); +MPU9250 myIMU(MPU9250_ADDRESS, I2Cport, I2Cclock); void setup() { diff --git a/examples/MPU9250BasicAHRS_SPI/MPU9250BasicAHRS_SPI.ino b/examples/MPU9250BasicAHRS_SPI/MPU9250BasicAHRS_SPI.ino index d00f181..a3d3295 100644 --- a/examples/MPU9250BasicAHRS_SPI/MPU9250BasicAHRS_SPI.ino +++ b/examples/MPU9250BasicAHRS_SPI/MPU9250BasicAHRS_SPI.ino @@ -42,7 +42,17 @@ Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); #define AHRS true // Set to false for basic data read #define SERIAL_DEBUG true // Set to true to get Serial output for debugging -MPU9250 myIMU = MPU9250(2); // Using digital pin to for chip select in demo +#define SPIport SPI +#define CSpin 2 +#define SPIrate 1000000 +MPU9250 myIMU = MPU9250(CSpin, SPIport, SPIrate); // Using digital pin to for chip select in demo + + +// Because of the way this library is written you still need to send SOME address - +// but in SPI mode it is not used, so I chose over 9000 - By the way if you look closely +// there will be a truncation warning during compilation because 9001 is too large for +// the parameter's type. +#define MPU9250_ADDRESS 9001 void setup() { diff --git a/src/MPU9250.cpp b/src/MPU9250.cpp index 988a675..ab836ea 100644 --- a/src/MPU9250.cpp +++ b/src/MPU9250.cpp @@ -5,7 +5,7 @@ //====== and temperature data //============================================================================== -MPU9250::MPU9250( int8_t csPin, SPIClass &spiInterface ) +MPU9250::MPU9250( int8_t csPin, SPIClass &spiInterface, uint32_t spi_freq ) { // Use hardware SPI communication // If used with sparkfun breakout board @@ -17,6 +17,8 @@ MPU9250::MPU9250( int8_t csPin, SPIClass &spiInterface ) _spi = &spiInterface; _wire = NULL; + _interfaceSpeed = spi_freq; + _spi->begin(); pinMode(_csPin, OUTPUT); deselect(); @@ -27,10 +29,12 @@ MPU9250::MPU9250( uint8_t address, TwoWire &wirePort, uint32_t clock_frequency ) _wire = &wirePort; _spi = NULL; + _interfaceSpeed = clock_frequency; + _csPin = NOT_SPI; // Used to tell the library that the sensor is using I2C _wire->begin(); - _wire->setClock(clock_frequency); + _wire->setClock(_interfaceSpeed); } void MPU9250::getMres() @@ -612,8 +616,8 @@ void MPU9250::magCalMPU9250(float * bias_dest, float * scale_dest) uint16_t ii = 0, sample_count = 0; int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0}; - int16_t mag_max[3] = {0x8000, 0x8000, 0x8000}, - mag_min[3] = {0x7FFF, 0x7FFF, 0x7FFF}, + int16_t mag_max[3] = {-32768, -32768, -32768}, // Wrote out decimal (signed) values to remove a conversion warning + mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0}; // Make sure resolution has been calculated @@ -715,7 +719,7 @@ uint8_t MPU9250::writeByteSPI(uint8_t registerAddress, uint8_t writeData) { uint8_t returnVal; - _spi->beginTransaction(SPISettings(SPI_DATA_RATE, MSBFIRST, SPI_MODE)); + _spi->beginTransaction(SPISettings(_interfaceSpeed, MSBFIRST, SPI_MODE)); select(); _spi->transfer(registerAddress); @@ -733,12 +737,14 @@ uint8_t MPU9250::writeByteSPI(uint8_t registerAddress, uint8_t writeData) uint8_t MPU9250::writeByteWire(uint8_t deviceAddress, uint8_t registerAddress, uint8_t data) { - _wire->beginTransmission(deviceAddress); // Initialize the Tx buffer - _wire->write(registerAddress); // Put slave register address in Tx buffer - _wire->write(data); // Put data in Tx buffer - _wire->endTransmission(); // Send the Tx buffer - // TODO: Fix this to return something meaningful - return NULL; + _wire->setClock(_interfaceSpeed); // Reset to the desired speed, in case other devices required a slowdown + _wire->beginTransmission(deviceAddress); // Initialize the Tx buffer + _wire->write(registerAddress); // Put slave register address in Tx buffer + _wire->write(data); // Put data in Tx buffer + _wire->endTransmission(); // Send the Tx buffer + // TODO: Fix this to return something meaningful + // return NULL; // In the meantime fix it to return the right type + return 0; } // Read a byte from given register on device. Calls necessary SPI or I2C diff --git a/src/MPU9250.h b/src/MPU9250.h index 4023555..d256dc6 100644 --- a/src/MPU9250.h +++ b/src/MPU9250.h @@ -180,7 +180,6 @@ #define READ_FLAG 0x80 #define NOT_SPI -1 #define SPI_DATA_RATE 1000000 // 1MHz is the max speed of the MPU-9250 -//#define SPI_DATA_RATE 1000000 // 1MHz is the max speed of the MPU-9250 #define SPI_MODE SPI_MODE3 class MPU9250 @@ -212,9 +211,14 @@ class MPU9250 M_100HZ = 0x06 // 100 Hz continuous magnetometer }; - uint8_t _I2Caddr = MPU9250_ADDRESS_AD0; // Use AD0 by default - SPIClass * _spi; // Allows for use of different SPI ports + TwoWire * _wire; // Allows for use of various I2C ports + uint8_t _I2Caddr = MPU9250_ADDRESS_AD0; // Use AD0 by default + + SPIClass * _spi; // Allows for use of different SPI ports + int8_t _csPin; // SPI chip select pin + + uint32_t _interfaceSpeed; // Stores the desired I2C or SPi clock rate // TODO: Add setter methods for this hard coded stuff // Specify sensor full scale @@ -226,8 +230,8 @@ class MPU9250 // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read uint8_t Mmode = M_8HZ; - // SPI chip select pin - int8_t _csPin; + + uint8_t writeByteWire(uint8_t, uint8_t, uint8_t); uint8_t writeByteSPI(uint8_t, uint8_t); @@ -270,7 +274,7 @@ class MPU9250 int16_t accelCount[3]; // Public method declarations - MPU9250( int8_t _csPin, SPIClass &spiInterface = SPI); + MPU9250( int8_t _csPin, SPIClass &spiInterface = SPI, uint32_t spi_freq = SPI_DATA_RATE); MPU9250( uint8_t address = MPU9250_ADDRESS_AD0, TwoWire &wirePort = Wire, uint32_t clock_frequency = 100000 ); void getMres(); void getGres(); From d2e0a9b70a363d5284a1e08a21c9fd283c4ae3c7 Mon Sep 17 00:00:00 2001 From: Owen L - SFE Date: Wed, 1 Aug 2018 16:47:35 -0600 Subject: [PATCH 21/28] Added unit tester for I2C --- .../Example9999_UnitTest.ino | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 examples/Example9999_UnitTest/Example9999_UnitTest.ino diff --git a/examples/Example9999_UnitTest/Example9999_UnitTest.ino b/examples/Example9999_UnitTest/Example9999_UnitTest.ino new file mode 100644 index 0000000..80ff25c --- /dev/null +++ b/examples/Example9999_UnitTest/Example9999_UnitTest.ino @@ -0,0 +1,37 @@ +#include "MPU9250.h" + +#define I2Cclock 400000 +#define I2Cport Wire +#define MPU9250_ADDRESS MPU9250_ADDRESS_AD0 // Use either this line or the next to select which I2C address your device is using +//#define MPU9250_ADDRESS MPU9250_ADDRESS_AD1 + +MPU9250 myIMU(MPU9250_ADDRESS, I2Cport, I2Cclock); + + byte c = 0x00; + byte d = 0x00; + bool ledOn = true; + +void setup() { + // put your setup code here, to run once: + Serial.begin(38400); + while(!Serial){}; + + pinMode(13, OUTPUT); + +} + +void loop() { + // put your main code here, to run repeatedly: + + c = myIMU.readByte(MPU9250_ADDRESS_AD0, WHO_AM_I_MPU9250); + d = myIMU.readByte(MPU9250_ADDRESS_AD1, WHO_AM_I_MPU9250); + + Serial.print("Received AD0: 0x"); + Serial.print(c, HEX); + Serial.print(", AD1: 0x"); + Serial.println(d, HEX); + digitalWrite(13, ledOn); + ledOn = !ledOn; + delay(100); + +} From 8f7866f574601a306d906a7c17bce75df8a18f53 Mon Sep 17 00:00:00 2001 From: Owen L - SFE Date: Thu, 2 Aug 2018 09:54:13 -0600 Subject: [PATCH 22/28] Fixed address definition problem after merge --- .../MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino | 2 +- src/MPU9250.cpp | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino b/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino index 3c3c20e..a55d391 100644 --- a/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino +++ b/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino @@ -42,7 +42,7 @@ Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); #endif // LCD -#define AHRS true // Set to false for basic data read +#define AHRS false // Set to false for basic data read #define SerialDebug true // Set to true to get Serial output for debugging // Pin definitions diff --git a/src/MPU9250.cpp b/src/MPU9250.cpp index 6d81640..6dd9d57 100644 --- a/src/MPU9250.cpp +++ b/src/MPU9250.cpp @@ -492,20 +492,20 @@ void MPU9250::MPU9250SelfTest(float * destination) float factoryTrim[6]; uint8_t FS = GFS_250DPS; - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, FS<<3); // Set full scale range for the gyro to 250 dps - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, FS<<3); // Set full scale range for the accelerometer to 2 g + writeByte(_I2Caddr, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz + writeByte(_I2Caddr, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz + writeByte(_I2Caddr, GYRO_CONFIG, FS<<3); // Set full scale range for the gyro to 250 dps + writeByte(_I2Caddr, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz + writeByte(_I2Caddr, ACCEL_CONFIG, FS<<3); // Set full scale range for the accelerometer to 2 g for( int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer - readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array + readBytes(_I2Caddr, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; - readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array + readBytes(_I2Caddr, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; From c949409a6ffa05b9d421096f40ebb824c1003b3f Mon Sep 17 00:00:00 2001 From: Owen L - SFE Date: Thu, 2 Aug 2018 14:30:43 -0600 Subject: [PATCH 23/28] rename --- src/MPU9250.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/MPU9250.h b/src/MPU9250.h index d256dc6..815ba8a 100644 --- a/src/MPU9250.h +++ b/src/MPU9250.h @@ -274,7 +274,7 @@ class MPU9250 int16_t accelCount[3]; // Public method declarations - MPU9250( int8_t _csPin, SPIClass &spiInterface = SPI, uint32_t spi_freq = SPI_DATA_RATE); + MPU9250( int8_t csPin, SPIClass &spiInterface = SPI, uint32_t spi_freq = SPI_DATA_RATE); MPU9250( uint8_t address = MPU9250_ADDRESS_AD0, TwoWire &wirePort = Wire, uint32_t clock_frequency = 100000 ); void getMres(); void getGres(); From e722caf3a9b7a99da63bdf75fcb51384246cea6a Mon Sep 17 00:00:00 2001 From: Owen L - SFE Date: Mon, 6 Aug 2018 14:26:11 -0600 Subject: [PATCH 24/28] Tried to extend SPI mode to be able to access magnetometer --- .../Example9999_UnitTest.ino | 8 +- .../MPU9250BasicAHRS_I2C.ino | 10 +- .../MPU9250BasicAHRS_SPI.ino | 471 +++++++++++------- src/MPU9250.cpp | 94 +++- src/MPU9250.h | 6 + 5 files changed, 385 insertions(+), 204 deletions(-) diff --git a/examples/Example9999_UnitTest/Example9999_UnitTest.ino b/examples/Example9999_UnitTest/Example9999_UnitTest.ino index 80ff25c..79a3059 100644 --- a/examples/Example9999_UnitTest/Example9999_UnitTest.ino +++ b/examples/Example9999_UnitTest/Example9999_UnitTest.ino @@ -1,3 +1,4 @@ +#include "quaternionFilters.h" #include "MPU9250.h" #define I2Cclock 400000 @@ -5,7 +6,8 @@ #define MPU9250_ADDRESS MPU9250_ADDRESS_AD0 // Use either this line or the next to select which I2C address your device is using //#define MPU9250_ADDRESS MPU9250_ADDRESS_AD1 -MPU9250 myIMU(MPU9250_ADDRESS, I2Cport, I2Cclock); +MPU9250 myIMU0(MPU9250_ADDRESS_AD0, I2Cport, I2Cclock); +MPU9250 myIMU1(MPU9250_ADDRESS_AD1, I2Cport, I2Cclock); byte c = 0x00; byte d = 0x00; @@ -23,8 +25,8 @@ void setup() { void loop() { // put your main code here, to run repeatedly: - c = myIMU.readByte(MPU9250_ADDRESS_AD0, WHO_AM_I_MPU9250); - d = myIMU.readByte(MPU9250_ADDRESS_AD1, WHO_AM_I_MPU9250); + c = myIMU0.readByte(MPU9250_ADDRESS_AD0, WHO_AM_I_MPU9250); + d = myIMU1.readByte(MPU9250_ADDRESS_AD1, WHO_AM_I_MPU9250); Serial.print("Received AD0: 0x"); Serial.print(c, HEX); diff --git a/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino b/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino index a55d391..dfaa38e 100644 --- a/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino +++ b/examples/MPU9250BasicAHRS_I2C/MPU9250BasicAHRS_I2C.ino @@ -51,8 +51,8 @@ int myLed = 13; // Set up pin 13 led for toggling #define I2Cclock 400000 #define I2Cport Wire -#define MPU9250_ADDRESS MPU9250_ADDRESS_AD0 // Use either this line or the next to select which I2C address your device is using -//#define MPU9250_ADDRESS MPU9250_ADDRESS_AD1 +//#define MPU9250_ADDRESS MPU9250_ADDRESS_AD0 // Use either this line or the next to select which I2C address your device is using +#define MPU9250_ADDRESS MPU9250_ADDRESS_AD1 MPU9250 myIMU(MPU9250_ADDRESS, I2Cport, I2Cclock); @@ -62,6 +62,8 @@ void setup() // TWBR = 12; // 400 kbit/sec I2C speed Serial.begin(38400); + while(!Serial){}; + // Set up the interrupt pin, its set as active high, push-pull pinMode(intPin, INPUT); digitalWrite(intPin, LOW); @@ -217,7 +219,7 @@ void setup() // The next call delays for 4 seconds, and then records about 15 seconds of // data to calculate bias and scale. - myIMU.magCalMPU9250(myIMU.magBias, myIMU.magScale); +// myIMU.magCalMPU9250(myIMU.magBias, myIMU.magScale); Serial.println("AK8963 mag biases (mG)"); Serial.println(myIMU.magBias[0]); Serial.println(myIMU.magBias[1]); @@ -227,7 +229,7 @@ void setup() Serial.println(myIMU.magScale[0]); Serial.println(myIMU.magScale[1]); Serial.println(myIMU.magScale[2]); - delay(2000); // Add delay to see results before serial spew of data +// delay(2000); // Add delay to see results before serial spew of data if(SerialDebug) { diff --git a/examples/MPU9250BasicAHRS_SPI/MPU9250BasicAHRS_SPI.ino b/examples/MPU9250BasicAHRS_SPI/MPU9250BasicAHRS_SPI.ino index a3d3295..5ee8e53 100644 --- a/examples/MPU9250BasicAHRS_SPI/MPU9250BasicAHRS_SPI.ino +++ b/examples/MPU9250BasicAHRS_SPI/MPU9250BasicAHRS_SPI.ino @@ -3,13 +3,18 @@ date: April 1, 2014 license: Beerware - Use this code however you'd like. If you find it useful you can buy me a beer some time. - Modified by Brent Wilkins January 9, 2016 + Modified by Brent Wilkins July 19, 2016 + Modified by Owen Lyke Aug 6 2018 + - Removed code about magnetometer that would cause an abort() + - Magnetometer can be accessed by using the MPU9250 as an I2C + master. See the register map for more details Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source - Madgwick and Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini. + Madgwick and Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini + and the Teensy 3.1. SDA and SCL should have external pull-up resistors (to 3.3V). 10k resistors are on the EMSENSR-9250 breakout board. @@ -26,6 +31,8 @@ #include "quaternionFilters.h" #include "MPU9250.h" + + #ifdef LCD #include #include @@ -39,66 +46,34 @@ Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); #endif // LCD -#define AHRS true // Set to false for basic data read -#define SERIAL_DEBUG true // Set to true to get Serial output for debugging +#define AHRS false // Set to false for basic data read +#define SerialDebug false // Set to true to get Serial output for debugging -#define SPIport SPI -#define CSpin 2 -#define SPIrate 1000000 -MPU9250 myIMU = MPU9250(CSpin, SPIport, SPIrate); // Using digital pin to for chip select in demo +// Pin definitions +int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins +int myLed = 13; // Set up pin 13 led for toggling +#define SPIspeed 1000000 +#define SPIport SPI +#define CSpin 2 +#define MPU9250_ADDRESS 9001 // Use any old address value because it is not used for SPI, but it is all over this example -// Because of the way this library is written you still need to send SOME address - -// but in SPI mode it is not used, so I chose over 9000 - By the way if you look closely -// there will be a truncation warning during compilation because 9001 is too large for -// the parameter's type. -#define MPU9250_ADDRESS 9001 +MPU9250 myIMU(CSpin, SPIport, SPIspeed); void setup() { + Serial.begin(38400); - myIMU.begin(); - - myIMU.ak8963WhoAmI_SPI(); - -Serial.println("Testing MPU9250::readBytes..."); -byte fc = 0; -Serial.println( myIMU.readBytes(MPU9250_ADDRESS, WHO_AM_I_MPU9250, 1, &fc) ); -Serial.println(fc, HEX); -// TODO BHW -Serial.flush(); -abort(); - -/* -// Set slave address of AK8963 and set AK8963 for read -myIMU.writeByteSPI(I2C_SLV0_ADDR, AK8963_ADDRESS | READ_FLAG); -// Set address to start read from -myIMU.writeByteSPI(I2C_SLV0_REG, WHO_AM_I_AK8963); -*/ -byte fd = 0; -myIMU.readBytes(AK8963_ADDRESS, WHO_AM_I_AK8963, 1, &fd); -Serial.println(fd, HEX); + while(!Serial){}; -#ifdef LCD - display.begin(); // Initialize the display - display.setContrast(58); // Set the contrast - - // Start device display with ID of sensor - display.clearDisplay(); - display.setTextSize(2); - display.setCursor(0,0); display.print("MPU9250"); - display.setTextSize(1); - display.setCursor(0, 20); display.print("9-DOF 16-bit"); - display.setCursor(0, 30); display.print("motion sensor"); - display.setCursor(20,40); display.print("60 ug LSB"); - display.display(); - delay(1000); - - // Set up for data display - display.setTextSize(1); // Set text size to normal, 2 is twice normal etc. - display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen - display.clearDisplay(); // Clears the screen and buffer -#endif // LCD + // Set up the interrupt pin, its set as active high, push-pull + pinMode(intPin, INPUT); + digitalWrite(intPin, LOW); + pinMode(myLed, OUTPUT); + digitalWrite(myLed, HIGH); + + SPIport.begin(); + myIMU.kickHardware(); // Kick the hardware // Read the WHO_AM_I register, this is a good test of communication byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); @@ -106,19 +81,6 @@ Serial.println(fd, HEX); Serial.print(c, HEX); Serial.print(F(" I should be 0x")); Serial.println(0x71, HEX); -// TODO BHW -Serial.flush(); -abort(); - -#ifdef LCD - display.setCursor(20,0); display.print("MPU9250"); - display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(c, HEX); - display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0x71, HEX); - display.display(); - delay(1000); -#endif // LCD if (c == 0x71) // WHO_AM_I should always be 0x71 { @@ -142,25 +104,6 @@ abort(); // Calibrate gyro and accelerometers, load biases in bias registers myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias); -#ifdef LCD - display.clearDisplay(); - - display.setCursor(0, 0); display.print("MPU9250 bias"); - display.setCursor(0, 8); display.print(" x y z "); - - display.setCursor(0, 16); display.print((int)(1000*myIMU.accelBias[0])); - display.setCursor(24, 16); display.print((int)(1000*myIMU.accelBias[1])); - display.setCursor(48, 16); display.print((int)(1000*myIMU.accelBias[2])); - display.setCursor(72, 16); display.print("mg"); - - display.setCursor(0, 24); display.print(myIMU.gyroBias[0], 1); - display.setCursor(24, 24); display.print(myIMU.gyroBias[1], 1); - display.setCursor(48, 24); display.print(myIMU.gyroBias[2], 1); - display.setCursor(66, 24); display.print("o/s"); - - display.display(); - delay(1000); -#endif // LCD myIMU.initMPU9250(); // Initialize device for active mode read of acclerometer, gyroscope, and @@ -176,16 +119,6 @@ abort(); Serial.print(" I should be 0x"); Serial.println(0x48, HEX); -#ifdef LCD - display.clearDisplay(); - display.setCursor(20,0); display.print("AK8963"); - display.setCursor(0,10); display.print("I AM"); - display.setCursor(0,20); display.print(d, HEX); - display.setCursor(0,30); display.print("I Should Be"); - display.setCursor(0,40); display.print(0x48, HEX); - display.display(); - delay(1000); -#endif // LCD if (d != 0x48) { @@ -200,7 +133,8 @@ abort(); // Initialize device for active mode read of magnetometer Serial.println("AK8963 initialized for active data mode...."); -#ifdef SERIAL_DEBUG + if (SerialDebug) + { // Serial.println("Calibration values: "); Serial.print("X-Axis factory sensitivity adjustment value "); Serial.println(myIMU.factoryMagCalibration[0], 2); @@ -208,20 +142,8 @@ abort(); Serial.println(myIMU.factoryMagCalibration[1], 2); Serial.print("Z-Axis factory sensitivity adjustment value "); Serial.println(myIMU.factoryMagCalibration[2], 2); -#endif // SERIAL_DEBUG + } -#ifdef LCD - display.clearDisplay(); - display.setCursor(20,0); display.print("AK8963"); - display.setCursor(0,10); display.print("ASAX "); - display.setCursor(50,10); display.print(myIMU.factoryMagCalibration[0], 2); - display.setCursor(0,20); display.print("ASAY "); - display.setCursor(50,20); display.print(myIMU.factoryMagCalibration[1], 2); - display.setCursor(0,30); display.print("ASAZ "); - display.setCursor(50,30); display.print(myIMU.factoryMagCalibration[2], 2); - display.display(); - delay(1000); -#endif // LCD // Get sensor resolutions, only need to do this once myIMU.getAres(); @@ -230,7 +152,7 @@ abort(); // The next call delays for 4 seconds, and then records about 15 seconds of // data to calculate bias and scale. - myIMU.magCalMPU9250(myIMU.magBias, myIMU.magScale); +// myIMU.magCalMPU9250(myIMU.magBias, myIMU.magScale); Serial.println("AK8963 mag biases (mG)"); Serial.println(myIMU.magBias[0]); Serial.println(myIMU.magBias[1]); @@ -240,30 +162,19 @@ abort(); Serial.println(myIMU.magScale[0]); Serial.println(myIMU.magScale[1]); Serial.println(myIMU.magScale[2]); - delay(2000); // Add delay to see results before serial spew of data - -#ifdef SERIAL_DEBUG - Serial.println("Magnetometer:"); - Serial.print("X-Axis sensitivity adjustment value "); - Serial.println(myIMU.factoryMagCalibration[0], 2); - Serial.print("Y-Axis sensitivity adjustment value "); - Serial.println(myIMU.factoryMagCalibration[1], 2); - Serial.print("Z-Axis sensitivity adjustment value "); - Serial.println(myIMU.factoryMagCalibration[2], 2); -#endif // SERIAL_DEBUG +// delay(2000); // Add delay to see results before serial spew of data + + if(SerialDebug) + { + Serial.println("Magnetometer:"); + Serial.print("X-Axis sensitivity adjustment value "); + Serial.println(myIMU.factoryMagCalibration[0], 2); + Serial.print("Y-Axis sensitivity adjustment value "); + Serial.println(myIMU.factoryMagCalibration[1], 2); + Serial.print("Z-Axis sensitivity adjustment value "); + Serial.println(myIMU.factoryMagCalibration[2], 2); + } -#ifdef LCD - display.clearDisplay(); - display.setCursor(20,0); display.print("AK8963"); - display.setCursor(0,10); display.print("ASAX "); display.setCursor(50,10); - display.print(myIMU.factoryMagCalibration[0], 2); - display.setCursor(0,20); display.print("ASAY "); display.setCursor(50,20); - display.print(myIMU.factoryMagCalibration[1], 2); - display.setCursor(0,30); display.print("ASAZ "); display.setCursor(50,30); - display.print(myIMU.factoryMagCalibration[2], 2); - display.display(); - delay(1000); -#endif // LCD } // if (c == 0x71) else { @@ -279,7 +190,6 @@ abort(); void loop() { -// TODO: Fix these comments not interrupt pin used // If intPin goes high, all data registers have new data // On interrupt, check if data ready interrupt if (myIMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) @@ -334,7 +244,8 @@ void loop() myIMU.delt_t = millis() - myIMU.count; if (myIMU.delt_t > 500) { -#ifdef SERIAL_DEBUG + if(SerialDebug) + { // Print acceleration values in milligs! Serial.print("X-acceleration: "); Serial.print(1000 * myIMU.ax); Serial.print(" mg "); @@ -365,7 +276,7 @@ void loop() // Print temperature in degrees Centigrade Serial.print("Temperature is "); Serial.print(myIMU.temperature, 1); Serial.println(" degrees C"); -#endif // SERIAL_DEBUG + } #ifdef LCD display.clearDisplay(); @@ -394,6 +305,7 @@ void loop() #endif // LCD myIMU.count = millis(); + digitalWrite(myLed, !digitalRead(myLed)); // toggle led } // if (myIMU.delt_t > 500) } // if (!AHRS) else @@ -404,7 +316,235 @@ void loop() // update LCD once per half-second independent of read rate if (myIMU.delt_t > 500) { -#ifdef SERIAL_DEBUG + if(SerialDebug) + { + Serial.print("ax = "); Serial.print((int)1000 * myIMU.ax); + Serial.print(" ay = "); Serial.print((int)1000 * myIMU.ay); + Serial.print(" az = "); Serial.print((int)1000 * myIMU.az); + Serial.println(" mg"); +/* MPU9250 Basic Example Code + by: Kris Winer + date: April 1, 2014 + license: Beerware - Use this code however you'd like. If you + find it useful you can buy me a beer some time. + Modified by Brent Wilkins July 19, 2016 + + Demonstrate basic MPU-9250 functionality including parameterizing the register + addresses, initializing the sensor, getting properly scaled accelerometer, + gyroscope, and magnetometer data out. Added display functions to allow display + to on breadboard monitor. Addition of 9 DoF sensor fusion using open source + Madgwick and Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini + and the Teensy 3.1. + + SDA and SCL should have external pull-up resistors (to 3.3V). + 10k resistors are on the EMSENSR-9250 breakout board. + + Hardware setup: + MPU9250 Breakout --------- Arduino + VDD ---------------------- 3.3V + VDDI --------------------- 3.3V + SDA ----------------------- A4 + SCL ----------------------- A5 + GND ---------------------- GND + */ + +#include "quaternionFilters.h" +#include "MPU9250.h" + +#define AHRS false // Set to false for basic data read +#define SerialDebug true // Set to true to get Serial output for debugging + +// Pin definitions +int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins +int myLed = 13; // Set up pin 13 led for toggling + +#define SPIspeed 1000000 +#define SPIport SPI +#define CSpin 2 +#define MPU9250_ADDRESS 9001 // Use any old address value because it is not used for SPI, but it is all over this example + +MPU9250 myIMU(CSpin, SPIport, SPIspeed); + +void setup() +{ + + Serial.begin(38400); + while(!Serial){}; + + // Set up the interrupt pin, its set as active high, push-pull + pinMode(intPin, INPUT); + digitalWrite(intPin, LOW); + pinMode(myLed, OUTPUT); + digitalWrite(myLed, HIGH); + + + SPIport.begin(); + + myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Kick the hardware + + + // Read the WHO_AM_I register, this is a good test of communication + byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); + Serial.print(F("MPU9250 I AM 0x")); + Serial.print(c, HEX); + Serial.print(F(" I should be 0x")); + Serial.println(0x71, HEX); + + if (c == 0x71) // WHO_AM_I should always be 0x71 + { + Serial.println(F("MPU9250 is online...")); + + // Start by performing self test and reporting values + myIMU.MPU9250SelfTest(myIMU.selfTest); + Serial.print(F("x-axis self test: acceleration trim within : ")); + Serial.print(myIMU.selfTest[0],1); Serial.println("% of factory value"); + Serial.print(F("y-axis self test: acceleration trim within : ")); + Serial.print(myIMU.selfTest[1],1); Serial.println("% of factory value"); + Serial.print(F("z-axis self test: acceleration trim within : ")); + Serial.print(myIMU.selfTest[2],1); Serial.println("% of factory value"); + Serial.print(F("x-axis self test: gyration trim within : ")); + Serial.print(myIMU.selfTest[3],1); Serial.println("% of factory value"); + Serial.print(F("y-axis self test: gyration trim within : ")); + Serial.print(myIMU.selfTest[4],1); Serial.println("% of factory value"); + Serial.print(F("z-axis self test: gyration trim within : ")); + Serial.print(myIMU.selfTest[5],1); Serial.println("% of factory value"); + + // Calibrate gyro and accelerometers, load biases in bias registers + myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias); + + myIMU.initMPU9250(); + // Initialize device for active mode read of acclerometer, gyroscope, and + // temperature + Serial.println("MPU9250 initialized for active data mode...."); + + // Get sensor resolutions, only need to do this once + myIMU.getAres(); + myIMU.getGres(); +// myIMU.getMres(); + + } // if (c == 0x71) + else + { + Serial.print("Could not connect to MPU9250: 0x"); + Serial.println(c, HEX); + + // Communication failed, stop here + Serial.println(F("Communication failed, abort!")); + Serial.flush(); + abort(); + } + +} + +void loop() +{ +//SPIport.begin(); +// myIMU.writeByteSPI(0x81, 0xCE); + + delay(100); + + // If intPin goes high, all data registers have new data + // On interrupt, check if data ready interrupt + if (myIMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) + { + myIMU.readAccelData(myIMU.accelCount); // Read the x/y/z adc values + + // Now we'll calculate the accleration value into actual g's + // This depends on scale being set + myIMU.ax = (float)myIMU.accelCount[0] * myIMU.aRes; // - myIMU.accelBias[0]; + myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes; // - myIMU.accelBias[1]; + myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes; // - myIMU.accelBias[2]; + + myIMU.readGyroData(myIMU.gyroCount); // Read the x/y/z adc values + + // Calculate the gyro value into actual degrees per second + // This depends on scale being set + myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes; + myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes; + myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes; + +// myIMU.readMagData(myIMU.magCount); // Read the x/y/z adc values +// +// // Calculate the magnetometer values in milliGauss +// // Include factory calibration per data sheet and user environmental +// // corrections +// // Get actual magnetometer value, this depends on scale being set +// myIMU.mx = (float)myIMU.magCount[0] * myIMU.mRes +// * myIMU.factoryMagCalibration[0] - myIMU.magBias[0]; +// myIMU.my = (float)myIMU.magCount[1] * myIMU.mRes +// * myIMU.factoryMagCalibration[1] - myIMU.magBias[1]; +// myIMU.mz = (float)myIMU.magCount[2] * myIMU.mRes +// * myIMU.factoryMagCalibration[2] - myIMU.magBias[2]; + } // if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) + + // Must be called before updating quaternions! + myIMU.updateTime(); + +// // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of +// // the magnetometer; the magnetometer z-axis (+ down) is opposite to z-axis +// // (+ up) of accelerometer and gyro! We have to make some allowance for this +// // orientationmismatch in feeding the output to the quaternion filter. For the +// // MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward +// // along the x-axis just like in the LSM9DS0 sensor. This rotation can be +// // modified to allow any convenient orientation convention. This is ok by +// // aircraft orientation standards! Pass gyro rate as rad/s +// MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx * DEG_TO_RAD, +// myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD, myIMU.my, +// myIMU.mx, myIMU.mz, myIMU.deltat); + + if (!AHRS) + { + myIMU.delt_t = millis() - myIMU.count; + if (myIMU.delt_t > 500) + { + if(SerialDebug) + { + // Print acceleration values in milligs! + Serial.print("X-acceleration: "); Serial.print(1000 * myIMU.ax); + Serial.print(" mg "); + Serial.print("Y-acceleration: "); Serial.print(1000 * myIMU.ay); + Serial.print(" mg "); + Serial.print("Z-acceleration: "); Serial.print(1000 * myIMU.az); + Serial.println(" mg "); + + // Print gyro values in degree/sec + Serial.print("X-gyro rate: "); Serial.print(myIMU.gx, 3); + Serial.print(" degrees/sec "); + Serial.print("Y-gyro rate: "); Serial.print(myIMU.gy, 3); + Serial.print(" degrees/sec "); + Serial.print("Z-gyro rate: "); Serial.print(myIMU.gz, 3); + Serial.println(" degrees/sec"); + + // Print mag values in degree/sec + Serial.print("X-mag field: "); Serial.print(myIMU.mx); + Serial.print(" mG "); + Serial.print("Y-mag field: "); Serial.print(myIMU.my); + Serial.print(" mG "); + Serial.print("Z-mag field: "); Serial.print(myIMU.mz); + Serial.println(" mG"); + + myIMU.tempCount = myIMU.readTempData(); // Read the adc values + // Temperature in degrees Centigrade + myIMU.temperature = ((float) myIMU.tempCount) / 333.87 + 21.0; + // Print temperature in degrees Centigrade + Serial.print("Temperature is "); Serial.print(myIMU.temperature, 1); + Serial.println(" degrees C"); + } + + myIMU.count = millis(); + digitalWrite(myLed, !digitalRead(myLed)); // toggle led + } // if (myIMU.delt_t > 500) + } // if (!AHRS) + else + { + // Serial print and/or display at 0.5 s rate independent of data rates + myIMU.delt_t = millis() - myIMU.count; + + // update LCD once per half-second independent of read rate + if (myIMU.delt_t > 500) + { + if(SerialDebug) + { Serial.print("ax = "); Serial.print((int)1000 * myIMU.ax); Serial.print(" ay = "); Serial.print((int)1000 * myIMU.ay); Serial.print(" az = "); Serial.print((int)1000 * myIMU.az); @@ -424,7 +564,7 @@ void loop() Serial.print(" qx = "); Serial.print(*(getQ() + 1)); Serial.print(" qy = "); Serial.print(*(getQ() + 2)); Serial.print(" qz = "); Serial.println(*(getQ() + 3)); -#endif // SERIAL_DEBUG + } // Define output variables from updated quaternion---these are Tait-Bryan // angles, commonly used in aircraft orientation. In this coordinate system, @@ -456,12 +596,13 @@ void loop() myIMU.yaw *= RAD_TO_DEG; // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is - // 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19 + // 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19 // - http://www.ngdc.noaa.gov/geomag-web/#declination myIMU.yaw -= 8.5; myIMU.roll *= RAD_TO_DEG; -#ifdef SERIAL_DEBUG + if(SerialDebug) + { Serial.print("Yaw, Pitch, Roll: "); Serial.print(myIMU.yaw, 2); Serial.print(", "); @@ -472,53 +613,7 @@ void loop() Serial.print("rate = "); Serial.print((float)myIMU.sumCount / myIMU.sum, 2); Serial.println(" Hz"); -#endif // SERIAL_DEBUG - -#ifdef LCD - display.clearDisplay(); - - display.setCursor(0, 0); display.print(" x y z "); - - display.setCursor(0, 8); display.print((int)(1000 * myIMU.ax)); - display.setCursor(24, 8); display.print((int)(1000 * myIMU.ay)); - display.setCursor(48, 8); display.print((int)(1000 * myIMU.az)); - display.setCursor(72, 8); display.print("mg"); - - display.setCursor(0, 16); display.print((int)(myIMU.gx)); - display.setCursor(24, 16); display.print((int)(myIMU.gy)); - display.setCursor(48, 16); display.print((int)(myIMU.gz)); - display.setCursor(66, 16); display.print("o/s"); - - display.setCursor(0, 24); display.print((int)(myIMU.mx)); - display.setCursor(24, 24); display.print((int)(myIMU.my)); - display.setCursor(48, 24); display.print((int)(myIMU.mz)); - display.setCursor(72, 24); display.print("mG"); - - display.setCursor(0, 32); display.print((int)(myIMU.yaw)); - display.setCursor(24, 32); display.print((int)(myIMU.pitch)); - display.setCursor(48, 32); display.print((int)(myIMU.roll)); - display.setCursor(66, 32); display.print("ypr"); - - // With these settings the filter is updating at a ~145 Hz rate using the - // Madgwick scheme and >200 Hz using the Mahony scheme even though the - // display refreshes at only 2 Hz. The filter update rate is determined - // mostly by the mathematical steps in the respective algorithms, the - // processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR: - // an ODR of 10 Hz for the magnetometer produce the above rates, maximum - // magnetometer ODR of 100 Hz produces filter update rates of 36 - 145 and - // ~38 Hz for the Madgwick and Mahony schemes, respectively. This is - // presumably because the magnetometer read takes longer than the gyro or - // accelerometer reads. This filter update rate should be fast enough to - // maintain accurate platform orientation for stabilization control of a - // fast-moving robot or quadcopter. Compare to the update rate of 200 Hz - // produced by the on-board Digital Motion Processor of Invensense's MPU6050 - // 6 DoF and MPU9150 9DoF sensors. The 3.3 V 8 MHz Pro Mini is doing pretty - // well! - display.setCursor(0, 40); display.print("rt: "); - display.print((float) myIMU.sumCount / myIMU.sum, 2); - display.print(" Hz"); - display.display(); -#endif // LCD + } myIMU.count = millis(); myIMU.sumCount = 0; diff --git a/src/MPU9250.cpp b/src/MPU9250.cpp index 6dd9d57..46b3c34 100644 --- a/src/MPU9250.cpp +++ b/src/MPU9250.cpp @@ -22,6 +22,7 @@ MPU9250::MPU9250( int8_t csPin, SPIClass &spiInterface, uint32_t spi_freq ) _spi->begin(); pinMode(_csPin, OUTPUT); deselect(); + } MPU9250::MPU9250( uint8_t address, TwoWire &wirePort, uint32_t clock_frequency ) { @@ -37,6 +38,15 @@ MPU9250::MPU9250( uint8_t address, TwoWire &wirePort, uint32_t clock_frequency ) _wire->setClock(_interfaceSpeed); } +void MPU9250::setupMagForSPI() +{ + // Use slave 4 for talking to the magnetometer + writeByteSPI(49, ((1 << 7) | AK8963_ADDRESS)); // Set the SLV_4_ADDR register to the magnetometer's address + writeByteSPI(52, 0b00000000); // Setup SLV_4 control as needed (but not set to do an operation yet) + + writeByteSPI(36, 0b10000000); // Enable the multi-master mode +} + void MPU9250::getMres() { switch (Mscale) @@ -198,6 +208,11 @@ void MPU9250::initAK8963(float * destination) // Set magnetometer data resolution and sample ODR writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); delay(10); + + if(_csPin == NOT_SPI) + { + setupMagForSPI(); + } } void MPU9250::initMPU9250() @@ -275,6 +290,11 @@ void MPU9250::initMPU9250() // Enable data ready (bit 0) interrupt writeByte(_I2Caddr, INT_ENABLE, 0x01); delay(100); + + if(_csPin == NOT_SPI) + { + setupMagForSPI(); + } } @@ -715,10 +735,10 @@ uint8_t MPU9250::writeByteSPI(uint8_t registerAddress, uint8_t writeData) deselect(); _spi->endTransaction(); -#ifdef SERIAL_DEBUG - Serial.print("MPU9250::writeByteSPI slave returned: 0x"); - Serial.println(returnVal, HEX); -#endif +// #ifdef SERIAL_DEBUG +// Serial.print("MPU9250::writeByteSPI slave returned: 0x"); +// Serial.println(returnVal, HEX); +// #endif return returnVal; } @@ -741,7 +761,14 @@ uint8_t MPU9250::readByte(uint8_t deviceAddress, uint8_t registerAddress) { if (_csPin != NOT_SPI) { - return readByteSPI(registerAddress); + if(deviceAddress == AK8963_ADDRESS) + { + return readMagByteSPI(registerAddress); + } + else + { + return readByteSPI(registerAddress); + } } else { @@ -749,6 +776,55 @@ uint8_t MPU9250::readByte(uint8_t deviceAddress, uint8_t registerAddress) } } +uint8_t MPU9250::readMagByteSPI(uint8_t registerAddress) +{ + setupMagForSPI(); + + writeByteSPI(49, ((1 << 7) | AK8963_ADDRESS)); + writeByteSPI(50, registerAddress); + writeByteSPI(52, 0b11000000); // Command the read into I2C_SLV4_DI register, cause an interrupt when complete + + // Wait for the data to be ready + uint8_t I2C_MASTER_STATUS = readByteSPI(54); + + uint32_t count = 0; + while(((I2C_MASTER_STATUS & 0b01000000) == 0) && (count++ < 100000)) // Checks against the I2C_SLV4_DONE bit in the I2C master status register + { + I2C_MASTER_STATUS = readByteSPI(54); + } + if(count > 10000) + { + Serial.println(F("Timed out")); + } + + + + + return readByteSPI(53); // Read the data that is in the SLV4_DI register +} + +uint8_t MPU9250::writeMagByteSPI(uint8_t registerAddress, uint8_t data) +{ + setupMagForSPI(); + + writeByteSPI(49, ((1 << 7) | AK8963_ADDRESS)); + writeByteSPI(50, registerAddress); + writeByteSPI(51, data); + writeByteSPI(52, 0b11000000); // Command the read into I2C_SLV4_DI register, cause an interrupt when complete + + uint8_t I2C_MASTER_STATUS = readByteSPI(54); + uint32_t count = 0; + while(((I2C_MASTER_STATUS & 0b01000000) == 0) && (count++ < 10000)) // Checks against the I2C_SLV4_DONE bit in the I2C master status register + { + I2C_MASTER_STATUS = readByteSPI(54); + } + if(count > 10000) + { + Serial.println(F("Timed out")); + } + return 0x00; +} + // Read a byte from the given register address from device using I2C uint8_t MPU9250::readByteWire(uint8_t deviceAddress, uint8_t registerAddress) { @@ -822,10 +898,10 @@ uint8_t MPU9250::readBytesSPI(uint8_t registerAddress, uint8_t count, for (i = 0; i < count; i++) { dest[i] = _spi->transfer(0x00); -#ifdef SERIAL_DEBUG - Serial.print("readBytesSPI::Read byte: 0x"); - Serial.println(dest[i], HEX); -#endif +// #ifdef SERIAL_DEBUG +// Serial.print("readBytesSPI::Read byte: 0x"); +// Serial.println(dest[i], HEX); +// #endif } _spi->endTransaction(); diff --git a/src/MPU9250.h b/src/MPU9250.h index 815ba8a..34cbafb 100644 --- a/src/MPU9250.h +++ b/src/MPU9250.h @@ -185,6 +185,9 @@ class MPU9250 { protected: + + public: // temporary + // Set initial input parameters enum Ascale { @@ -235,12 +238,15 @@ class MPU9250 uint8_t writeByteWire(uint8_t, uint8_t, uint8_t); uint8_t writeByteSPI(uint8_t, uint8_t); + uint8_t writeMagByteSPI(uint8_t subAddress, uint8_t data); uint8_t readByteSPI(uint8_t subAddress); + uint8_t readMagByteSPI(uint8_t subAddress); uint8_t readByteWire(uint8_t address, uint8_t subAddress); bool magInit(); void kickHardware(); void select(); void deselect(); + void setupMagForSPI(); // TODO: Remove this next line public: uint8_t ak8963WhoAmI_SPI(); From 77d2d2fb12e97af8f4e311013392113a900673a4 Mon Sep 17 00:00:00 2001 From: Jim Lindblom Date: Fri, 9 Nov 2018 08:40:37 -0700 Subject: [PATCH 25/28] Add note on MPU-9250 DMP library. --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 1608145..24146bc 100644 --- a/README.md +++ b/README.md @@ -5,9 +5,9 @@ MPU-9250 9 DOF IMU Arduino Library [*MPU-9250 Breakout (SEN-13762)*](https://www.sparkfun.com/products/13762) -This is an arduino IDE library to control the MPU-9250. +This is an arduino IDE library to control and read from the MPU-9250. It provides access to accelerometer, gyroscope, and magnetometer sensor readings. It should be compatible with any Arduino-compatible development board. It has been thoroughly tested with Arduino Pro Mini. -This has been tested with Arduino Pro Mini. +If you're looking for a more advanced library, which takes advantage of the MPU-9250's digital motion processing (DMP) features (tap-detection, pedometer, orientation, quaternion-calculation), and if you have a more advanced Arduino development board (e.g. [Arduino Zero](https://store.arduino.cc/usa/arduino-zero), [SparkFun SAMD21 Breakout](https://www.sparkfun.com/products/13664)), check out the [SparkFun MPU-9250 DMP Library](https://github.com/sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library). Repository Contents ------------------- From 6b7434971e47a5c027cd7f57b1a9f7b40766bfcd Mon Sep 17 00:00:00 2001 From: Lane Kolbly Date: Sat, 10 Nov 2018 13:32:36 -0600 Subject: [PATCH 26/28] Don't enable SPI bus when SPI mode is disabled. --- src/MPU9250.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/MPU9250.cpp b/src/MPU9250.cpp index 46b3c34..409192e 100644 --- a/src/MPU9250.cpp +++ b/src/MPU9250.cpp @@ -209,7 +209,7 @@ void MPU9250::initAK8963(float * destination) writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); delay(10); - if(_csPin == NOT_SPI) + if(_csPin != NOT_SPI) { setupMagForSPI(); } @@ -291,7 +291,7 @@ void MPU9250::initMPU9250() writeByte(_I2Caddr, INT_ENABLE, 0x01); delay(100); - if(_csPin == NOT_SPI) + if(_csPin != NOT_SPI) { setupMagForSPI(); } From 850217e91ef11caa092d095c9abfcebb8396e7f0 Mon Sep 17 00:00:00 2001 From: Nathan Seidle Date: Tue, 18 Dec 2018 16:17:08 -0700 Subject: [PATCH 27/28] Lib ver roll --- library.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.properties b/library.properties index a97c9c5..ae244f9 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=SparkFun MPU-9250 9 DOF IMU Breakout -version=1.0.0 +version=1.0.1 author=SparkFun Electronics maintainer=SparkFun Electronics sentence=Driver for InvenSense's MPU-9250 9-DOF IMU (3-axis gyroscope, 3-axis accelerometer & 3-axis magnetometer) From ea379ce8c930ef90f4ebc37799544d68108e3126 Mon Sep 17 00:00:00 2001 From: Nathan Seidle Date: Tue, 18 Dec 2018 16:33:02 -0700 Subject: [PATCH 28/28] Lib ver roll --- library.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.properties b/library.properties index ae244f9..0540639 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=SparkFun MPU-9250 9 DOF IMU Breakout -version=1.0.1 +version=1.0.2 author=SparkFun Electronics maintainer=SparkFun Electronics sentence=Driver for InvenSense's MPU-9250 9-DOF IMU (3-axis gyroscope, 3-axis accelerometer & 3-axis magnetometer)