Thread_LPC
Embed:
(wiki syntax)
Show/hide line numbers
MPU9250.h
00001 /*CODED by Qiyong Mu on 21/06/2014 00002 kylongmu@msn.com 00003 00004 Modified by Raghid Mardini 00005 */ 00006 00007 00008 #ifndef mpu9250_h 00009 #define mpu9250_h 00010 #include "mbed.h" 00011 00012 /** IMU class. 00013 * Used to communicate with IMU. 00014 */ 00015 class mpu9250_spi 00016 { 00017 00018 00019 public: 00020 /** Create mpu9250_spi class instance 00021 * @param mosi SPI MOSI pin for encoder 00022 * @param miso SPI MISO pin for encoder 00023 * @param sck SPI SCK pin for encoder 00024 * @param ncs SPI CS pin for encoder 00025 */ 00026 mpu9250_spi(PinName mosi, PinName miso, PinName sck, PinName ncs); 00027 00028 /** Writes one byte to a register address 00029 * @param WriteAddr Register Address 00030 * @param WriteData Data 00031 * @returns temp_data recieved from SPI 00032 */ 00033 unsigned int WriteReg( uint8_t WriteAddr, uint8_t WriteData ); 00034 00035 /** Reads one byte from a register address 00036 * @param WriteAddr Register Address 00037 * @param WriteData Data 00038 * @returns temp_data recieved from SPI 00039 */ 00040 unsigned int ReadReg( uint8_t WriteAddr, uint8_t WriteData ); 00041 00042 /** Reads multiple bytes from a register address 00043 * @param ReadAddr Register Address 00044 * @param ReadBuf Pointer to an array where data will be written 00045 * @param Bytes Number of bytes to read 00046 */ 00047 void ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes ); 00048 00049 /** Initializes the IMU 00050 * @param sample_rate_div sample rate divider (raging from 0 to 255) 00051 * @param low_pass_filter low pass filter value; suitable values are: 00052 * BITS_DLPF_CFG_256HZ_NOLPF2 00053 * BITS_DLPF_CFG_188HZ 00054 * BITS_DLPF_CFG_98HZ 00055 * BITS_DLPF_CFG_42HZ 00056 * BITS_DLPF_CFG_20HZ 00057 * BITS_DLPF_CFG_10HZ 00058 * BITS_DLPF_CFG_5HZ 00059 * BITS_DLPF_CFG_2100HZ_NOLPF 00060 * @returns 1 if an error occurred 00061 */ 00062 bool init(int sample_rate_div,int low_pass_filter); 00063 00064 /** Reads tempreture reading 00065 */ 00066 void read_temp(); 00067 /** Reads accelerometer reading 00068 */ 00069 void read_acc(); 00070 /** Reads gyroscope reading 00071 */ 00072 void read_rot(); 00073 /** Sets gyroscope scale 00074 * @param scale Gyro scale range. Suitable Values: {BITS_FS_250DPS, BITS_FS_500DPS, BITS_FS_1000DPS, BITS_FS_2000DPS} 00075 * @returns the scale set 00076 */ 00077 unsigned int set_gyro_scale(int scale); 00078 00079 /** Sets accelerometer scale 00080 * @param scale Acce scale range. Suitable Values: {BITS_FS_2G, BITS_FS_4G, BITS_FS_8G, BITS_FS_16G} 00081 * @returns the scale set 00082 */ 00083 unsigned int set_acc_scale(int scale); 00084 00085 /** Calibrates accelerometer 00086 */ 00087 void calib_acc(); 00088 00089 /** Calibrates magnetometer 00090 */ 00091 void AK8963_calib_Magnetometer(); 00092 00093 /** Selects spi slave pin 00094 */ 00095 void select(); 00096 00097 /** Deselects spi slave pin 00098 */ 00099 void deselect(); 00100 00101 /** Returns the mpu9250 I2C address 00102 * @returns I2C adress of mpu9250 00103 */ 00104 unsigned int whoami(); 00105 00106 /** Returns the magnetormeter I2C address 00107 * @returns I2C adress of magnetometer 00108 */ 00109 uint8_t AK8963_whoami(); 00110 00111 /** Reads magnetometer reading 00112 */ 00113 void AK8963_read_Magnetometer(); 00114 00115 /** Reads all readings from all sensors 00116 * 00117 */ 00118 void read_all(); 00119 00120 /** Calibrates mpu9250 and calculates accelerometer and gyroscope bias 00121 */ 00122 void calibrateMPU9250(); //added 00123 00124 /** Calculates quaternions based on Madgwick filter from accelerometer, gyroscope and magnetometer readings 00125 * @param ax Accelermoter x reading 00126 * @param ay Accelermoter y reading 00127 * @param az Accelermoter z reading 00128 * @param gx Gyroscope x reading 00129 * @param gy Gyroscope y reading 00130 * @param gz Gyroscope z reading 00131 * @param mx Magnetometer x reading 00132 * @param my Magnetometer y reading 00133 * @param mz Magnetometer z reading 00134 * @param deltat Time diffference since last calculation 00135 * @param beta Integration gain 00136 */ 00137 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz,float deltat,float beta); 00138 00139 /** Calculates quaternions based on Madgwick filter from accelerometer and gyroscope readings 00140 * @param ax Accelermoter x reading 00141 * @param ay Accelermoter y reading 00142 * @param az Accelermoter z reading 00143 * @param gx Gyroscope x reading 00144 * @param gy Gyroscope y reading 00145 * @param gz Gyroscope z reading 00146 * @param deltat Time diffference since last calculation 00147 * @param beta Integration gain 00148 */ 00149 void MadgwickAHRSupdateIMU( float ax, float ay, float az,float gx, float gy, float gz, float deltat,float beta); 00150 00151 /** Used in Madgwick filter. Calculates inverse squared value. 00152 * @param x input value 00153 * @returns inverse squared 00154 */ 00155 float invSqrt(float x); 00156 00157 /** Calculates euler angle 00158 * @param angle 0=Roll, 1=Pitch, 2=Yaw 00159 * @returns Euler angle 00160 */ 00161 float euler_angle(int angle); 00162 00163 int calib_data[3]; 00164 float Magnetometer_ASA[3]; 00165 float gyroBias[3], accelBias[3]; // Bias corrections for gyro and accelerometer 00166 float accelerometer_data[3]; 00167 float Temperature; 00168 float gyroscope_data[3]; 00169 float Magnetometer[3]; 00170 float roll,pitch,yaw; 00171 float q1,q2,q3,q4; 00172 00173 private: 00174 SPI _spi; 00175 DigitalOut _cs; 00176 00177 float acc_divider; 00178 float gyro_divider; 00179 00180 // PinName _CS_pin; 00181 // PinName _SO_pin; 00182 // PinName _SCK_pin; 00183 float _error; 00184 }; 00185 00186 #endif 00187 00188 00189 #define PI 3.14159265 00190 #define RadToDeg 57.2957795131 00191 #define DegToRad 0.01745329251 00192 00193 00194 // mpu9250 registers 00195 #define MPUREG_XG_OFFS_TC 0x00 00196 #define MPUREG_YG_OFFS_TC 0x01 00197 #define MPUREG_ZG_OFFS_TC 0x02 00198 #define MPUREG_X_FINE_GAIN 0x03 00199 #define MPUREG_Y_FINE_GAIN 0x04 00200 #define MPUREG_Z_FINE_GAIN 0x05 00201 #define MPUREG_XA_OFFS_H 0x06 00202 #define MPUREG_XA_OFFS_L 0x07 00203 #define MPUREG_YA_OFFS_H 0x08 00204 #define MPUREG_YA_OFFS_L 0x09 00205 #define MPUREG_ZA_OFFS_H 0x0A 00206 #define MPUREG_ZA_OFFS_L 0x0B 00207 #define MPUREG_PRODUCT_ID 0x0C 00208 #define MPUREG_SELF_TEST_X 0x0D 00209 #define MPUREG_SELF_TEST_Y 0x0E 00210 #define MPUREG_SELF_TEST_Z 0x0F 00211 #define MPUREG_SELF_TEST_A 0x10 00212 #define MPUREG_XG_OFFS_USRH 0x13 00213 #define MPUREG_XG_OFFS_USRL 0x14 00214 #define MPUREG_YG_OFFS_USRH 0x15 00215 #define MPUREG_YG_OFFS_USRL 0x16 00216 #define MPUREG_ZG_OFFS_USRH 0x17 00217 #define MPUREG_ZG_OFFS_USRL 0x18 00218 #define MPUREG_SMPLRT_DIV 0x19 00219 #define MPUREG_CONFIG 0x1A 00220 #define MPUREG_GYRO_CONFIG 0x1B 00221 #define MPUREG_ACCEL_CONFIG 0x1C 00222 #define MPUREG_ACCEL_CONFIG_2 0x1D 00223 #define MPUREG_LP_ACCEL_ODR 0x1E 00224 #define MPUREG_MOT_THR 0x1F 00225 #define MPUREG_FIFO_EN 0x23 00226 #define MPUREG_I2C_MST_CTRL 0x24 00227 #define MPUREG_I2C_SLV0_ADDR 0x25 00228 #define MPUREG_I2C_SLV0_REG 0x26 00229 #define MPUREG_I2C_SLV0_CTRL 0x27 00230 #define MPUREG_I2C_SLV1_ADDR 0x28 00231 #define MPUREG_I2C_SLV1_REG 0x29 00232 #define MPUREG_I2C_SLV1_CTRL 0x2A 00233 #define MPUREG_I2C_SLV2_ADDR 0x2B 00234 #define MPUREG_I2C_SLV2_REG 0x2C 00235 #define MPUREG_I2C_SLV2_CTRL 0x2D 00236 #define MPUREG_I2C_SLV3_ADDR 0x2E 00237 #define MPUREG_I2C_SLV3_REG 0x2F 00238 #define MPUREG_I2C_SLV3_CTRL 0x30 00239 #define MPUREG_I2C_SLV4_ADDR 0x31 00240 #define MPUREG_I2C_SLV4_REG 0x32 00241 #define MPUREG_I2C_SLV4_DO 0x33 00242 #define MPUREG_I2C_SLV4_CTRL 0x34 00243 #define MPUREG_I2C_SLV4_DI 0x35 00244 #define MPUREG_I2C_MST_STATUS 0x36 00245 #define MPUREG_INT_PIN_CFG 0x37 00246 #define MPUREG_INT_ENABLE 0x38 00247 #define MPUREG_ACCEL_XOUT_H 0x3B 00248 #define MPUREG_ACCEL_XOUT_L 0x3C 00249 #define MPUREG_ACCEL_YOUT_H 0x3D 00250 #define MPUREG_ACCEL_YOUT_L 0x3E 00251 #define MPUREG_ACCEL_ZOUT_H 0x3F 00252 #define MPUREG_ACCEL_ZOUT_L 0x40 00253 #define MPUREG_TEMP_OUT_H 0x41 00254 #define MPUREG_TEMP_OUT_L 0x42 00255 #define MPUREG_GYRO_XOUT_H 0x43 00256 #define MPUREG_GYRO_XOUT_L 0x44 00257 #define MPUREG_GYRO_YOUT_H 0x45 00258 #define MPUREG_GYRO_YOUT_L 0x46 00259 #define MPUREG_GYRO_ZOUT_H 0x47 00260 #define MPUREG_GYRO_ZOUT_L 0x48 00261 #define MPUREG_EXT_SENS_DATA_00 0x49 00262 #define MPUREG_EXT_SENS_DATA_01 0x4A 00263 #define MPUREG_EXT_SENS_DATA_02 0x4B 00264 #define MPUREG_EXT_SENS_DATA_03 0x4C 00265 #define MPUREG_EXT_SENS_DATA_04 0x4D 00266 #define MPUREG_EXT_SENS_DATA_05 0x4E 00267 #define MPUREG_EXT_SENS_DATA_06 0x4F 00268 #define MPUREG_EXT_SENS_DATA_07 0x50 00269 #define MPUREG_EXT_SENS_DATA_08 0x51 00270 #define MPUREG_EXT_SENS_DATA_09 0x52 00271 #define MPUREG_EXT_SENS_DATA_10 0x53 00272 #define MPUREG_EXT_SENS_DATA_11 0x54 00273 #define MPUREG_EXT_SENS_DATA_12 0x55 00274 #define MPUREG_EXT_SENS_DATA_13 0x56 00275 #define MPUREG_EXT_SENS_DATA_14 0x57 00276 #define MPUREG_EXT_SENS_DATA_15 0x58 00277 #define MPUREG_EXT_SENS_DATA_16 0x59 00278 #define MPUREG_EXT_SENS_DATA_17 0x5A 00279 #define MPUREG_EXT_SENS_DATA_18 0x5B 00280 #define MPUREG_EXT_SENS_DATA_19 0x5C 00281 #define MPUREG_EXT_SENS_DATA_20 0x5D 00282 #define MPUREG_EXT_SENS_DATA_21 0x5E 00283 #define MPUREG_EXT_SENS_DATA_22 0x5F 00284 #define MPUREG_EXT_SENS_DATA_23 0x60 00285 #define MPUREG_I2C_SLV0_DO 0x63 00286 #define MPUREG_I2C_SLV1_DO 0x64 00287 #define MPUREG_I2C_SLV2_DO 0x65 00288 #define MPUREG_I2C_SLV3_DO 0x66 00289 #define MPUREG_I2C_MST_DELAY_CTRL 0x67 00290 #define MPUREG_SIGNAL_PATH_RESET 0x68 00291 #define MPUREG_MOT_DETECT_CTRL 0x69 00292 #define MPUREG_USER_CTRL 0x6A 00293 #define MPUREG_PWR_MGMT_1 0x6B 00294 #define MPUREG_PWR_MGMT_2 0x6C 00295 #define MPUREG_BANK_SEL 0x6D 00296 #define MPUREG_MEM_START_ADDR 0x6E 00297 #define MPUREG_MEM_R_W 0x6F 00298 #define MPUREG_DMP_CFG_1 0x70 00299 #define MPUREG_DMP_CFG_2 0x71 00300 #define MPUREG_FIFO_COUNTH 0x72 00301 #define MPUREG_FIFO_COUNTL 0x73 00302 #define MPUREG_FIFO_R_W 0x74 00303 #define MPUREG_WHOAMI 0x75 00304 #define MPUREG_XA_OFFSET_H 0x77 00305 #define MPUREG_XA_OFFSET_L 0x78 00306 #define MPUREG_YA_OFFSET_H 0x7A 00307 #define MPUREG_YA_OFFSET_L 0x7B 00308 #define MPUREG_ZA_OFFSET_H 0x7D 00309 #define MPUREG_ZA_OFFSET_L 0x7E 00310 /* ---- AK8963 Reg In MPU9250 ----------------------------------------------- */ 00311 00312 #define AK8963_I2C_ADDR 0x0c//0x18 00313 #define AK8963_Device_ID 0x48 00314 00315 // Read-only Reg 00316 #define AK8963_WIA 0x00 00317 #define AK8963_INFO 0x01 00318 #define AK8963_ST1 0x02 00319 #define AK8963_HXL 0x03 00320 #define AK8963_HXH 0x04 00321 #define AK8963_HYL 0x05 00322 #define AK8963_HYH 0x06 00323 #define AK8963_HZL 0x07 00324 #define AK8963_HZH 0x08 00325 #define AK8963_ST2 0x09 00326 // Write/Read Reg 00327 #define AK8963_CNTL1 0x0A 00328 #define AK8963_CNTL2 0x0B 00329 #define AK8963_ASTC 0x0C 00330 #define AK8963_TS1 0x0D 00331 #define AK8963_TS2 0x0E 00332 #define AK8963_I2CDIS 0x0F 00333 // Read-only Reg ( ROM ) 00334 #define AK8963_ASAX 0x10 00335 #define AK8963_ASAY 0x11 00336 #define AK8963_ASAZ 0x12 00337 00338 // Configuration bits mpu9250 00339 #define BIT_SLEEP 0x40 00340 #define BIT_H_RESET 0x80 00341 #define BITS_CLKSEL 0x07 00342 #define MPU_CLK_SEL_PLLGYROX 0x01 00343 #define MPU_CLK_SEL_PLLGYROZ 0x03 00344 #define MPU_EXT_SYNC_GYROX 0x02 00345 #define BITS_FS_250DPS 0x00 00346 #define BITS_FS_500DPS 0x08 00347 #define BITS_FS_1000DPS 0x10 00348 #define BITS_FS_2000DPS 0x18 00349 #define BITS_FS_2G 0x00 00350 #define BITS_FS_4G 0x08 00351 #define BITS_FS_8G 0x10 00352 #define BITS_FS_16G 0x18 00353 #define BITS_FS_MASK 0x18 00354 #define BITS_DLPF_CFG_256HZ_NOLPF2 0x00 00355 #define BITS_DLPF_CFG_188HZ 0x01 00356 #define BITS_DLPF_CFG_98HZ 0x02 00357 #define BITS_DLPF_CFG_42HZ 0x03 00358 #define BITS_DLPF_CFG_20HZ 0x04 00359 #define BITS_DLPF_CFG_10HZ 0x05 00360 #define BITS_DLPF_CFG_5HZ 0x06 00361 #define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 00362 #define BITS_DLPF_CFG_MASK 0x07 00363 #define BIT_INT_ANYRD_2CLEAR 0x10 00364 #define BIT_RAW_RDY_EN 0x01 00365 #define BIT_I2C_IF_DIS 0x10 00366 00367 #define READ_FLAG 0x80 00368 00369 /* ---- Sensitivity --------------------------------------------------------- */ 00370 00371 #define MPU9250A_2g ((float)0.000061035156f) // 0.000061035156 g/LSB 00372 #define MPU9250A_4g ((float)0.000122070312f) // 0.000122070312 g/LSB 00373 #define MPU9250A_8g ((float)0.000244140625f) // 0.000244140625 g/LSB 00374 #define MPU9250A_16g ((float)0.000488281250f) // 0.000488281250 g/LSB 00375 00376 #define MPU9250G_250dps ((float)0.007633587786f) // 0.007633587786 dps/LSB 00377 #define MPU9250G_500dps ((float)0.015267175572f) // 0.015267175572 dps/LSB 00378 #define MPU9250G_1000dps ((float)0.030487804878f) // 0.030487804878 dps/LSB 00379 #define MPU9250G_2000dps ((float)0.060975609756f) // 0.060975609756 dps/LSB 00380 00381 #define MPU9250M_4800uT ((float)0.6f) // 0.6 uT/LSB 00382 00383 #define MPU9250T_85degC ((float)0.002995177763f) // 0.002995177763 degC/LSB 00384 00385 #define Magnetometer_Sensitivity_Scale_Factor ((float)0.15f) 00386 00387 // For Calibration Function 00388 #define XA_OFFSET_H 0x77 00389 #define XA_OFFSET_L 0x78 00390 #define YA_OFFSET_H 0x7A 00391 #define YA_OFFSET_L 0x7B 00392 #define ZA_OFFSET_H 0x7D 00393 #define ZA_OFFSET_L 0x7E 00394 #define PWR_MGMT_2 0x6C 00395 #define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP 00396 #define CONFIG 0x1A 00397 #define GYRO_CONFIG 0x1B 00398 #define ACCEL_CONFIG 0x1C 00399 #define FIFO_EN 0x23 00400 #define I2C_MST_CTRL 0x24 00401 #define FIFO_COUNTH 0x72 00402 #define FIFO_COUNTL 0x73 00403 #define FIFO_R_W 0x74 00404 #define SMPLRT_DIV 0x19 00405 #define PWR_MGMT_1 0x6B 00406 00407 ///////////// 00408
Generated on Sun Jul 17 2022 08:21:43 by
1.7.2