Los Putacos / MPU9250_SPI

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of MPU9250_SPI by Mu kylong

MPU9250.cpp@12:2fb5ac400fbd, 2017-12-19 (annotated)

Committer:
Muglug
Date:
Tue Dec 19 10:14:59 2017 +0000
Revision:
12:2fb5ac400fbd
Parent:
11:084e8ba240c1 
Updated to Integer only.;

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