Jordan Beason / MPU9250

fin

Dependents:   A5_jordanbeason

MPU9250.h@0:a1a586e251c8, 2019-11-19 (annotated)

Committer:
jbeason3
Date:
Tue Nov 19 04:30:54 2019 +0000
Revision:
0:a1a586e251c8
fin;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jbeason3 0:a1a586e251c8 1 #include "mbed.h"
jbeason3 0:a1a586e251c8 2 #include "math.h"
jbeason3 0:a1a586e251c8 3
jbeason3 0:a1a586e251c8 4 // 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
jbeason3 0:a1a586e251c8 5 // above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map
jbeason3 0:a1a586e251c8 6 //
jbeason3 0:a1a586e251c8 7 //Magnetometer Registers
jbeason3 0:a1a586e251c8 8 #define AK8963_ADDRESS 0x0C<<1
jbeason3 0:a1a586e251c8 9 #define WHO_AM_I_AK8963 0x00 // should return 0x48
jbeason3 0:a1a586e251c8 10 #define INFO 0x01
jbeason3 0:a1a586e251c8 11 #define AK8963_ST1 0x02 // data ready status bit 0
jbeason3 0:a1a586e251c8 12 #define AK8963_XOUT_L 0x03 // data
jbeason3 0:a1a586e251c8 13 #define AK8963_XOUT_H 0x04
jbeason3 0:a1a586e251c8 14 #define AK8963_YOUT_L 0x05
jbeason3 0:a1a586e251c8 15 #define AK8963_YOUT_H 0x06
jbeason3 0:a1a586e251c8 16 #define AK8963_ZOUT_L 0x07
jbeason3 0:a1a586e251c8 17 #define AK8963_ZOUT_H 0x08
jbeason3 0:a1a586e251c8 18 #define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2
jbeason3 0:a1a586e251c8 19 #define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
jbeason3 0:a1a586e251c8 20 #define AK8963_ASTC 0x0C // Self test control
jbeason3 0:a1a586e251c8 21 #define AK8963_I2CDIS 0x0F // I2C disable
jbeason3 0:a1a586e251c8 22 #define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
jbeason3 0:a1a586e251c8 23 #define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
jbeason3 0:a1a586e251c8 24 #define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
jbeason3 0:a1a586e251c8 25
jbeason3 0:a1a586e251c8 26 #define SELF_TEST_X_GYRO 0x00
jbeason3 0:a1a586e251c8 27 #define SELF_TEST_Y_GYRO 0x01
jbeason3 0:a1a586e251c8 28 #define SELF_TEST_Z_GYRO 0x02
jbeason3 0:a1a586e251c8 29
jbeason3 0:a1a586e251c8 30 /*#define X_FINE_GAIN 0x03 // [7:0] fine gain
jbeason3 0:a1a586e251c8 31 #define Y_FINE_GAIN 0x04
jbeason3 0:a1a586e251c8 32 #define Z_FINE_GAIN 0x05
jbeason3 0:a1a586e251c8 33 #define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer
jbeason3 0:a1a586e251c8 34 #define XA_OFFSET_L_TC 0x07
jbeason3 0:a1a586e251c8 35 #define YA_OFFSET_H 0x08
jbeason3 0:a1a586e251c8 36 #define YA_OFFSET_L_TC 0x09
jbeason3 0:a1a586e251c8 37 #define ZA_OFFSET_H 0x0A
jbeason3 0:a1a586e251c8 38 #define ZA_OFFSET_L_TC 0x0B */
jbeason3 0:a1a586e251c8 39
jbeason3 0:a1a586e251c8 40 #define SELF_TEST_X_ACCEL 0x0D
jbeason3 0:a1a586e251c8 41 #define SELF_TEST_Y_ACCEL 0x0E
jbeason3 0:a1a586e251c8 42 #define SELF_TEST_Z_ACCEL 0x0F
jbeason3 0:a1a586e251c8 43
jbeason3 0:a1a586e251c8 44 #define SELF_TEST_A 0x10
jbeason3 0:a1a586e251c8 45
jbeason3 0:a1a586e251c8 46 #define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope
jbeason3 0:a1a586e251c8 47 #define XG_OFFSET_L 0x14
jbeason3 0:a1a586e251c8 48 #define YG_OFFSET_H 0x15
jbeason3 0:a1a586e251c8 49 #define YG_OFFSET_L 0x16
jbeason3 0:a1a586e251c8 50 #define ZG_OFFSET_H 0x17
jbeason3 0:a1a586e251c8 51 #define ZG_OFFSET_L 0x18
jbeason3 0:a1a586e251c8 52 #define SMPLRT_DIV 0x19
jbeason3 0:a1a586e251c8 53 #define CONFIG 0x1A
jbeason3 0:a1a586e251c8 54 #define GYRO_CONFIG 0x1B
jbeason3 0:a1a586e251c8 55 #define ACCEL_CONFIG 0x1C
jbeason3 0:a1a586e251c8 56 #define ACCEL_CONFIG2 0x1D
jbeason3 0:a1a586e251c8 57 #define LP_ACCEL_ODR 0x1E
jbeason3 0:a1a586e251c8 58 #define WOM_THR 0x1F
jbeason3 0:a1a586e251c8 59
jbeason3 0:a1a586e251c8 60 #define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
jbeason3 0:a1a586e251c8 61 #define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0]
jbeason3 0:a1a586e251c8 62 #define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
jbeason3 0:a1a586e251c8 63
jbeason3 0:a1a586e251c8 64 #define FIFO_EN 0x23
jbeason3 0:a1a586e251c8 65 #define I2C_MST_CTRL 0x24
jbeason3 0:a1a586e251c8 66 #define I2C_SLV0_ADDR 0x25
jbeason3 0:a1a586e251c8 67 #define I2C_SLV0_REG 0x26
jbeason3 0:a1a586e251c8 68 #define I2C_SLV0_CTRL 0x27
jbeason3 0:a1a586e251c8 69 #define I2C_SLV1_ADDR 0x28
jbeason3 0:a1a586e251c8 70 #define I2C_SLV1_REG 0x29
jbeason3 0:a1a586e251c8 71 #define I2C_SLV1_CTRL 0x2A
jbeason3 0:a1a586e251c8 72 #define I2C_SLV2_ADDR 0x2B
jbeason3 0:a1a586e251c8 73 #define I2C_SLV2_REG 0x2C
jbeason3 0:a1a586e251c8 74 #define I2C_SLV2_CTRL 0x2D
jbeason3 0:a1a586e251c8 75 #define I2C_SLV3_ADDR 0x2E
jbeason3 0:a1a586e251c8 76 #define I2C_SLV3_REG 0x2F
jbeason3 0:a1a586e251c8 77 #define I2C_SLV3_CTRL 0x30
jbeason3 0:a1a586e251c8 78 #define I2C_SLV4_ADDR 0x31
jbeason3 0:a1a586e251c8 79 #define I2C_SLV4_REG 0x32
jbeason3 0:a1a586e251c8 80 #define I2C_SLV4_DO 0x33
jbeason3 0:a1a586e251c8 81 #define I2C_SLV4_CTRL 0x34
jbeason3 0:a1a586e251c8 82 #define I2C_SLV4_DI 0x35
jbeason3 0:a1a586e251c8 83 #define I2C_MST_STATUS 0x36
jbeason3 0:a1a586e251c8 84 #define INT_PIN_CFG 0x37
jbeason3 0:a1a586e251c8 85 #define INT_ENABLE 0x38
jbeason3 0:a1a586e251c8 86 #define DMP_INT_STATUS 0x39 // Check DMP interrupt
jbeason3 0:a1a586e251c8 87 #define INT_STATUS 0x3A
jbeason3 0:a1a586e251c8 88 #define ACCEL_XOUT_H 0x3B
jbeason3 0:a1a586e251c8 89 #define ACCEL_XOUT_L 0x3C
jbeason3 0:a1a586e251c8 90 #define ACCEL_YOUT_H 0x3D
jbeason3 0:a1a586e251c8 91 #define ACCEL_YOUT_L 0x3E
jbeason3 0:a1a586e251c8 92 #define ACCEL_ZOUT_H 0x3F
jbeason3 0:a1a586e251c8 93 #define ACCEL_ZOUT_L 0x40
jbeason3 0:a1a586e251c8 94 #define TEMP_OUT_H 0x41
jbeason3 0:a1a586e251c8 95 #define TEMP_OUT_L 0x42
jbeason3 0:a1a586e251c8 96 #define GYRO_XOUT_H 0x43
jbeason3 0:a1a586e251c8 97 #define GYRO_XOUT_L 0x44
jbeason3 0:a1a586e251c8 98 #define GYRO_YOUT_H 0x45
jbeason3 0:a1a586e251c8 99 #define GYRO_YOUT_L 0x46
jbeason3 0:a1a586e251c8 100 #define GYRO_ZOUT_H 0x47
jbeason3 0:a1a586e251c8 101 #define GYRO_ZOUT_L 0x48
jbeason3 0:a1a586e251c8 102 #define EXT_SENS_DATA_00 0x49
jbeason3 0:a1a586e251c8 103 #define EXT_SENS_DATA_01 0x4A
jbeason3 0:a1a586e251c8 104 #define EXT_SENS_DATA_02 0x4B
jbeason3 0:a1a586e251c8 105 #define EXT_SENS_DATA_03 0x4C
jbeason3 0:a1a586e251c8 106 #define EXT_SENS_DATA_04 0x4D
jbeason3 0:a1a586e251c8 107 #define EXT_SENS_DATA_05 0x4E
jbeason3 0:a1a586e251c8 108 #define EXT_SENS_DATA_06 0x4F
jbeason3 0:a1a586e251c8 109 #define EXT_SENS_DATA_07 0x50
jbeason3 0:a1a586e251c8 110 #define EXT_SENS_DATA_08 0x51
jbeason3 0:a1a586e251c8 111 #define EXT_SENS_DATA_09 0x52
jbeason3 0:a1a586e251c8 112 #define EXT_SENS_DATA_10 0x53
jbeason3 0:a1a586e251c8 113 #define EXT_SENS_DATA_11 0x54
jbeason3 0:a1a586e251c8 114 #define EXT_SENS_DATA_12 0x55
jbeason3 0:a1a586e251c8 115 #define EXT_SENS_DATA_13 0x56
jbeason3 0:a1a586e251c8 116 #define EXT_SENS_DATA_14 0x57
jbeason3 0:a1a586e251c8 117 #define EXT_SENS_DATA_15 0x58
jbeason3 0:a1a586e251c8 118 #define EXT_SENS_DATA_16 0x59
jbeason3 0:a1a586e251c8 119 #define EXT_SENS_DATA_17 0x5A
jbeason3 0:a1a586e251c8 120 #define EXT_SENS_DATA_18 0x5B
jbeason3 0:a1a586e251c8 121 #define EXT_SENS_DATA_19 0x5C
jbeason3 0:a1a586e251c8 122 #define EXT_SENS_DATA_20 0x5D
jbeason3 0:a1a586e251c8 123 #define EXT_SENS_DATA_21 0x5E
jbeason3 0:a1a586e251c8 124 #define EXT_SENS_DATA_22 0x5F
jbeason3 0:a1a586e251c8 125 #define EXT_SENS_DATA_23 0x60
jbeason3 0:a1a586e251c8 126 #define MOT_DETECT_STATUS 0x61
jbeason3 0:a1a586e251c8 127 #define I2C_SLV0_DO 0x63
jbeason3 0:a1a586e251c8 128 #define I2C_SLV1_DO 0x64
jbeason3 0:a1a586e251c8 129 #define I2C_SLV2_DO 0x65
jbeason3 0:a1a586e251c8 130 #define I2C_SLV3_DO 0x66
jbeason3 0:a1a586e251c8 131 #define I2C_MST_DELAY_CTRL 0x67
jbeason3 0:a1a586e251c8 132 #define SIGNAL_PATH_RESET 0x68
jbeason3 0:a1a586e251c8 133 #define MOT_DETECT_CTRL 0x69
jbeason3 0:a1a586e251c8 134 #define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP
jbeason3 0:a1a586e251c8 135 #define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode
jbeason3 0:a1a586e251c8 136 #define PWR_MGMT_2 0x6C
jbeason3 0:a1a586e251c8 137 #define DMP_BANK 0x6D // Activates a specific bank in the DMP
jbeason3 0:a1a586e251c8 138 #define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank
jbeason3 0:a1a586e251c8 139 #define DMP_REG 0x6F // Register in DMP from which to read or to which to write
jbeason3 0:a1a586e251c8 140 #define DMP_REG_1 0x70
jbeason3 0:a1a586e251c8 141 #define DMP_REG_2 0x71
jbeason3 0:a1a586e251c8 142 #define FIFO_COUNTH 0x72
jbeason3 0:a1a586e251c8 143 #define FIFO_COUNTL 0x73
jbeason3 0:a1a586e251c8 144 #define FIFO_R_W 0x74
jbeason3 0:a1a586e251c8 145 #define WHO_AM_I_MPU9250 0x75 // Should return 0x71
jbeason3 0:a1a586e251c8 146 #define XA_OFFSET_H 0x77
jbeason3 0:a1a586e251c8 147 #define XA_OFFSET_L 0x78
jbeason3 0:a1a586e251c8 148 #define YA_OFFSET_H 0x7A
jbeason3 0:a1a586e251c8 149 #define YA_OFFSET_L 0x7B
jbeason3 0:a1a586e251c8 150 #define ZA_OFFSET_H 0x7D
jbeason3 0:a1a586e251c8 151 #define ZA_OFFSET_L 0x7E
jbeason3 0:a1a586e251c8 152
jbeason3 0:a1a586e251c8 153 #define PI 3.14159265358979323846f
jbeason3 0:a1a586e251c8 154 // Using the MSENSR-9250 breakout board, ADO is set to 0
jbeason3 0:a1a586e251c8 155 // Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
jbeason3 0:a1a586e251c8 156 //mbed uses the eight-bit device address, so shift seven-bit addresses left by one!
jbeason3 0:a1a586e251c8 157 #define ADO 0
jbeason3 0:a1a586e251c8 158 #if ADO
jbeason3 0:a1a586e251c8 159 #define MPU9250_ADDRESS 0x69<<1 // Device address when ADO = 1
jbeason3 0:a1a586e251c8 160 #else
jbeason3 0:a1a586e251c8 161 #define MPU9250_ADDRESS 0x68<<1 // Device address when ADO = 0
jbeason3 0:a1a586e251c8 162 #endif
jbeason3 0:a1a586e251c8 163
jbeason3 0:a1a586e251c8 164 // Set initial input parameters
jbeason3 0:a1a586e251c8 165
jbeason3 0:a1a586e251c8 166 enum Ascale {
jbeason3 0:a1a586e251c8 167 AFS_2G = 0,
jbeason3 0:a1a586e251c8 168 AFS_4G,
jbeason3 0:a1a586e251c8 169 AFS_8G,
jbeason3 0:a1a586e251c8 170 AFS_16G
jbeason3 0:a1a586e251c8 171 };
jbeason3 0:a1a586e251c8 172
jbeason3 0:a1a586e251c8 173 enum Gscale {
jbeason3 0:a1a586e251c8 174 GFS_250DPS = 0,
jbeason3 0:a1a586e251c8 175 GFS_500DPS,
jbeason3 0:a1a586e251c8 176 GFS_1000DPS,
jbeason3 0:a1a586e251c8 177 GFS_2000DPS
jbeason3 0:a1a586e251c8 178 };
jbeason3 0:a1a586e251c8 179
jbeason3 0:a1a586e251c8 180 enum Mscale {
jbeason3 0:a1a586e251c8 181 MFS_14BITS = 0, // 0.6 mG per LSB
jbeason3 0:a1a586e251c8 182 MFS_16BITS // 0.15 mG per LSB
jbeason3 0:a1a586e251c8 183 };
jbeason3 0:a1a586e251c8 184
jbeason3 0:a1a586e251c8 185
jbeason3 0:a1a586e251c8 186 class MPU9250 {
jbeason3 0:a1a586e251c8 187
jbeason3 0:a1a586e251c8 188 protected:
jbeason3 0:a1a586e251c8 189
jbeason3 0:a1a586e251c8 190 public:
jbeason3 0:a1a586e251c8 191
jbeason3 0:a1a586e251c8 192 MPU9250(PinName sda, PinName scl);
jbeason3 0:a1a586e251c8 193 MPU9250(I2C *i2c);
jbeason3 0:a1a586e251c8 194 ~MPU9250();
jbeason3 0:a1a586e251c8 195
jbeason3 0:a1a586e251c8 196 void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
jbeason3 0:a1a586e251c8 197 char readByte(uint8_t address, uint8_t subAddress);
jbeason3 0:a1a586e251c8 198 void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
jbeason3 0:a1a586e251c8 199 void getMres();
jbeason3 0:a1a586e251c8 200 void getGres();
jbeason3 0:a1a586e251c8 201 void getAres();
jbeason3 0:a1a586e251c8 202 void readAccelData(int16_t * destination);
jbeason3 0:a1a586e251c8 203 void readGyroData(int16_t * destination);
jbeason3 0:a1a586e251c8 204 void readMagData(int16_t * destination);
jbeason3 0:a1a586e251c8 205 int16_t readTempData();
jbeason3 0:a1a586e251c8 206 void resetMPU9250();
jbeason3 0:a1a586e251c8 207 void initAK8963(float * destination);
jbeason3 0:a1a586e251c8 208 void initMPU9250();
jbeason3 0:a1a586e251c8 209 void calibrateMPU9250(float * dest1, float * dest2);
jbeason3 0:a1a586e251c8 210 void MPU9250SelfTest(float * destination);
jbeason3 0:a1a586e251c8 211 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
jbeason3 0:a1a586e251c8 212 void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
jbeason3 0:a1a586e251c8 213
jbeason3 0:a1a586e251c8 214 float SelfTest[6];
jbeason3 0:a1a586e251c8 215 float gyroBias[3],accelBias[3]; // Bias corrections for gyro and accelerometer
jbeason3 0:a1a586e251c8 216 float magCalibration[3], magbias[3]; // Factory mag calibration and mag bias
jbeason3 0:a1a586e251c8 217 //Ascale Ascale=AFS_2G;
jbeason3 0:a1a586e251c8 218 uint8_t Ascale; // AFS_2G, AFS_4G, AFS_8G, AFS_16G
jbeason3 0:a1a586e251c8 219 uint8_t Gscale; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
jbeason3 0:a1a586e251c8 220 uint8_t Mscale; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution
jbeason3 0:a1a586e251c8 221 uint8_t Mmode; // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR
jbeason3 0:a1a586e251c8 222 float aRes, gRes, mRes; // scale resolutions per LSB for the sensors
jbeason3 0:a1a586e251c8 223
jbeason3 0:a1a586e251c8 224 int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output
jbeason3 0:a1a586e251c8 225 int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
jbeason3 0:a1a586e251c8 226 int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output
jbeason3 0:a1a586e251c8 227 float q[4]; // vector to hold quaternion
jbeason3 0:a1a586e251c8 228
jbeason3 0:a1a586e251c8 229 float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values
jbeason3 0:a1a586e251c8 230 float pitch, yaw, roll;
jbeason3 0:a1a586e251c8 231 float deltat; // integration interval for both filter schemes
jbeason3 0:a1a586e251c8 232 int lastUpdate, firstUpdate, Now; // used to calculate integration interval
jbeason3 0:a1a586e251c8 233 int delt_t; // used to control display output rate
jbeason3 0:a1a586e251c8 234 int count; // used to control display output rate
jbeason3 0:a1a586e251c8 235 int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius
jbeason3 0:a1a586e251c8 236 float temperature;
jbeason3 0:a1a586e251c8 237 private:
jbeason3 0:a1a586e251c8 238 I2C *i2c_;
jbeason3 0:a1a586e251c8 239 };