R L / MPU9250_SPI

mpu

Dependents:   TestRig

Fork of MPU9250_SPI by Mu kylong

MPU9250.cpp@13:63853084a545, 2016-02-21 (annotated)

Committer:
rllamado
Date:
Sun Feb 21 07:26:46 2016 +0000
Revision:
13:63853084a545
Parent:
12:620be2376124 
something

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kylongmu 0:768d2e151834 1 /*CODED by Qiyong Mu on 21/06/2014
kylongmu 7:1bcd61cdbf18 2 kylongmu@msn.com
kylongmu 0:768d2e151834 3 */
kylongmu 0:768d2e151834 4
rllamado 13:63853084a545 5 //#include <mbed.h>
kylongmu 1:f738165e54f0 6 #include "MPU9250.h"
kylongmu 0:768d2e151834 7
rllamado 13:63853084a545 8 mpu9250_spi::mpu9250_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs)
rllamado 13:63853084a545 9 {
rllamado 13:63853084a545 10 cs = 1;
rllamado 13:63853084a545 11 }
kylongmu 0:768d2e151834 12
kylongmu 0:768d2e151834 13 unsigned int mpu9250_spi::WriteReg( uint8_t WriteAddr, uint8_t WriteData )
kylongmu 0:768d2e151834 14 {
kylongmu 0:768d2e151834 15 unsigned int temp_val;
kylongmu 0:768d2e151834 16 select();
kylongmu 0:768d2e151834 17 spi.write(WriteAddr);
kylongmu 0:768d2e151834 18 temp_val=spi.write(WriteData);
kylongmu 0:768d2e151834 19 deselect();
kylongmu 0:768d2e151834 20 wait_us(50);
kylongmu 0:768d2e151834 21 return temp_val;
kylongmu 0:768d2e151834 22 }
kylongmu 2:f274ea3bced9 23 unsigned int mpu9250_spi::ReadReg( uint8_t WriteAddr, uint8_t WriteData )
kylongmu 2:f274ea3bced9 24 {
kylongmu 2:f274ea3bced9 25 return WriteReg(WriteAddr | READ_FLAG,WriteData);
kylongmu 2:f274ea3bced9 26 }
kylongmu 2:f274ea3bced9 27 void mpu9250_spi::ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes )
kylongmu 0:768d2e151834 28 {
kylongmu 0:768d2e151834 29 unsigned int i = 0;
kylongmu 0:768d2e151834 30
kylongmu 0:768d2e151834 31 select();
kylongmu 0:768d2e151834 32 spi.write(ReadAddr | READ_FLAG);
kylongmu 0:768d2e151834 33 for(i=0; i<Bytes; i++)
kylongmu 0:768d2e151834 34 ReadBuf[i] = spi.write(0x00);
kylongmu 0:768d2e151834 35 deselect();
kylongmu 0:768d2e151834 36 wait_us(50);
kylongmu 0:768d2e151834 37 }
kylongmu 0:768d2e151834 38
kylongmu 0:768d2e151834 39 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 40 INITIALIZATION
kylongmu 0:768d2e151834 41 usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and
kylongmu 0:768d2e151834 42 low pass filter value; suitable values are:
kylongmu 0:768d2e151834 43 BITS_DLPF_CFG_256HZ_NOLPF2
kylongmu 0:768d2e151834 44 BITS_DLPF_CFG_188HZ
kylongmu 0:768d2e151834 45 BITS_DLPF_CFG_98HZ
kylongmu 0:768d2e151834 46 BITS_DLPF_CFG_42HZ
kylongmu 0:768d2e151834 47 BITS_DLPF_CFG_20HZ
kylongmu 0:768d2e151834 48 BITS_DLPF_CFG_10HZ
kylongmu 0:768d2e151834 49 BITS_DLPF_CFG_5HZ
kylongmu 0:768d2e151834 50 BITS_DLPF_CFG_2100HZ_NOLPF
kylongmu 0:768d2e151834 51 returns 1 if an error occurred
kylongmu 0:768d2e151834 52 -----------------------------------------------------------------------------------------------*/
kylongmu 5:f15d1d9d1561 53 #define MPU_InitRegNum 17
kylongmu 0:768d2e151834 54
kylongmu 0:768d2e151834 55 bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter){
kylongmu 0:768d2e151834 56 uint8_t i = 0;
kylongmu 0:768d2e151834 57 uint8_t MPU_Init_Data[MPU_InitRegNum][2] = {
kylongmu 0:768d2e151834 58 {0x80, MPUREG_PWR_MGMT_1}, // Reset Device
kylongmu 0:768d2e151834 59 {0x01, MPUREG_PWR_MGMT_1}, // Clock Source
kylongmu 0:768d2e151834 60 {0x00, MPUREG_PWR_MGMT_2}, // Enable Acc & Gyro
kylongmu 11:084e8ba240c1 61 {low_pass_filter, MPUREG_CONFIG}, // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz
kylongmu 0:768d2e151834 62 {0x18, MPUREG_GYRO_CONFIG}, // +-2000dps
kylongmu 0:768d2e151834 63 {0x08, MPUREG_ACCEL_CONFIG}, // +-4G
kylongmu 9:a8733542d0fb 64 {0x09, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz
kylongmu 0:768d2e151834 65 {0x30, MPUREG_INT_PIN_CFG}, //
kylongmu 4:79185409730f 66 //{0x40, MPUREG_I2C_MST_CTRL}, // I2C Speed 348 kHz
kylongmu 4:79185409730f 67 //{0x20, MPUREG_USER_CTRL}, // Enable AUX
kylongmu 5:f15d1d9d1561 68 {0x20, MPUREG_USER_CTRL}, // I2C Master mode
kylongmu 5:f15d1d9d1561 69 {0x0D, MPUREG_I2C_MST_CTRL}, // I2C configuration multi-master IIC 400KHz
kylongmu 5:f15d1d9d1561 70
kylongmu 5:f15d1d9d1561 71 {AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR}, //Set the I2C slave addres of AK8963 and set for write.
kylongmu 4:79185409730f 72 //{0x09, MPUREG_I2C_SLV4_CTRL},
kylongmu 5:f15d1d9d1561 73 //{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay
kylongmu 11:084e8ba240c1 74
kylongmu 5:f15d1d9d1561 75 {AK8963_CNTL2, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
kylongmu 5:f15d1d9d1561 76 {0x01, MPUREG_I2C_SLV0_DO}, // Reset AK8963
kylongmu 5:f15d1d9d1561 77 {0x81, MPUREG_I2C_SLV0_CTRL}, //Enable I2C and set 1 byte
kylongmu 11:084e8ba240c1 78
kylongmu 5:f15d1d9d1561 79 {AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
kylongmu 6:c82c48348f8c 80 {0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit
kylongmu 5:f15d1d9d1561 81 {0x81, MPUREG_I2C_SLV0_CTRL} //Enable I2C and set 1 byte
kylongmu 5:f15d1d9d1561 82
kylongmu 0:768d2e151834 83 };
kylongmu 0:768d2e151834 84 spi.format(8,0);
kylongmu 0:768d2e151834 85 spi.frequency(1000000);
kylongmu 0:768d2e151834 86
kylongmu 0:768d2e151834 87 for(i=0; i<MPU_InitRegNum; i++) {
kylongmu 0:768d2e151834 88 WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
kylongmu 5:f15d1d9d1561 89 wait(0.001); //I2C must slow down the write speed, otherwise it won't work
kylongmu 0:768d2e151834 90 }
kylongmu 11:084e8ba240c1 91
kylongmu 11:084e8ba240c1 92 set_acc_scale(2);
kylongmu 11:084e8ba240c1 93 set_gyro_scale(250);
kylongmu 4:79185409730f 94
kylongmu 11:084e8ba240c1 95 //AK8963_calib_Magnetometer(); //Can't load this function here , strange problem?
kylongmu 0:768d2e151834 96 return 0;
kylongmu 0:768d2e151834 97 }
kylongmu 0:768d2e151834 98 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 99 ACCELEROMETER SCALE
kylongmu 0:768d2e151834 100 usage: call this function at startup, after initialization, to set the right range for the
kylongmu 0:768d2e151834 101 accelerometers. Suitable ranges are:
kylongmu 0:768d2e151834 102 BITS_FS_2G
kylongmu 0:768d2e151834 103 BITS_FS_4G
kylongmu 0:768d2e151834 104 BITS_FS_8G
kylongmu 0:768d2e151834 105 BITS_FS_16G
kylongmu 0:768d2e151834 106 returns the range set (2,4,8 or 16)
kylongmu 0:768d2e151834 107 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 108 unsigned int mpu9250_spi::set_acc_scale(int scale){
kylongmu 0:768d2e151834 109 unsigned int temp_scale;
kylongmu 0:768d2e151834 110 WriteReg(MPUREG_ACCEL_CONFIG, scale);
kylongmu 0:768d2e151834 111
kylongmu 0:768d2e151834 112 switch (scale){
kylongmu 0:768d2e151834 113 case BITS_FS_2G:
kylongmu 0:768d2e151834 114 acc_divider=16384;
kylongmu 0:768d2e151834 115 break;
kylongmu 0:768d2e151834 116 case BITS_FS_4G:
kylongmu 0:768d2e151834 117 acc_divider=8192;
kylongmu 0:768d2e151834 118 break;
kylongmu 0:768d2e151834 119 case BITS_FS_8G:
kylongmu 0:768d2e151834 120 acc_divider=4096;
kylongmu 0:768d2e151834 121 break;
kylongmu 0:768d2e151834 122 case BITS_FS_16G:
kylongmu 0:768d2e151834 123 acc_divider=2048;
kylongmu 0:768d2e151834 124 break;
kylongmu 0:768d2e151834 125 }
kylongmu 0:768d2e151834 126 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
kylongmu 0:768d2e151834 127
kylongmu 0:768d2e151834 128 switch (temp_scale){
kylongmu 0:768d2e151834 129 case BITS_FS_2G:
kylongmu 0:768d2e151834 130 temp_scale=2;
kylongmu 0:768d2e151834 131 break;
kylongmu 0:768d2e151834 132 case BITS_FS_4G:
kylongmu 0:768d2e151834 133 temp_scale=4;
kylongmu 0:768d2e151834 134 break;
kylongmu 0:768d2e151834 135 case BITS_FS_8G:
kylongmu 0:768d2e151834 136 temp_scale=8;
kylongmu 0:768d2e151834 137 break;
kylongmu 0:768d2e151834 138 case BITS_FS_16G:
kylongmu 0:768d2e151834 139 temp_scale=16;
kylongmu 0:768d2e151834 140 break;
kylongmu 0:768d2e151834 141 }
kylongmu 0:768d2e151834 142 return temp_scale;
kylongmu 0:768d2e151834 143 }
kylongmu 0:768d2e151834 144
kylongmu 0:768d2e151834 145
kylongmu 0:768d2e151834 146 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 147 GYROSCOPE SCALE
kylongmu 0:768d2e151834 148 usage: call this function at startup, after initialization, to set the right range for the
kylongmu 0:768d2e151834 149 gyroscopes. Suitable ranges are:
kylongmu 0:768d2e151834 150 BITS_FS_250DPS
kylongmu 0:768d2e151834 151 BITS_FS_500DPS
kylongmu 0:768d2e151834 152 BITS_FS_1000DPS
kylongmu 0:768d2e151834 153 BITS_FS_2000DPS
kylongmu 0:768d2e151834 154 returns the range set (250,500,1000 or 2000)
kylongmu 0:768d2e151834 155 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 156 unsigned int mpu9250_spi::set_gyro_scale(int scale){
kylongmu 0:768d2e151834 157 unsigned int temp_scale;
kylongmu 0:768d2e151834 158 WriteReg(MPUREG_GYRO_CONFIG, scale);
kylongmu 0:768d2e151834 159 switch (scale){
kylongmu 0:768d2e151834 160 case BITS_FS_250DPS:
kylongmu 0:768d2e151834 161 gyro_divider=131;
kylongmu 0:768d2e151834 162 break;
kylongmu 0:768d2e151834 163 case BITS_FS_500DPS:
kylongmu 0:768d2e151834 164 gyro_divider=65.5;
kylongmu 0:768d2e151834 165 break;
kylongmu 0:768d2e151834 166 case BITS_FS_1000DPS:
kylongmu 0:768d2e151834 167 gyro_divider=32.8;
kylongmu 0:768d2e151834 168 break;
kylongmu 0:768d2e151834 169 case BITS_FS_2000DPS:
kylongmu 0:768d2e151834 170 gyro_divider=16.4;
kylongmu 0:768d2e151834 171 break;
kylongmu 0:768d2e151834 172 }
kylongmu 0:768d2e151834 173 temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00);
kylongmu 0:768d2e151834 174 switch (temp_scale){
kylongmu 0:768d2e151834 175 case BITS_FS_250DPS:
kylongmu 0:768d2e151834 176 temp_scale=250;
kylongmu 0:768d2e151834 177 break;
kylongmu 0:768d2e151834 178 case BITS_FS_500DPS:
kylongmu 0:768d2e151834 179 temp_scale=500;
kylongmu 0:768d2e151834 180 break;
kylongmu 0:768d2e151834 181 case BITS_FS_1000DPS:
kylongmu 0:768d2e151834 182 temp_scale=1000;
kylongmu 0:768d2e151834 183 break;
kylongmu 0:768d2e151834 184 case BITS_FS_2000DPS:
kylongmu 0:768d2e151834 185 temp_scale=2000;
kylongmu 0:768d2e151834 186 break;
kylongmu 0:768d2e151834 187 }
kylongmu 0:768d2e151834 188 return temp_scale;
kylongmu 0:768d2e151834 189 }
kylongmu 0:768d2e151834 190
kylongmu 0:768d2e151834 191
kylongmu 0:768d2e151834 192 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 193 WHO AM I?
kylongmu 0:768d2e151834 194 usage: call this function to know if SPI is working correctly. It checks the I2C address of the
kylongmu 0:768d2e151834 195 mpu9250 which should be 104 when in SPI mode.
kylongmu 0:768d2e151834 196 returns the I2C address (104)
kylongmu 0:768d2e151834 197 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 198 unsigned int mpu9250_spi::whoami(){
kylongmu 0:768d2e151834 199 unsigned int response;
kylongmu 0:768d2e151834 200 response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00);
kylongmu 0:768d2e151834 201 return response;
kylongmu 0:768d2e151834 202 }
kylongmu 0:768d2e151834 203
kylongmu 0:768d2e151834 204
kylongmu 0:768d2e151834 205 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 206 READ ACCELEROMETER
kylongmu 0:768d2e151834 207 usage: call this function to read accelerometer data. Axis represents selected axis:
kylongmu 0:768d2e151834 208 0 -> X axis
kylongmu 0:768d2e151834 209 1 -> Y axis
kylongmu 0:768d2e151834 210 2 -> Z axis
kylongmu 0:768d2e151834 211 -----------------------------------------------------------------------------------------------*/
kylongmu 2:f274ea3bced9 212 void mpu9250_spi::read_acc()
kylongmu 2:f274ea3bced9 213 {
kylongmu 4:79185409730f 214 uint8_t response[6];
kylongmu 0:768d2e151834 215 int16_t bit_data;
rllamado 12:620be2376124 216 /*float*/int16_t data;
kylongmu 4:79185409730f 217 int i;
kylongmu 4:79185409730f 218 ReadRegs(MPUREG_ACCEL_XOUT_H,response,6);
kylongmu 4:79185409730f 219 for(i=0; i<3; i++) {
kylongmu 4:79185409730f 220 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
rllamado 12:620be2376124 221 data=/*(float)*/bit_data;
rllamado 12:620be2376124 222 accelerometer_data[i]=data/*/acc_divider*/;
kylongmu 4:79185409730f 223 }
kylongmu 4:79185409730f 224
kylongmu 0:768d2e151834 225 }
kylongmu 0:768d2e151834 226
kylongmu 0:768d2e151834 227 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 228 READ GYROSCOPE
kylongmu 0:768d2e151834 229 usage: call this function to read gyroscope data. Axis represents selected axis:
kylongmu 0:768d2e151834 230 0 -> X axis
kylongmu 0:768d2e151834 231 1 -> Y axis
kylongmu 0:768d2e151834 232 2 -> Z axis
kylongmu 0:768d2e151834 233 -----------------------------------------------------------------------------------------------*/
kylongmu 2:f274ea3bced9 234 void mpu9250_spi::read_rot()
kylongmu 2:f274ea3bced9 235 {
kylongmu 4:79185409730f 236 uint8_t response[6];
kylongmu 0:768d2e151834 237 int16_t bit_data;
rllamado 12:620be2376124 238 /*float*/int16_t data;
kylongmu 4:79185409730f 239 int i;
kylongmu 4:79185409730f 240 ReadRegs(MPUREG_GYRO_XOUT_H,response,6);
kylongmu 4:79185409730f 241 for(i=0; i<3; i++) {
kylongmu 4:79185409730f 242 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
rllamado 12:620be2376124 243 data=/*(float)*/bit_data;
rllamado 12:620be2376124 244 gyroscope_data[i]=data/*/gyro_divider*/;
kylongmu 4:79185409730f 245 }
kylongmu 2:f274ea3bced9 246
kylongmu 0:768d2e151834 247 }
kylongmu 0:768d2e151834 248
kylongmu 0:768d2e151834 249 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 250 READ TEMPERATURE
kylongmu 0:768d2e151834 251 usage: call this function to read temperature data.
kylongmu 0:768d2e151834 252 returns the value in °C
kylongmu 0:768d2e151834 253 -----------------------------------------------------------------------------------------------*/
kylongmu 3:f4fa24cc247d 254 void mpu9250_spi::read_temp(){
kylongmu 2:f274ea3bced9 255 uint8_t response[2];
kylongmu 0:768d2e151834 256 int16_t bit_data;
rllamado 12:620be2376124 257 /*float*/int16_t data;
kylongmu 2:f274ea3bced9 258 ReadRegs(MPUREG_TEMP_OUT_H,response,2);
kylongmu 2:f274ea3bced9 259
kylongmu 2:f274ea3bced9 260 bit_data=((int16_t)response[0]<<8)|response[1];
rllamado 12:620be2376124 261 data=/*(float)*/bit_data;
kylongmu 3:f4fa24cc247d 262 Temperature=(data/340)+36.53;
kylongmu 0:768d2e151834 263 deselect();
kylongmu 0:768d2e151834 264 }
kylongmu 0:768d2e151834 265
kylongmu 0:768d2e151834 266 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 267 READ ACCELEROMETER CALIBRATION
kylongmu 0:768d2e151834 268 usage: call this function to read accelerometer data. Axis represents selected axis:
kylongmu 0:768d2e151834 269 0 -> X axis
kylongmu 0:768d2e151834 270 1 -> Y axis
kylongmu 0:768d2e151834 271 2 -> Z axis
kylongmu 0:768d2e151834 272 returns Factory Trim value
kylongmu 0:768d2e151834 273 -----------------------------------------------------------------------------------------------*/
kylongmu 2:f274ea3bced9 274 void mpu9250_spi::calib_acc()
kylongmu 2:f274ea3bced9 275 {
kylongmu 2:f274ea3bced9 276 uint8_t response[4];
kylongmu 0:768d2e151834 277 int temp_scale;
kylongmu 0:768d2e151834 278 //READ CURRENT ACC SCALE
kylongmu 0:768d2e151834 279 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
kylongmu 0:768d2e151834 280 set_acc_scale(BITS_FS_8G);
kylongmu 2:f274ea3bced9 281 //ENABLE SELF TEST need modify
kylongmu 2:f274ea3bced9 282 //temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis);
kylongmu 0:768d2e151834 283
kylongmu 2:f274ea3bced9 284 ReadRegs(MPUREG_SELF_TEST_X,response,4);
kylongmu 2:f274ea3bced9 285 calib_data[0]=((response[0]&11100000)>>3)|((response[3]&00110000)>>4);
kylongmu 2:f274ea3bced9 286 calib_data[1]=((response[1]&11100000)>>3)|((response[3]&00001100)>>2);
kylongmu 2:f274ea3bced9 287 calib_data[2]=((response[2]&11100000)>>3)|((response[3]&00000011));
kylongmu 2:f274ea3bced9 288
kylongmu 0:768d2e151834 289 set_acc_scale(temp_scale);
kylongmu 2:f274ea3bced9 290 }
kylongmu 6:c82c48348f8c 291 uint8_t mpu9250_spi::AK8963_whoami(){
kylongmu 11:084e8ba240c1 292 uint8_t response;
kylongmu 5:f15d1d9d1561 293 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
kylongmu 4:79185409730f 294 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer
kylongmu 11:084e8ba240c1 295 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Read 1 byte from the magnetometer
kylongmu 11:084e8ba240c1 296
kylongmu 11:084e8ba240c1 297 //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes
kylongmu 11:084e8ba240c1 298 wait(0.001);
kylongmu 11:084e8ba240c1 299 response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00); //Read I2C
kylongmu 11:084e8ba240c1 300 //ReadRegs(MPUREG_EXT_SENS_DATA_00,response,1);
kylongmu 11:084e8ba240c1 301 //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
kylongmu 11:084e8ba240c1 302
kylongmu 11:084e8ba240c1 303 return response;
kylongmu 11:084e8ba240c1 304 }
kylongmu 11:084e8ba240c1 305 void mpu9250_spi::AK8963_calib_Magnetometer(){
kylongmu 11:084e8ba240c1 306 uint8_t response[3];
rllamado 12:620be2376124 307 /*float*/int16_t data;
kylongmu 11:084e8ba240c1 308 int i;
kylongmu 11:084e8ba240c1 309
kylongmu 11:084e8ba240c1 310 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
kylongmu 11:084e8ba240c1 311 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_ASAX); //I2C slave 0 register address from where to begin data transfer
kylongmu 11:084e8ba240c1 312 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer
kylongmu 5:f15d1d9d1561 313
kylongmu 5:f15d1d9d1561 314 //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes
kylongmu 6:c82c48348f8c 315 wait(0.001);
kylongmu 6:c82c48348f8c 316 //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00); //Read I2C
kylongmu 11:084e8ba240c1 317 ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3);
kylongmu 11:084e8ba240c1 318
kylongmu 5:f15d1d9d1561 319 //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
kylongmu 11:084e8ba240c1 320 for(i=0; i<3; i++) {
kylongmu 11:084e8ba240c1 321 data=response[i];
kylongmu 11:084e8ba240c1 322 Magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor;
kylongmu 11:084e8ba240c1 323 }
kylongmu 4:79185409730f 324 }
kylongmu 4:79185409730f 325 void mpu9250_spi::AK8963_read_Magnetometer(){
kylongmu 5:f15d1d9d1561 326 uint8_t response[7];
kylongmu 4:79185409730f 327 int16_t bit_data;
rllamado 12:620be2376124 328 /*float*/int16_t data;
kylongmu 5:f15d1d9d1561 329 int i;
kylongmu 5:f15d1d9d1561 330
kylongmu 5:f15d1d9d1561 331 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
kylongmu 4:79185409730f 332 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
kylongmu 5:f15d1d9d1561 333 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 6 bytes from the magnetometer
kylongmu 4:79185409730f 334
kylongmu 6:c82c48348f8c 335 wait(0.001);
kylongmu 5:f15d1d9d1561 336 ReadRegs(MPUREG_EXT_SENS_DATA_00,response,7);
kylongmu 5:f15d1d9d1561 337 //must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.
kylongmu 5:f15d1d9d1561 338 for(i=0; i<3; i++) {
kylongmu 10:d27b3298e9e0 339 bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
rllamado 12:620be2376124 340 data=/*(float)*/bit_data;
rllamado 12:620be2376124 341 Magnetometer[i]=data/**Magnetometer_ASA[i]*/;
kylongmu 5:f15d1d9d1561 342 }
kylongmu 4:79185409730f 343 }
kylongmu 8:492028cb604e 344 void mpu9250_spi::read_all(){
kylongmu 8:492028cb604e 345 uint8_t response[21];
kylongmu 8:492028cb604e 346 int16_t bit_data;
rllamado 12:620be2376124 347 /*float*/int16_t data;
kylongmu 8:492028cb604e 348 int i;
kylongmu 8:492028cb604e 349
kylongmu 8:492028cb604e 350 //Send I2C command at first
kylongmu 8:492028cb604e 351 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
kylongmu 8:492028cb604e 352 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
kylongmu 8:492028cb604e 353 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 7 bytes from the magnetometer
kylongmu 8:492028cb604e 354 //must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.
kylongmu 8:492028cb604e 355
kylongmu 8:492028cb604e 356 //wait(0.001);
kylongmu 8:492028cb604e 357 ReadRegs(MPUREG_ACCEL_XOUT_H,response,21);
kylongmu 8:492028cb604e 358 //Get accelerometer value
kylongmu 8:492028cb604e 359 for(i=0; i<3; i++) {
kylongmu 8:492028cb604e 360 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
rllamado 12:620be2376124 361 data=/*(float)*/bit_data;
rllamado 12:620be2376124 362 accelerometer_data[i]=data/*/acc_divider*/;
kylongmu 8:492028cb604e 363 }
kylongmu 8:492028cb604e 364 //Get temperature
kylongmu 8:492028cb604e 365 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
rllamado 12:620be2376124 366 data=/*(float)*/bit_data;
kylongmu 9:a8733542d0fb 367 Temperature=((data-21)/333.87)+21;
rllamado 12:620be2376124 368
rllamado 12:620be2376124 369 //Get gyroscope value
kylongmu 8:492028cb604e 370 for(i=4; i<7; i++) {
kylongmu 8:492028cb604e 371 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
rllamado 12:620be2376124 372 data=/*(float)*/bit_data;
rllamado 12:620be2376124 373 gyroscope_data[i-4]=data/*/gyro_divider*/;
kylongmu 8:492028cb604e 374 }
kylongmu 8:492028cb604e 375 //Get Magnetometer value
kylongmu 8:492028cb604e 376 for(i=7; i<10; i++) {
kylongmu 10:d27b3298e9e0 377 bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
rllamado 12:620be2376124 378 data=/*(float)*/bit_data;
rllamado 12:620be2376124 379 Magnetometer[i-7]=data/**Magnetometer_ASA[i-7]*/;
kylongmu 8:492028cb604e 380 }
kylongmu 8:492028cb604e 381 }
kylongmu 0:768d2e151834 382
kylongmu 0:768d2e151834 383 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 384 SPI SELECT AND DESELECT
kylongmu 0:768d2e151834 385 usage: enable and disable mpu9250 communication bus
kylongmu 0:768d2e151834 386 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 387 void mpu9250_spi::select() {
kylongmu 0:768d2e151834 388 //Set CS low to start transmission (interrupts conversion)
kylongmu 0:768d2e151834 389 cs = 0;
kylongmu 0:768d2e151834 390 }
kylongmu 0:768d2e151834 391 void mpu9250_spi::deselect() {
kylongmu 0:768d2e151834 392 //Set CS high to stop transmission (restarts conversion)
kylongmu 0:768d2e151834 393 cs = 1;
kylongmu 0:768d2e151834 394 }