SAKURA Internet / MPU9250_SPI

forked

Fork of MPU9250_SPI by Mu kylong

MPU9250.cpp@3:f4fa24cc247d, 2014-06-25 (annotated)

Committer:
kylongmu
Date:
Wed Jun 25 15:41:05 2014 +0000
Revision:
3:f4fa24cc247d
Parent:
2:f274ea3bced9
Child:
4:79185409730f
It's function now, but Magnetometer part not tested.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kylongmu 0:768d2e151834 1 /*CODED by Qiyong Mu on 21/06/2014
kylongmu 0:768d2e151834 2
kylongmu 0:768d2e151834 3 */
kylongmu 0:768d2e151834 4
kylongmu 0:768d2e151834 5 #include <mbed.h>
kylongmu 1:f738165e54f0 6 #include "MPU9250.h"
kylongmu 0:768d2e151834 7
kylongmu 0:768d2e151834 8 mpu9250_spi::mpu9250_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs) {}
kylongmu 0:768d2e151834 9
kylongmu 0:768d2e151834 10 unsigned int mpu9250_spi::WriteReg( uint8_t WriteAddr, uint8_t WriteData )
kylongmu 0:768d2e151834 11 {
kylongmu 0:768d2e151834 12 unsigned int temp_val;
kylongmu 0:768d2e151834 13 select();
kylongmu 0:768d2e151834 14 spi.write(WriteAddr);
kylongmu 0:768d2e151834 15 temp_val=spi.write(WriteData);
kylongmu 0:768d2e151834 16 deselect();
kylongmu 0:768d2e151834 17 wait_us(50);
kylongmu 0:768d2e151834 18 return temp_val;
kylongmu 0:768d2e151834 19 }
kylongmu 2:f274ea3bced9 20 unsigned int mpu9250_spi::ReadReg( uint8_t WriteAddr, uint8_t WriteData )
kylongmu 2:f274ea3bced9 21 {
kylongmu 2:f274ea3bced9 22 return WriteReg(WriteAddr | READ_FLAG,WriteData);
kylongmu 2:f274ea3bced9 23 }
kylongmu 2:f274ea3bced9 24 void mpu9250_spi::ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes )
kylongmu 0:768d2e151834 25 {
kylongmu 0:768d2e151834 26 unsigned int i = 0;
kylongmu 0:768d2e151834 27
kylongmu 0:768d2e151834 28 select();
kylongmu 0:768d2e151834 29 spi.write(ReadAddr | READ_FLAG);
kylongmu 0:768d2e151834 30 for(i=0; i<Bytes; i++)
kylongmu 0:768d2e151834 31 ReadBuf[i] = spi.write(0x00);
kylongmu 0:768d2e151834 32 deselect();
kylongmu 0:768d2e151834 33 wait_us(50);
kylongmu 0:768d2e151834 34 }
kylongmu 0:768d2e151834 35
kylongmu 0:768d2e151834 36 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 37 INITIALIZATION
kylongmu 0:768d2e151834 38 usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and
kylongmu 0:768d2e151834 39 low pass filter value; suitable values are:
kylongmu 0:768d2e151834 40 BITS_DLPF_CFG_256HZ_NOLPF2
kylongmu 0:768d2e151834 41 BITS_DLPF_CFG_188HZ
kylongmu 0:768d2e151834 42 BITS_DLPF_CFG_98HZ
kylongmu 0:768d2e151834 43 BITS_DLPF_CFG_42HZ
kylongmu 0:768d2e151834 44 BITS_DLPF_CFG_20HZ
kylongmu 0:768d2e151834 45 BITS_DLPF_CFG_10HZ
kylongmu 0:768d2e151834 46 BITS_DLPF_CFG_5HZ
kylongmu 0:768d2e151834 47 BITS_DLPF_CFG_2100HZ_NOLPF
kylongmu 0:768d2e151834 48 returns 1 if an error occurred
kylongmu 0:768d2e151834 49 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 50 #define MPU_InitRegNum 10
kylongmu 0:768d2e151834 51
kylongmu 0:768d2e151834 52 bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter){
kylongmu 0:768d2e151834 53 uint8_t i = 0;
kylongmu 0:768d2e151834 54 uint8_t MPU_Init_Data[MPU_InitRegNum][2] = {
kylongmu 0:768d2e151834 55 {0x80, MPUREG_PWR_MGMT_1}, // Reset Device
kylongmu 0:768d2e151834 56 {0x01, MPUREG_PWR_MGMT_1}, // Clock Source
kylongmu 0:768d2e151834 57 {0x00, MPUREG_PWR_MGMT_2}, // Enable Acc & Gyro
kylongmu 0:768d2e151834 58 {0x07, MPUREG_CONFIG}, //
kylongmu 0:768d2e151834 59 {0x18, MPUREG_GYRO_CONFIG}, // +-2000dps
kylongmu 0:768d2e151834 60 {0x08, MPUREG_ACCEL_CONFIG}, // +-4G
kylongmu 0:768d2e151834 61 {0x00, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates
kylongmu 0:768d2e151834 62 {0x30, MPUREG_INT_PIN_CFG}, //
kylongmu 0:768d2e151834 63 {0x40, MPUREG_I2C_MST_CTRL}, // I2C Speed 348 kHz
kylongmu 0:768d2e151834 64 {0x20, MPUREG_USER_CTRL}, // Enable AUX
kylongmu 0:768d2e151834 65 };
kylongmu 0:768d2e151834 66 spi.format(8,0);
kylongmu 0:768d2e151834 67 spi.frequency(1000000);
kylongmu 0:768d2e151834 68
kylongmu 0:768d2e151834 69 for(i=0; i<MPU_InitRegNum; i++) {
kylongmu 0:768d2e151834 70 WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
kylongmu 0:768d2e151834 71 }
kylongmu 0:768d2e151834 72 return 0;
kylongmu 0:768d2e151834 73 }
kylongmu 0:768d2e151834 74
kylongmu 0:768d2e151834 75 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 76 ACCELEROMETER SCALE
kylongmu 0:768d2e151834 77 usage: call this function at startup, after initialization, to set the right range for the
kylongmu 0:768d2e151834 78 accelerometers. Suitable ranges are:
kylongmu 0:768d2e151834 79 BITS_FS_2G
kylongmu 0:768d2e151834 80 BITS_FS_4G
kylongmu 0:768d2e151834 81 BITS_FS_8G
kylongmu 0:768d2e151834 82 BITS_FS_16G
kylongmu 0:768d2e151834 83 returns the range set (2,4,8 or 16)
kylongmu 0:768d2e151834 84 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 85 unsigned int mpu9250_spi::set_acc_scale(int scale){
kylongmu 0:768d2e151834 86 unsigned int temp_scale;
kylongmu 0:768d2e151834 87 WriteReg(MPUREG_ACCEL_CONFIG, scale);
kylongmu 0:768d2e151834 88
kylongmu 0:768d2e151834 89 switch (scale){
kylongmu 0:768d2e151834 90 case BITS_FS_2G:
kylongmu 0:768d2e151834 91 acc_divider=16384;
kylongmu 0:768d2e151834 92 break;
kylongmu 0:768d2e151834 93 case BITS_FS_4G:
kylongmu 0:768d2e151834 94 acc_divider=8192;
kylongmu 0:768d2e151834 95 break;
kylongmu 0:768d2e151834 96 case BITS_FS_8G:
kylongmu 0:768d2e151834 97 acc_divider=4096;
kylongmu 0:768d2e151834 98 break;
kylongmu 0:768d2e151834 99 case BITS_FS_16G:
kylongmu 0:768d2e151834 100 acc_divider=2048;
kylongmu 0:768d2e151834 101 break;
kylongmu 0:768d2e151834 102 }
kylongmu 0:768d2e151834 103 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
kylongmu 0:768d2e151834 104
kylongmu 0:768d2e151834 105 switch (temp_scale){
kylongmu 0:768d2e151834 106 case BITS_FS_2G:
kylongmu 0:768d2e151834 107 temp_scale=2;
kylongmu 0:768d2e151834 108 break;
kylongmu 0:768d2e151834 109 case BITS_FS_4G:
kylongmu 0:768d2e151834 110 temp_scale=4;
kylongmu 0:768d2e151834 111 break;
kylongmu 0:768d2e151834 112 case BITS_FS_8G:
kylongmu 0:768d2e151834 113 temp_scale=8;
kylongmu 0:768d2e151834 114 break;
kylongmu 0:768d2e151834 115 case BITS_FS_16G:
kylongmu 0:768d2e151834 116 temp_scale=16;
kylongmu 0:768d2e151834 117 break;
kylongmu 0:768d2e151834 118 }
kylongmu 0:768d2e151834 119 return temp_scale;
kylongmu 0:768d2e151834 120 }
kylongmu 0:768d2e151834 121
kylongmu 0:768d2e151834 122
kylongmu 0:768d2e151834 123 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 124 GYROSCOPE SCALE
kylongmu 0:768d2e151834 125 usage: call this function at startup, after initialization, to set the right range for the
kylongmu 0:768d2e151834 126 gyroscopes. Suitable ranges are:
kylongmu 0:768d2e151834 127 BITS_FS_250DPS
kylongmu 0:768d2e151834 128 BITS_FS_500DPS
kylongmu 0:768d2e151834 129 BITS_FS_1000DPS
kylongmu 0:768d2e151834 130 BITS_FS_2000DPS
kylongmu 0:768d2e151834 131 returns the range set (250,500,1000 or 2000)
kylongmu 0:768d2e151834 132 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 133 unsigned int mpu9250_spi::set_gyro_scale(int scale){
kylongmu 0:768d2e151834 134 unsigned int temp_scale;
kylongmu 0:768d2e151834 135 WriteReg(MPUREG_GYRO_CONFIG, scale);
kylongmu 0:768d2e151834 136 switch (scale){
kylongmu 0:768d2e151834 137 case BITS_FS_250DPS:
kylongmu 0:768d2e151834 138 gyro_divider=131;
kylongmu 0:768d2e151834 139 break;
kylongmu 0:768d2e151834 140 case BITS_FS_500DPS:
kylongmu 0:768d2e151834 141 gyro_divider=65.5;
kylongmu 0:768d2e151834 142 break;
kylongmu 0:768d2e151834 143 case BITS_FS_1000DPS:
kylongmu 0:768d2e151834 144 gyro_divider=32.8;
kylongmu 0:768d2e151834 145 break;
kylongmu 0:768d2e151834 146 case BITS_FS_2000DPS:
kylongmu 0:768d2e151834 147 gyro_divider=16.4;
kylongmu 0:768d2e151834 148 break;
kylongmu 0:768d2e151834 149 }
kylongmu 0:768d2e151834 150 temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00);
kylongmu 0:768d2e151834 151 switch (temp_scale){
kylongmu 0:768d2e151834 152 case BITS_FS_250DPS:
kylongmu 0:768d2e151834 153 temp_scale=250;
kylongmu 0:768d2e151834 154 break;
kylongmu 0:768d2e151834 155 case BITS_FS_500DPS:
kylongmu 0:768d2e151834 156 temp_scale=500;
kylongmu 0:768d2e151834 157 break;
kylongmu 0:768d2e151834 158 case BITS_FS_1000DPS:
kylongmu 0:768d2e151834 159 temp_scale=1000;
kylongmu 0:768d2e151834 160 break;
kylongmu 0:768d2e151834 161 case BITS_FS_2000DPS:
kylongmu 0:768d2e151834 162 temp_scale=2000;
kylongmu 0:768d2e151834 163 break;
kylongmu 0:768d2e151834 164 }
kylongmu 0:768d2e151834 165 return temp_scale;
kylongmu 0:768d2e151834 166 }
kylongmu 0:768d2e151834 167
kylongmu 0:768d2e151834 168
kylongmu 0:768d2e151834 169 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 170 WHO AM I?
kylongmu 0:768d2e151834 171 usage: call this function to know if SPI is working correctly. It checks the I2C address of the
kylongmu 0:768d2e151834 172 mpu9250 which should be 104 when in SPI mode.
kylongmu 0:768d2e151834 173 returns the I2C address (104)
kylongmu 0:768d2e151834 174 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 175 unsigned int mpu9250_spi::whoami(){
kylongmu 0:768d2e151834 176 unsigned int response;
kylongmu 0:768d2e151834 177 response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00);
kylongmu 0:768d2e151834 178 return response;
kylongmu 0:768d2e151834 179 }
kylongmu 0:768d2e151834 180
kylongmu 0:768d2e151834 181
kylongmu 0:768d2e151834 182 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 183 READ ACCELEROMETER
kylongmu 0:768d2e151834 184 usage: call this function to read accelerometer data. Axis represents selected axis:
kylongmu 0:768d2e151834 185 0 -> X axis
kylongmu 0:768d2e151834 186 1 -> Y axis
kylongmu 0:768d2e151834 187 2 -> Z axis
kylongmu 0:768d2e151834 188 returns the value in Gs
kylongmu 0:768d2e151834 189 -----------------------------------------------------------------------------------------------*/
kylongmu 2:f274ea3bced9 190 void mpu9250_spi::read_acc()
kylongmu 2:f274ea3bced9 191 {
kylongmu 2:f274ea3bced9 192 uint8_t response[2];
kylongmu 0:768d2e151834 193 int16_t bit_data;
kylongmu 0:768d2e151834 194 float data;
kylongmu 2:f274ea3bced9 195
kylongmu 2:f274ea3bced9 196 ReadRegs(MPUREG_ACCEL_XOUT_H,response,2);
kylongmu 2:f274ea3bced9 197 bit_data=((int16_t)response[0]<<8)|response[1];
kylongmu 0:768d2e151834 198 data=(float)bit_data;
kylongmu 2:f274ea3bced9 199 accelerometer_data[0]=data/acc_divider;
kylongmu 2:f274ea3bced9 200
kylongmu 2:f274ea3bced9 201 ReadRegs(MPUREG_ACCEL_YOUT_H,response,2);
kylongmu 2:f274ea3bced9 202 bit_data=((int16_t)response[0]<<8)|response[1];
kylongmu 2:f274ea3bced9 203 data=(float)bit_data;
kylongmu 2:f274ea3bced9 204 accelerometer_data[1]=data/acc_divider;
kylongmu 2:f274ea3bced9 205
kylongmu 2:f274ea3bced9 206 ReadRegs(MPUREG_ACCEL_ZOUT_H,response,2);
kylongmu 2:f274ea3bced9 207 bit_data=((int16_t)response[0]<<8)|response[1];
kylongmu 2:f274ea3bced9 208 data=(float)bit_data;
kylongmu 2:f274ea3bced9 209 accelerometer_data[2]=data/acc_divider;
kylongmu 0:768d2e151834 210 }
kylongmu 0:768d2e151834 211
kylongmu 0:768d2e151834 212 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 213 READ GYROSCOPE
kylongmu 0:768d2e151834 214 usage: call this function to read gyroscope data. Axis represents selected axis:
kylongmu 0:768d2e151834 215 0 -> X axis
kylongmu 0:768d2e151834 216 1 -> Y axis
kylongmu 0:768d2e151834 217 2 -> Z axis
kylongmu 0:768d2e151834 218 returns the value in Degrees per second
kylongmu 0:768d2e151834 219 -----------------------------------------------------------------------------------------------*/
kylongmu 2:f274ea3bced9 220 void mpu9250_spi::read_rot()
kylongmu 2:f274ea3bced9 221 {
kylongmu 2:f274ea3bced9 222 uint8_t response[2];
kylongmu 0:768d2e151834 223 int16_t bit_data;
kylongmu 0:768d2e151834 224 float data;
kylongmu 2:f274ea3bced9 225
kylongmu 2:f274ea3bced9 226 ReadRegs(MPUREG_GYRO_XOUT_H,response,2);
kylongmu 2:f274ea3bced9 227 bit_data=((int16_t)response[0]<<8)|response[1];
kylongmu 0:768d2e151834 228 data=(float)bit_data;
kylongmu 2:f274ea3bced9 229 gyroscope_data[0]=data/gyro_divider;
kylongmu 2:f274ea3bced9 230
kylongmu 2:f274ea3bced9 231 ReadRegs(MPUREG_GYRO_YOUT_H,response,2);
kylongmu 2:f274ea3bced9 232 bit_data=((int16_t)response[0]<<8)|response[1];
kylongmu 2:f274ea3bced9 233 data=(float)bit_data;
kylongmu 2:f274ea3bced9 234 gyroscope_data[1]=data/gyro_divider;
kylongmu 2:f274ea3bced9 235
kylongmu 2:f274ea3bced9 236 ReadRegs(MPUREG_GYRO_ZOUT_H,response,2);
kylongmu 2:f274ea3bced9 237 bit_data=((int16_t)response[0]<<8)|response[1];
kylongmu 2:f274ea3bced9 238 data=(float)bit_data;
kylongmu 2:f274ea3bced9 239 gyroscope_data[2]=data/gyro_divider;
kylongmu 0:768d2e151834 240 }
kylongmu 0:768d2e151834 241
kylongmu 0:768d2e151834 242 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 243 READ TEMPERATURE
kylongmu 0:768d2e151834 244 usage: call this function to read temperature data.
kylongmu 0:768d2e151834 245 returns the value in °C
kylongmu 0:768d2e151834 246 -----------------------------------------------------------------------------------------------*/
kylongmu 3:f4fa24cc247d 247 void mpu9250_spi::read_temp(){
kylongmu 2:f274ea3bced9 248 uint8_t response[2];
kylongmu 0:768d2e151834 249 int16_t bit_data;
kylongmu 0:768d2e151834 250 float data;
kylongmu 2:f274ea3bced9 251 ReadRegs(MPUREG_TEMP_OUT_H,response,2);
kylongmu 2:f274ea3bced9 252
kylongmu 2:f274ea3bced9 253 bit_data=((int16_t)response[0]<<8)|response[1];
kylongmu 0:768d2e151834 254 data=(float)bit_data;
kylongmu 3:f4fa24cc247d 255 Temperature=(data/340)+36.53;
kylongmu 0:768d2e151834 256 deselect();
kylongmu 0:768d2e151834 257 }
kylongmu 0:768d2e151834 258
kylongmu 0:768d2e151834 259 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 260 READ ACCELEROMETER CALIBRATION
kylongmu 0:768d2e151834 261 usage: call this function to read accelerometer data. Axis represents selected axis:
kylongmu 0:768d2e151834 262 0 -> X axis
kylongmu 0:768d2e151834 263 1 -> Y axis
kylongmu 0:768d2e151834 264 2 -> Z axis
kylongmu 0:768d2e151834 265 returns Factory Trim value
kylongmu 0:768d2e151834 266 -----------------------------------------------------------------------------------------------*/
kylongmu 2:f274ea3bced9 267 void mpu9250_spi::calib_acc()
kylongmu 2:f274ea3bced9 268 {
kylongmu 2:f274ea3bced9 269 uint8_t response[4];
kylongmu 0:768d2e151834 270 int temp_scale;
kylongmu 0:768d2e151834 271 //READ CURRENT ACC SCALE
kylongmu 0:768d2e151834 272 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
kylongmu 0:768d2e151834 273 set_acc_scale(BITS_FS_8G);
kylongmu 2:f274ea3bced9 274 //ENABLE SELF TEST need modify
kylongmu 2:f274ea3bced9 275 //temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis);
kylongmu 0:768d2e151834 276
kylongmu 2:f274ea3bced9 277 ReadRegs(MPUREG_SELF_TEST_X,response,4);
kylongmu 2:f274ea3bced9 278 calib_data[0]=((response[0]&11100000)>>3)|((response[3]&00110000)>>4);
kylongmu 2:f274ea3bced9 279 calib_data[1]=((response[1]&11100000)>>3)|((response[3]&00001100)>>2);
kylongmu 2:f274ea3bced9 280 calib_data[2]=((response[2]&11100000)>>3)|((response[3]&00000011));
kylongmu 2:f274ea3bced9 281
kylongmu 0:768d2e151834 282 set_acc_scale(temp_scale);
kylongmu 2:f274ea3bced9 283 }
kylongmu 0:768d2e151834 284
kylongmu 0:768d2e151834 285 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 286 SPI SELECT AND DESELECT
kylongmu 0:768d2e151834 287 usage: enable and disable mpu9250 communication bus
kylongmu 0:768d2e151834 288 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 289 void mpu9250_spi::select() {
kylongmu 0:768d2e151834 290 //Set CS low to start transmission (interrupts conversion)
kylongmu 0:768d2e151834 291 cs = 0;
kylongmu 0:768d2e151834 292 }
kylongmu 0:768d2e151834 293 void mpu9250_spi::deselect() {
kylongmu 0:768d2e151834 294 //Set CS high to stop transmission (restarts conversion)
kylongmu 0:768d2e151834 295 cs = 1;
kylongmu 0:768d2e151834 296 }