SAKURA Internet / MPU9250_SPI

forked

Fork of MPU9250_SPI by Mu kylong

MPU9250.cpp@12:1056a7f87bd0, 2017-11-29 (annotated)

Committer:
misodengaku
Date:
Wed Nov 29 06:29:21 2017 +0000
Revision:
12:1056a7f87bd0
Parent:
11:084e8ba240c1 
cast

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