Project Football / Mbed 2 deprecated Nucleo_MPU_9250_

as

Dependencies:   CommonTypes ESC Matrix PID Servo kalman mbed-rtos mbed

Fork of Nucleo_MPU_9250_ by Alan Huchin Herrera

MPU9250.h@0:89cf0851969b, 2018-06-26 (annotated)

Committer:
AlanHuchin
Date:
Tue Jun 26 18:24:45 2018 +0000
Revision:
0:89cf0851969b
hello

Who changed what in which revision?

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