Masahiro Furukawa / Mbed 2 deprecated MPU-9250-MagSens

2Chx3dof Magnetrometer supported

Dependencies:   mbed

MPU9250_SPI/MPU9250.cpp@0:4656a133ed1a, 2021-02-01 (annotated)

Committer:
mfurukawa
Date:
Mon Feb 01 17:11:03 2021 +0000
Revision:
0:4656a133ed1a
initial commit;

Who changed what in which revision?

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