Driver for MPU9250 with SPI .
Embed:
(wiki syntax)
Show/hide line numbers
MPU9205_SPI.cpp
00001 #include "mbed.h" 00002 #include "MPU9250_SPI.h" 00003 #include "MPU9250RegisterMap.h" 00004 #include <cmath> 00005 // MPU9250 with SPI interface library Ver. 0.98 00006 // Made by HeeJae Park 00007 // 2019.05.27 00008 //extern Serial pc; 00009 volatile bool MPU9250_SPI::_dataReady=false; 00010 MPU9250_SPI::MPU9250_SPI(PinName mosi,PinName miso,PinName sclk, PinName cs, PinName intpin) 00011 : _spi(mosi,miso,sclk), _csPin(cs), _intPin(intpin),_mMode(MGN_CONT_MEAS2),_mBits( MGN_16BITS),_srd(SR_100HZ) { 00012 magCalibration.x=0;magCalibration.y=0;magCalibration.z=0; 00013 magBias.x=0; magBias.y=0; magBias.z=0; 00014 magScale.x=1;magScale.y=1;magScale.z=1; 00015 gyroBias.x =0; gyroBias.y =0; gyroBias.z =0; 00016 accelBias.x=0; accelBias.y=0; accelBias.z=0; 00017 magnetic_declination = 8.5; 00018 _csPin=1; 00019 } 00020 void MPU9250_SPI::setup() { 00021 _csPin=1; // setting CS pin high 00022 _spi.format(8,3); // SPI mode 3 00023 _spi.frequency(SPI_HS_CLOCK); // 1Mega 00024 uint8_t m_whoami = 0x00; 00025 uint8_t a_whoami = 0x00; 00026 m_whoami = isConnectedMPU9250(); 00027 if (m_whoami==MPU9250_WHOAMI_DEFAULT_VALUE) { 00028 initMPU9250(); 00029 a_whoami = isConnectedAK8963(); 00030 if (a_whoami == AK8963_WHOAMI_DEFAULT_VALUE){ 00031 initAK8963(); 00032 } 00033 else { 00034 while(1); 00035 } 00036 } 00037 else { 00038 while(1); 00039 } 00040 _intPin.rise(callback(this, &MPU9250_SPI::intService)); 00041 _tmr.start(); 00042 } 00043 void MPU9250_SPI::update(Vect3& _a,Vect3& _g,Vect3& _m) { 00044 if (_dataReady){ // On interrupt, check if data ready interrupt 00045 updateSensors(); 00046 _a=a;_g=g;_m=m; 00047 } 00048 } 00049 00050 uint8_t MPU9250_SPI::isConnectedMPU9250() { 00051 uint8_t c = readByte(WHO_AM_I_MPU9250); 00052 return c; // (c == MPU9250_WHOAMI_DEFAULT_VALUE); 00053 } 00054 uint8_t MPU9250_SPI::isConnectedAK8963() { 00055 uint8_t c = readAK8963Byte(AK8963_WHO_AM_I); 00056 return c; // (c == AK8963_WHOAMI_DEFAULT_VALUE); 00057 } 00058 00059 void MPU9250_SPI::initMPU9250() { 00060 wait_ms(100); 00061 writeByte(PWR_MGMT_1, CLOCK_SEL_PLL); 00062 writeByte(USER_CTRL,I2C_MST_EN); // Master enable 00063 writeByte(I2C_MST_CTRL,I2C_MST_CLK); // I2C master clock =400HZ 00064 replaceBlockAK(AK8963_CNTL,MGN_POWER_DN,0,4); // Power down 00065 writeByte(PWR_MGMT_1, PWR_RESET); // Clear sleep mode bit (6), enable all sensors 00066 wait_ms(100); 00067 writeByte(PWR_MGMT_1, CLOCK_SEL_PLL); 00068 setDlpfBandwidth( DLPF_BANDWIDTH_5HZ); 00069 writeByte(SMPLRT_DIV, SR_100HZ); //{SR_1000HZ=0, SR_200HZ=4, SR_100HZ=9 } 00070 setGyroRange(GYRO_RANGE_2000DPS); 00071 writeByte(PWR_MGMT_2,SEN_ENABLE); 00072 setAccelRange(ACCEL_RANGE_16G);//{ _2G, _4G, _8G, _16G } 00073 setDlpfBandwidth(DLPF_BANDWIDTH_184HZ); // [250HZ, 184HZ, 92HZ, 41HZ, 20HZ, 10HZ, 5HZ] 00074 writeByte(INT_PIN_CFG, 0x20); // LATCH_INT_EN=1, BYPASS_EN=1-->0 (0x22) 00075 writeByte(INT_ENABLE, 0x01); // Enable raw data ready (bit 0) interrupt 00076 writeByte(USER_CTRL,I2C_MST_EN); 00077 wait_ms(100); 00078 writeByte(I2C_MST_CTRL,I2C_MST_CLK); 00079 wait_ms(100); 00080 } 00081 00082 void MPU9250_SPI::initAK8963() { 00083 uint8_t rawData[3]; // x/y/z gyro calibration data stored here 00084 replaceBlockAK(AK8963_CNTL,MGN_POWER_DN,0,4); // Power down magnetometer 00085 wait_ms(50); 00086 replaceBlockAK(AK8963_CNTL,MGN_FUSE_ROM,0,4); 00087 wait_ms(50); 00088 readAK8963Bytes( AK8963_ASAX, 3, rawData); // Read the x-, y-, and z-axis calibration values 00089 magCalibration.x = (float)(rawData[0] - 128)/256.f + 1.f; // Return x-axis sensitivity adjustment values, etc. 00090 magCalibration.y = (float)(rawData[1] - 128)/256.f + 1.f; 00091 magCalibration.z = (float)(rawData[2] - 128)/256.f + 1.f; 00092 replaceBlockAK(AK8963_CNTL,MGN_POWER_DN,0,4); // Power down magnetometer 00093 wait_ms(50); 00094 replaceBlockAK(AK8963_CNTL,((_mBits << 4 )| _mMode),0,5); // Set measurment mode, mMode[0:3] 00095 writeByte(PWR_MGMT_1,CLOCK_SEL_PLL); 00096 wait_ms(50); 00097 mRes=10. * 4912. / 32760.0; // for Magenetometer 16BITS 00098 } 00099 00100 void MPU9250_SPI::setAccelRange(AccelRange range) { 00101 switch(range) { 00102 case ACCEL_RANGE_2G: 00103 aRes = 2.0f/32767.5f; break; 00104 case ACCEL_RANGE_4G: 00105 aRes = 4.0f/32767.5f; break; 00106 case ACCEL_RANGE_8G: 00107 aRes = 8.0f/32767.5f; break; 00108 case ACCEL_RANGE_16G: 00109 aRes = 16.0f/32767.5f; // setting the accel scale to 16G 00110 break; 00111 } 00112 replaceBlock(ACCEL_CONFIG,range,3,2); // addr, value, at, size 00113 _accelRange = range; 00114 } 00115 void MPU9250_SPI::setGyroRange(GyroRange range) { 00116 switch(range) { 00117 case GYRO_RANGE_250DPS: 00118 gRes = 250.0f/32767.5f; break; 00119 case GYRO_RANGE_500DPS: 00120 gRes = 500.0f/32767.5f; break; 00121 case GYRO_RANGE_1000DPS: 00122 gRes = 1000.0f/32767.5f; break; 00123 case GYRO_RANGE_2000DPS: 00124 gRes = 2000.0f/32767.5f ; break; 00125 } 00126 replaceBlock(GYRO_CONFIG,range,3,2); 00127 _gyroRange = range; 00128 } 00129 void MPU9250_SPI::setDlpfBandwidth(DlpfBandwidth bandwidth) { 00130 replaceBlock(ACCEL_CONFIG2,bandwidth,0,4); //Accel DLPF [0:2] 00131 replaceBlock(MPU_CONFIG,bandwidth,0,3); //Gyro DLPF [0:2] 00132 _bandwidth = bandwidth; 00133 } 00134 00135 void MPU9250_SPI::setSampleRate(SampleRate srd){ 00136 writeByte(SMPLRT_DIV, srd); // sampling rate set 00137 _srd = srd; 00138 } 00139 00140 void MPU9250_SPI::enableDataReadyInterrupt() { 00141 writeByte(INT_PIN_CFG,0x00); // setup interrupt, 50 us pulse 00142 writeByte(INT_ENABLE,0x01) ; // set to data ready 00143 } 00144 00145 void MPU9250_SPI::updateSensors(){ 00146 int16_t MPU9250Data[10]; // MPU9250 accel/gyro 에서 16비트 정수로 7개 저장 00147 uint8_t rawData[21]; // 가속도 자이로 원시 데이터 보관 00148 writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR|SPI_READ); // Set the I2C slave addres of AK8963 and set for read. 00149 writeByte(I2C_SLV0_REG,AK8963_XOUT_L); // I2C slave 0 register address from where to begin data transfer 00150 writeByte(I2C_SLV0_CTRL, 0x87); // Read 7 bytes from the magnetometer 00151 readBytes(ACCEL_XOUT_H, 21, rawData); // 16비트 정수로 7개 저장--> 14byte 00152 MPU9250Data[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // signed 16-bit (MSB + LSB) 00153 MPU9250Data[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; 00154 MPU9250Data[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; 00155 MPU9250Data[3] = ((int16_t)rawData[6] << 8) | rawData[7] ; 00156 MPU9250Data[4] = ((int16_t)rawData[8] << 8) | rawData[9] ; 00157 MPU9250Data[5] = ((int16_t)rawData[10] << 8) | rawData[11] ; 00158 MPU9250Data[6] = ((int16_t)rawData[12] << 8) | rawData[13] ; 00159 MPU9250Data[7] = (((int16_t)rawData[15]) << 8) |rawData[14]; 00160 MPU9250Data[8] = (((int16_t)rawData[17]) << 8) |rawData[16]; 00161 MPU9250Data[9] = (((int16_t)rawData[19]) << 8) |rawData[18]; 00162 a.x = (float)MPU9250Data[0] * aRes - accelBias.x; // 가속도 해상도와 바이어스 보정 00163 a.y = (float)MPU9250Data[1] * aRes - accelBias.y; 00164 a.z = (float)MPU9250Data[2] * aRes - accelBias.z; 00165 g.x = (float)MPU9250Data[4] * gRes - gyroBias.x; // 자이로 해상도 보정 00166 g.y = (float)MPU9250Data[5] * gRes - gyroBias.y; // 자이로 바이어스는 칩내부에서 보정함!!! 00167 g.z = (float)MPU9250Data[6] * gRes - gyroBias.z; 00168 m.x = (float)(MPU9250Data[7] * mRes * magCalibration.x - magBias.x) * magScale.x; 00169 m.y = (float)(MPU9250Data[8] * mRes * magCalibration.y - magBias.y) * magScale.y; 00170 m.z = (float)(MPU9250Data[9] * mRes * magCalibration.z - magBias.z) * magScale.z; 00171 } 00172 void MPU9250_SPI::updateAccelGyro() { 00173 int16_t MPU9250Data[7]; // MPU9250 accel/gyro 에서 16비트 정수로 7개 저장 00174 readMPU9250Data(MPU9250Data); // 읽으면 INT 핀 해제 00175 a.x = (float)MPU9250Data[0] * aRes - accelBias.x; // 가속도 해상도와 바이어스 보정 00176 a.y = (float)MPU9250Data[1] * aRes - accelBias.y; 00177 a.z = (float)MPU9250Data[2] * aRes - accelBias.z; 00178 g.x = (float)MPU9250Data[4] * gRes - gyroBias.x; // 자이로 해상도 보정 00179 g.y = (float)MPU9250Data[5] * gRes - gyroBias.y; // 00180 g.z = (float)MPU9250Data[6] * gRes - gyroBias.z; 00181 } 00182 00183 void MPU9250_SPI::readMPU9250Data(int16_t * destination) { 00184 uint8_t rawData[14]; // 가속도 자이로 원시 데이터 보관 00185 readBytes(ACCEL_XOUT_H, 14, rawData); // 16비트 정수로 7개 저장--> 14byte 00186 destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // signed 16-bit (MSB + LSB) 00187 destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; 00188 destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; 00189 destination[3] = ((int16_t)rawData[6] << 8) | rawData[7] ; 00190 destination[4] = ((int16_t)rawData[8] << 8) | rawData[9] ; 00191 destination[5] = ((int16_t)rawData[10] << 8) | rawData[11] ; 00192 destination[6] = ((int16_t)rawData[12] << 8) | rawData[13] ; 00193 } 00194 00195 void MPU9250_SPI::updateMag() { 00196 int16_t magCount[3] = {0, 0, 0}; // 16-bit 지자기 데이터 00197 readMagData(magCount); // 지자기 데이터 읽기 00198 // 지자기 해상도, 검정값, 바이어스 보정, 검정값 (magCalibration[] )은 칩의 ROM에서 00199 m.x = (float)(magCount[0] * mRes * magCalibration.x - magBias.x) * magScale.x; 00200 m.y = (float)(magCount[1] * mRes * magCalibration.y - magBias.y) * magScale.y; 00201 m.z = (float)(magCount[2] * mRes * magCalibration.z - magBias.z) * magScale.z; 00202 } 00203 void MPU9250_SPI::readMagData(int16_t * destination) { 00204 uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition 00205 if(readAK8963Byte(AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set 00206 readAK8963Bytes(AK8963_XOUT_L, 7,rawData); // Read the six raw data and ST2 registers sequentially into data array 00207 uint8_t c = rawData[6]; // End data read by reading ST2 register 00208 if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data 00209 destination[0] = ((int16_t)rawData[1] << 8) | rawData[0]; // Turn the MSB and LSB into a signed 16-bit value 00210 destination[1] = ((int16_t)rawData[3] << 8) | rawData[2]; // Data stored as little Endian 00211 destination[2] = ((int16_t)rawData[5] << 8) | rawData[4]; 00212 } 00213 } 00214 } 00215 00216 void MPU9250_SPI::writeByte(uint8_t subAddress, uint8_t data){ /* write data to device */ 00217 // _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction 00218 // digitalWrite(_csPin,LOW); // select the MPU9250 chip 00219 // _spi->transfer(subAddress); // write the register address 00220 // _spi->transfer(data); // write the data 00221 // digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip 00222 // _spi->endTransaction(); // end the transaction 00223 _spi.frequency(SPI_LS_CLOCK); // setup clock 00224 _csPin=0; // select the MPU9250 chip 00225 _spi.write(subAddress); // write the register address 00226 _spi.write(data); // write the data 00227 _csPin=1; // deselect the MPU9250 chip 00228 } 00229 uint8_t MPU9250_SPI::readByte(uint8_t subAddress){ 00230 // _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3)); 00231 // digitalWrite(_csPin,LOW); // select the MPU9250 chip 00232 // _spi->transfer(subAddress | SPI_READ); // specify the starting register address 00233 // uint8_t data = _spi->transfer(0x00); // read the data 00234 // digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip 00235 // _spi->endTransaction(); // end the transaction 00236 00237 _spi.frequency(SPI_LS_CLOCK); // setup clock 00238 _csPin=0; // select the MPU9250 chip 00239 _spi.write(subAddress| SPI_READ); // use READ MASK 00240 uint8_t data =_spi.write(0); // write any to get data 00241 _csPin=1; // deselect the MPU9250 chip 00242 return data; 00243 } 00244 00245 void MPU9250_SPI::readBytes(uint8_t subAddress, uint8_t cnt, uint8_t* dest){ 00246 // _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3)); 00247 // digitalWrite(_csPin,LOW); // select the MPU9250 chip 00248 // _spi->transfer(subAddress | SPI_READ); // specify the starting register address 00249 // for(uint8_t i = 0; i < count; i++){ 00250 // dest[i] = _spi->transfer(0x00); // read the data 00251 // } 00252 // digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip 00253 // _spi->endTransaction(); // end the transaction 00254 _spi.frequency(SPI_HS_CLOCK); // setup clock 00255 _csPin=0; // select the MPU9250 chip 00256 _spi.write(subAddress | SPI_READ); // specify the starting register address 00257 for(uint8_t i = 0; i < cnt; i++){ 00258 dest[i] = _spi.write(0x00); // read the data 00259 } 00260 _csPin=1; // deselect the MPU9250 chip 00261 } 00262 00263 void MPU9250_SPI::writeAK8963Byte(uint8_t subAddress, uint8_t data){ 00264 writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR) ; // set slave 0 to the AK8963 and set for write 00265 writeByte(I2C_SLV0_REG,subAddress) ; // set the register to the desired AK8963 sub address 00266 writeByte(I2C_SLV0_DO,data) ; // store the data for write 00267 writeByte(I2C_SLV0_CTRL,I2C_SLV0_EN | (uint8_t)1); // enable I2C and send 1 byte 00268 } 00269 00270 void MPU9250_SPI::readAK8963Bytes(uint8_t subAddress, uint8_t count, uint8_t* dest){ 00271 writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG) ; // set slave 0 to the AK8963 and set for read 00272 writeByte(I2C_SLV0_REG,subAddress) ; // set the register to the desired AK8963 sub address 00273 writeByte(I2C_SLV0_CTRL,I2C_SLV0_EN | count); // enable I2C and request the bytes 00274 wait_ms(1); // takes some time for these registers to fill 00275 readBytes(EXT_SENS_DATA_00,count,dest); // read the bytes off the MPU9250 EXT_SENS_DATA registers 00276 } 00277 00278 uint8_t MPU9250_SPI::readAK8963Byte(uint8_t subAddress){ 00279 writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG) ; // set slave 0 to the AK8963 and set for read 00280 writeByte(I2C_SLV0_REG,subAddress) ; // set the register to the desired AK8963 sub address 00281 writeByte(I2C_SLV0_CTRL,I2C_SLV0_EN | (uint8_t)1); // enable I2C and request the bytes 00282 wait_ms(11); // takes some time for these registers to fill 00283 return readByte(EXT_SENS_DATA_00); // read the bytes off the MPU9250 EXT_SENS_DATA registers 00284 } 00285 void MPU9250_SPI::replaceBlock(uint8_t address, uint8_t block, uint8_t at, uint8_t sz){ 00286 uint8_t data=readByte(address); 00287 data &= ~(((1<<sz)-1)<<at); 00288 data |= block<<at; 00289 writeByte(address, data ); 00290 } 00291 void MPU9250_SPI::replaceBlockAK(uint8_t address, uint8_t block, uint8_t at, uint8_t sz){ 00292 uint8_t data=readByte(address); 00293 data &= ~(((1<<sz)-1)<<at); 00294 data |= block<<at; 00295 writeAK8963Byte(address, data ); 00296 } 00297
Generated on Wed Aug 3 2022 10:23:10 by
1.7.2