MPU9250
Dependents: pdiot-ble-notify-array StepTracking IMU_MPU925X_YAW chair_MPU9250 ... more
MPU9250.h
00001 #include "mbed.h" 00002 #include "math.h" 00003 00004 // 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 00005 // above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map 00006 // 00007 //Magnetometer Registers 00008 #define AK8963_ADDRESS 0x0C<<1 00009 #define WHO_AM_I_AK8963 0x00 // should return 0x48 00010 #define INFO 0x01 00011 #define AK8963_ST1 0x02 // data ready status bit 0 00012 #define AK8963_XOUT_L 0x03 // data 00013 #define AK8963_XOUT_H 0x04 00014 #define AK8963_YOUT_L 0x05 00015 #define AK8963_YOUT_H 0x06 00016 #define AK8963_ZOUT_L 0x07 00017 #define AK8963_ZOUT_H 0x08 00018 #define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 00019 #define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 00020 #define AK8963_ASTC 0x0C // Self test control 00021 #define AK8963_I2CDIS 0x0F // I2C disable 00022 #define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value 00023 #define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value 00024 #define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value 00025 00026 #define SELF_TEST_X_GYRO 0x00 00027 #define SELF_TEST_Y_GYRO 0x01 00028 #define SELF_TEST_Z_GYRO 0x02 00029 00030 /*#define X_FINE_GAIN 0x03 // [7:0] fine gain 00031 #define Y_FINE_GAIN 0x04 00032 #define Z_FINE_GAIN 0x05 00033 #define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer 00034 #define XA_OFFSET_L_TC 0x07 00035 #define YA_OFFSET_H 0x08 00036 #define YA_OFFSET_L_TC 0x09 00037 #define ZA_OFFSET_H 0x0A 00038 #define ZA_OFFSET_L_TC 0x0B */ 00039 00040 #define SELF_TEST_X_ACCEL 0x0D 00041 #define SELF_TEST_Y_ACCEL 0x0E 00042 #define SELF_TEST_Z_ACCEL 0x0F 00043 00044 #define SELF_TEST_A 0x10 00045 00046 #define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope 00047 #define XG_OFFSET_L 0x14 00048 #define YG_OFFSET_H 0x15 00049 #define YG_OFFSET_L 0x16 00050 #define ZG_OFFSET_H 0x17 00051 #define ZG_OFFSET_L 0x18 00052 #define SMPLRT_DIV 0x19 00053 #define CONFIG 0x1A 00054 #define GYRO_CONFIG 0x1B 00055 #define ACCEL_CONFIG 0x1C 00056 #define ACCEL_CONFIG2 0x1D 00057 #define LP_ACCEL_ODR 0x1E 00058 #define WOM_THR 0x1F 00059 00060 #define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms 00061 #define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] 00062 #define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms 00063 00064 #define FIFO_EN 0x23 00065 #define I2C_MST_CTRL 0x24 00066 #define I2C_SLV0_ADDR 0x25 00067 #define I2C_SLV0_REG 0x26 00068 #define I2C_SLV0_CTRL 0x27 00069 #define I2C_SLV1_ADDR 0x28 00070 #define I2C_SLV1_REG 0x29 00071 #define I2C_SLV1_CTRL 0x2A 00072 #define I2C_SLV2_ADDR 0x2B 00073 #define I2C_SLV2_REG 0x2C 00074 #define I2C_SLV2_CTRL 0x2D 00075 #define I2C_SLV3_ADDR 0x2E 00076 #define I2C_SLV3_REG 0x2F 00077 #define I2C_SLV3_CTRL 0x30 00078 #define I2C_SLV4_ADDR 0x31 00079 #define I2C_SLV4_REG 0x32 00080 #define I2C_SLV4_DO 0x33 00081 #define I2C_SLV4_CTRL 0x34 00082 #define I2C_SLV4_DI 0x35 00083 #define I2C_MST_STATUS 0x36 00084 #define INT_PIN_CFG 0x37 00085 #define INT_ENABLE 0x38 00086 #define DMP_INT_STATUS 0x39 // Check DMP interrupt 00087 #define INT_STATUS 0x3A 00088 #define ACCEL_XOUT_H 0x3B 00089 #define ACCEL_XOUT_L 0x3C 00090 #define ACCEL_YOUT_H 0x3D 00091 #define ACCEL_YOUT_L 0x3E 00092 #define ACCEL_ZOUT_H 0x3F 00093 #define ACCEL_ZOUT_L 0x40 00094 #define TEMP_OUT_H 0x41 00095 #define TEMP_OUT_L 0x42 00096 #define GYRO_XOUT_H 0x43 00097 #define GYRO_XOUT_L 0x44 00098 #define GYRO_YOUT_H 0x45 00099 #define GYRO_YOUT_L 0x46 00100 #define GYRO_ZOUT_H 0x47 00101 #define GYRO_ZOUT_L 0x48 00102 #define EXT_SENS_DATA_00 0x49 00103 #define EXT_SENS_DATA_01 0x4A 00104 #define EXT_SENS_DATA_02 0x4B 00105 #define EXT_SENS_DATA_03 0x4C 00106 #define EXT_SENS_DATA_04 0x4D 00107 #define EXT_SENS_DATA_05 0x4E 00108 #define EXT_SENS_DATA_06 0x4F 00109 #define EXT_SENS_DATA_07 0x50 00110 #define EXT_SENS_DATA_08 0x51 00111 #define EXT_SENS_DATA_09 0x52 00112 #define EXT_SENS_DATA_10 0x53 00113 #define EXT_SENS_DATA_11 0x54 00114 #define EXT_SENS_DATA_12 0x55 00115 #define EXT_SENS_DATA_13 0x56 00116 #define EXT_SENS_DATA_14 0x57 00117 #define EXT_SENS_DATA_15 0x58 00118 #define EXT_SENS_DATA_16 0x59 00119 #define EXT_SENS_DATA_17 0x5A 00120 #define EXT_SENS_DATA_18 0x5B 00121 #define EXT_SENS_DATA_19 0x5C 00122 #define EXT_SENS_DATA_20 0x5D 00123 #define EXT_SENS_DATA_21 0x5E 00124 #define EXT_SENS_DATA_22 0x5F 00125 #define EXT_SENS_DATA_23 0x60 00126 #define MOT_DETECT_STATUS 0x61 00127 #define I2C_SLV0_DO 0x63 00128 #define I2C_SLV1_DO 0x64 00129 #define I2C_SLV2_DO 0x65 00130 #define I2C_SLV3_DO 0x66 00131 #define I2C_MST_DELAY_CTRL 0x67 00132 #define SIGNAL_PATH_RESET 0x68 00133 #define MOT_DETECT_CTRL 0x69 00134 #define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP 00135 #define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode 00136 #define PWR_MGMT_2 0x6C 00137 #define DMP_BANK 0x6D // Activates a specific bank in the DMP 00138 #define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank 00139 #define DMP_REG 0x6F // Register in DMP from which to read or to which to write 00140 #define DMP_REG_1 0x70 00141 #define DMP_REG_2 0x71 00142 #define FIFO_COUNTH 0x72 00143 #define FIFO_COUNTL 0x73 00144 #define FIFO_R_W 0x74 00145 #define WHO_AM_I_MPU9250 0x75 // Should return 0x71 00146 #define XA_OFFSET_H 0x77 00147 #define XA_OFFSET_L 0x78 00148 #define YA_OFFSET_H 0x7A 00149 #define YA_OFFSET_L 0x7B 00150 #define ZA_OFFSET_H 0x7D 00151 #define ZA_OFFSET_L 0x7E 00152 00153 #define PI 3.14159265358979323846f 00154 // Using the MSENSR-9250 breakout board, ADO is set to 0 00155 // Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 00156 //mbed uses the eight-bit device address, so shift seven-bit addresses left by one! 00157 #define ADO 0 00158 #if ADO 00159 #define MPU9250_ADDRESS 0x69<<1 // Device address when ADO = 1 00160 #else 00161 #define MPU9250_ADDRESS 0x68<<1 // Device address when ADO = 0 00162 #endif 00163 00164 // Set initial input parameters 00165 enum Ascale { 00166 AFS_2G = 0, 00167 AFS_4G, 00168 AFS_8G, 00169 AFS_16G 00170 }; 00171 00172 enum Gscale { 00173 GFS_250DPS = 0, 00174 GFS_500DPS, 00175 GFS_1000DPS, 00176 GFS_2000DPS 00177 }; 00178 00179 enum Mscale { 00180 MFS_14BITS = 0, // 0.6 mG per LSB 00181 MFS_16BITS // 0.15 mG per LSB 00182 }; 00183 00184 class MPU9250 { 00185 00186 protected: 00187 00188 public: 00189 00190 MPU9250(PinName sda, PinName scl); 00191 MPU9250(I2C *i2c); 00192 ~MPU9250(); 00193 00194 void writeByte(uint8_t address, uint8_t subAddress, uint8_t data); 00195 char readByte(uint8_t address, uint8_t subAddress); 00196 void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest); 00197 void getMres(); 00198 void getGres(); 00199 void getAres(); 00200 void readAccelData(int16_t * destination); 00201 void readGyroData(int16_t * destination); 00202 void readMagData(int16_t * destination); 00203 int16_t readTempData(); 00204 void resetMPU9250(); 00205 void initAK8963(float * destination); 00206 void initMPU9250(); 00207 void calibrateMPU9250(float * dest1, float * dest2); 00208 void MPU9250SelfTest(float * destination); 00209 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); 00210 void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); 00211 00212 float SelfTest[6]; 00213 float gyroBias[3],accelBias[3]; // Bias corrections for gyro and accelerometer 00214 float magCalibration[3], magbias[3]; // Factory mag calibration and mag bias 00215 00216 uint8_t Ascale; // AFS_2G, AFS_4G, AFS_8G, AFS_16G 00217 uint8_t Gscale; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS 00218 uint8_t Mscale; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution 00219 uint8_t Mmode; // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR 00220 float aRes, gRes, mRes; // scale resolutions per LSB for the sensors 00221 00222 int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output 00223 int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output 00224 int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output 00225 float q[4]; // vector to hold quaternion 00226 00227 float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 00228 float pitch, yaw, roll; 00229 float deltat; // integration interval for both filter schemes 00230 int lastUpdate, firstUpdate, Now; // used to calculate integration interval 00231 int delt_t; // used to control display output rate 00232 int count; // used to control display output rate 00233 int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius 00234 float temperature; 00235 private: 00236 I2C *i2c_; 00237 };
Generated on Tue Jul 12 2022 15:39:13 by 1.7.2