Atsumi Toda / MPU9250_SPI

I modified continuos measurement rate from 8Hz to 100Hz

MPU9250.cpp@12:a5033fbe71a1, 2021-11-16 (annotated)

Committer:
Joeatsumi
Date:
Tue Nov 16 04:02:25 2021 +0000
Revision:
12:a5033fbe71a1
Parent:
11:084e8ba240c1 
I modified continuos measurement rate from 8Hz to 100Hz

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