주 형진
ai_car
Revision 12:f3816a0f498e, committed 2021-05-03
- Comitter:
- wngudwls000
- Date:
- Mon May 03 07:20:25 2021 +0000
- Parent:
- 11:084e8ba240c1
- Commit message:
- adas
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9205_SPI.cpp Mon May 03 07:20:25 2021 +0000
@@ -0,0 +1,297 @@
+#include "mbed.h"
+#include "MPU9250_SPI.h"
+#include "MPU9250RegisterMap.h"
+#include <cmath>
+// MPU9250 with SPI interface library Ver. 0.98
+// Made by HeeJae Park
+// 2019.05.27
+//extern Serial pc;
+volatile bool MPU9250_SPI::_dataReady=false;
+MPU9250_SPI::MPU9250_SPI(PinName mosi,PinName miso,PinName sclk, PinName cs, PinName intpin)
+: _spi(mosi,miso,sclk), _csPin(cs), _intPin(intpin),_mMode(MGN_CONT_MEAS2),_mBits( MGN_16BITS),_srd(SR_100HZ) {
+ magCalibration.x=0;magCalibration.y=0;magCalibration.z=0;
+ magBias.x=0; magBias.y=0; magBias.z=0;
+ magScale.x=1;magScale.y=1;magScale.z=1;
+ gyroBias.x =0; gyroBias.y =0; gyroBias.z =0;
+ accelBias.x=0; accelBias.y=0; accelBias.z=0;
+ magnetic_declination = 8.5;
+ _csPin=1;
+}
+void MPU9250_SPI::setup() {
+ _csPin=1; // setting CS pin high
+ _spi.format(8,3); // SPI mode 3
+ _spi.frequency(SPI_HS_CLOCK); // 1Mega
+ uint8_t m_whoami = 0x00;
+ uint8_t a_whoami = 0x00;
+ m_whoami = isConnectedMPU9250();
+ if (m_whoami==MPU9250_WHOAMI_DEFAULT_VALUE) {
+ initMPU9250();
+ a_whoami = isConnectedAK8963();
+ if (a_whoami == AK8963_WHOAMI_DEFAULT_VALUE){
+ initAK8963();
+ }
+ else {
+ while(1);
+ }
+ }
+ else {
+ while(1);
+ }
+ _intPin.rise(callback(this, &MPU9250_SPI::intService));
+ _tmr.start();
+}
+void MPU9250_SPI::update(Vect3& _a,Vect3& _g,Vect3& _m) {
+ if (_dataReady){ // On interrupt, check if data ready interrupt
+ updateSensors();
+ _a=a;_g=g;_m=m;
+ }
+}
+
+uint8_t MPU9250_SPI::isConnectedMPU9250() {
+ uint8_t c = readByte(WHO_AM_I_MPU9250);
+ return c; // (c == MPU9250_WHOAMI_DEFAULT_VALUE);
+}
+uint8_t MPU9250_SPI::isConnectedAK8963() {
+ uint8_t c = readAK8963Byte(AK8963_WHO_AM_I);
+ return c; // (c == AK8963_WHOAMI_DEFAULT_VALUE);
+}
+
+void MPU9250_SPI::initMPU9250() {
+ wait_ms(100);
+ writeByte(PWR_MGMT_1, CLOCK_SEL_PLL);
+ writeByte(USER_CTRL,I2C_MST_EN); // Master enable
+ writeByte(I2C_MST_CTRL,I2C_MST_CLK); // I2C master clock =400HZ
+ replaceBlockAK(AK8963_CNTL,MGN_POWER_DN,0,4); // Power down
+ writeByte(PWR_MGMT_1, PWR_RESET); // Clear sleep mode bit (6), enable all sensors
+ wait_ms(100);
+ writeByte(PWR_MGMT_1, CLOCK_SEL_PLL);
+ setDlpfBandwidth( DLPF_BANDWIDTH_5HZ);
+ writeByte(SMPLRT_DIV, SR_100HZ); //{SR_1000HZ=0, SR_200HZ=4, SR_100HZ=9 }
+ setGyroRange(GYRO_RANGE_2000DPS);
+ writeByte(PWR_MGMT_2,SEN_ENABLE);
+ setAccelRange(ACCEL_RANGE_16G);//{ _2G, _4G, _8G, _16G }
+ setDlpfBandwidth(DLPF_BANDWIDTH_184HZ); // [250HZ, 184HZ, 92HZ, 41HZ, 20HZ, 10HZ, 5HZ]
+ writeByte(INT_PIN_CFG, 0x20); // LATCH_INT_EN=1, BYPASS_EN=1-->0 (0x22)
+ writeByte(INT_ENABLE, 0x01); // Enable raw data ready (bit 0) interrupt
+ writeByte(USER_CTRL,I2C_MST_EN);
+ wait_ms(100);
+ writeByte(I2C_MST_CTRL,I2C_MST_CLK);
+ wait_ms(100);
+}
+
+void MPU9250_SPI::initAK8963() {
+ uint8_t rawData[3]; // x/y/z gyro calibration data stored here
+ replaceBlockAK(AK8963_CNTL,MGN_POWER_DN,0,4); // Power down magnetometer
+ wait_ms(50);
+ replaceBlockAK(AK8963_CNTL,MGN_FUSE_ROM,0,4);
+ wait_ms(50);
+ readAK8963Bytes( AK8963_ASAX, 3, rawData); // Read the x-, y-, and z-axis calibration values
+ magCalibration.x = (float)(rawData[0] - 128)/256.f + 1.f; // Return x-axis sensitivity adjustment values, etc.
+ magCalibration.y = (float)(rawData[1] - 128)/256.f + 1.f;
+ magCalibration.z = (float)(rawData[2] - 128)/256.f + 1.f;
+ replaceBlockAK(AK8963_CNTL,MGN_POWER_DN,0,4); // Power down magnetometer
+ wait_ms(50);
+ replaceBlockAK(AK8963_CNTL,((_mBits << 4 )| _mMode),0,5); // Set measurment mode, mMode[0:3]
+ writeByte(PWR_MGMT_1,CLOCK_SEL_PLL);
+ wait_ms(50);
+ mRes=10. * 4912. / 32760.0; // for Magenetometer 16BITS
+}
+
+void MPU9250_SPI::setAccelRange(AccelRange range) {
+ switch(range) {
+ case ACCEL_RANGE_2G:
+ aRes = 2.0f/32767.5f; break;
+ case ACCEL_RANGE_4G:
+ aRes = 4.0f/32767.5f; break;
+ case ACCEL_RANGE_8G:
+ aRes = 8.0f/32767.5f; break;
+ case ACCEL_RANGE_16G:
+ aRes = 16.0f/32767.5f; // setting the accel scale to 16G
+ break;
+ }
+ replaceBlock(ACCEL_CONFIG,range,3,2); // addr, value, at, size
+ _accelRange = range;
+}
+void MPU9250_SPI::setGyroRange(GyroRange range) {
+ switch(range) {
+ case GYRO_RANGE_250DPS:
+ gRes = 250.0f/32767.5f; break;
+ case GYRO_RANGE_500DPS:
+ gRes = 500.0f/32767.5f; break;
+ case GYRO_RANGE_1000DPS:
+ gRes = 1000.0f/32767.5f; break;
+ case GYRO_RANGE_2000DPS:
+ gRes = 2000.0f/32767.5f ; break;
+ }
+ replaceBlock(GYRO_CONFIG,range,3,2);
+ _gyroRange = range;
+}
+void MPU9250_SPI::setDlpfBandwidth(DlpfBandwidth bandwidth) {
+ replaceBlock(ACCEL_CONFIG2,bandwidth,0,4); //Accel DLPF [0:2]
+ replaceBlock(MPU_CONFIG,bandwidth,0,3); //Gyro DLPF [0:2]
+ _bandwidth = bandwidth;
+}
+
+void MPU9250_SPI::setSampleRate(SampleRate srd){
+ writeByte(SMPLRT_DIV, srd); // sampling rate set
+ _srd = srd;
+}
+
+void MPU9250_SPI::enableDataReadyInterrupt() {
+ writeByte(INT_PIN_CFG,0x00); // setup interrupt, 50 us pulse
+ writeByte(INT_ENABLE,0x01) ; // set to data ready
+}
+
+void MPU9250_SPI::updateSensors(){
+ int16_t MPU9250Data[10]; // MPU9250 accel/gyro 에서 16비트 정수로 7개 저장
+ uint8_t rawData[21]; // 가속도 자이로 원시 데이터 보관
+ writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR|SPI_READ); // Set the I2C slave addres of AK8963 and set for read.
+ writeByte(I2C_SLV0_REG,AK8963_XOUT_L); // I2C slave 0 register address from where to begin data transfer
+ writeByte(I2C_SLV0_CTRL, 0x87); // Read 7 bytes from the magnetometer
+ readBytes(ACCEL_XOUT_H, 21, rawData); // 16비트 정수로 7개 저장--> 14byte
+ MPU9250Data[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // signed 16-bit (MSB + LSB)
+ MPU9250Data[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;
+ MPU9250Data[2] = ((int16_t)rawData[4] << 8) | rawData[5] ;
+ MPU9250Data[3] = ((int16_t)rawData[6] << 8) | rawData[7] ;
+ MPU9250Data[4] = ((int16_t)rawData[8] << 8) | rawData[9] ;
+ MPU9250Data[5] = ((int16_t)rawData[10] << 8) | rawData[11] ;
+ MPU9250Data[6] = ((int16_t)rawData[12] << 8) | rawData[13] ;
+ MPU9250Data[7] = (((int16_t)rawData[15]) << 8) |rawData[14];
+ MPU9250Data[8] = (((int16_t)rawData[17]) << 8) |rawData[16];
+ MPU9250Data[9] = (((int16_t)rawData[19]) << 8) |rawData[18];
+ a.x = (float)MPU9250Data[0] * aRes - accelBias.x; // 가속도 해상도와 바이어스 보정
+ a.y = (float)MPU9250Data[1] * aRes - accelBias.y;
+ a.z = (float)MPU9250Data[2] * aRes - accelBias.z;
+ g.x = (float)MPU9250Data[4] * gRes - gyroBias.x; // 자이로 해상도 보정
+ g.y = (float)MPU9250Data[5] * gRes - gyroBias.y; // 자이로 바이어스는 칩내부에서 보정함!!!
+ g.z = (float)MPU9250Data[6] * gRes - gyroBias.z;
+ m.x = (float)(MPU9250Data[7] * mRes * magCalibration.x - magBias.x) * magScale.x;
+ m.y = (float)(MPU9250Data[8] * mRes * magCalibration.y - magBias.y) * magScale.y;
+ m.z = (float)(MPU9250Data[9] * mRes * magCalibration.z - magBias.z) * magScale.z;
+}
+void MPU9250_SPI::updateAccelGyro() {
+ int16_t MPU9250Data[7]; // MPU9250 accel/gyro 에서 16비트 정수로 7개 저장
+ readMPU9250Data(MPU9250Data); // 읽으면 INT 핀 해제
+ a.x = (float)MPU9250Data[0] * aRes - accelBias.x; // 가속도 해상도와 바이어스 보정
+ a.y = (float)MPU9250Data[1] * aRes - accelBias.y;
+ a.z = (float)MPU9250Data[2] * aRes - accelBias.z;
+ g.x = (float)MPU9250Data[4] * gRes - gyroBias.x; // 자이로 해상도 보정
+ g.y = (float)MPU9250Data[5] * gRes - gyroBias.y; //
+ g.z = (float)MPU9250Data[6] * gRes - gyroBias.z;
+}
+
+void MPU9250_SPI::readMPU9250Data(int16_t * destination) {
+ uint8_t rawData[14]; // 가속도 자이로 원시 데이터 보관
+ readBytes(ACCEL_XOUT_H, 14, rawData); // 16비트 정수로 7개 저장--> 14byte
+ destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // signed 16-bit (MSB + LSB)
+ destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;
+ destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ;
+ destination[3] = ((int16_t)rawData[6] << 8) | rawData[7] ;
+ destination[4] = ((int16_t)rawData[8] << 8) | rawData[9] ;
+ destination[5] = ((int16_t)rawData[10] << 8) | rawData[11] ;
+ destination[6] = ((int16_t)rawData[12] << 8) | rawData[13] ;
+}
+
+void MPU9250_SPI::updateMag() {
+ int16_t magCount[3] = {0, 0, 0}; // 16-bit 지자기 데이터
+ readMagData(magCount); // 지자기 데이터 읽기
+ // 지자기 해상도, 검정값, 바이어스 보정, 검정값 (magCalibration[] )은 칩의 ROM에서
+ m.x = (float)(magCount[0] * mRes * magCalibration.x - magBias.x) * magScale.x;
+ m.y = (float)(magCount[1] * mRes * magCalibration.y - magBias.y) * magScale.y;
+ m.z = (float)(magCount[2] * mRes * magCalibration.z - magBias.z) * magScale.z;
+}
+void MPU9250_SPI::readMagData(int16_t * destination) {
+ uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
+ if(readAK8963Byte(AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
+ readAK8963Bytes(AK8963_XOUT_L, 7,rawData); // Read the six raw data and ST2 registers sequentially into data array
+ uint8_t c = rawData[6]; // End data read by reading ST2 register
+ if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
+ destination[0] = ((int16_t)rawData[1] << 8) | rawData[0]; // Turn the MSB and LSB into a signed 16-bit value
+ destination[1] = ((int16_t)rawData[3] << 8) | rawData[2]; // Data stored as little Endian
+ destination[2] = ((int16_t)rawData[5] << 8) | rawData[4];
+ }
+ }
+}
+
+void MPU9250_SPI::writeByte(uint8_t subAddress, uint8_t data){ /* write data to device */
+ // _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction
+ // digitalWrite(_csPin,LOW); // select the MPU9250 chip
+ // _spi->transfer(subAddress); // write the register address
+ // _spi->transfer(data); // write the data
+ // digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
+ // _spi->endTransaction(); // end the transaction
+ _spi.frequency(SPI_LS_CLOCK); // setup clock
+ _csPin=0; // select the MPU9250 chip
+ _spi.write(subAddress); // write the register address
+ _spi.write(data); // write the data
+ _csPin=1; // deselect the MPU9250 chip
+}
+uint8_t MPU9250_SPI::readByte(uint8_t subAddress){
+ // _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3));
+ // digitalWrite(_csPin,LOW); // select the MPU9250 chip
+ // _spi->transfer(subAddress | SPI_READ); // specify the starting register address
+ // uint8_t data = _spi->transfer(0x00); // read the data
+ // digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
+ // _spi->endTransaction(); // end the transaction
+
+ _spi.frequency(SPI_LS_CLOCK); // setup clock
+ _csPin=0; // select the MPU9250 chip
+ _spi.write(subAddress| SPI_READ); // use READ MASK
+ uint8_t data =_spi.write(0); // write any to get data
+ _csPin=1; // deselect the MPU9250 chip
+ return data;
+}
+
+void MPU9250_SPI::readBytes(uint8_t subAddress, uint8_t cnt, uint8_t* dest){
+ // _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3));
+ // digitalWrite(_csPin,LOW); // select the MPU9250 chip
+ // _spi->transfer(subAddress | SPI_READ); // specify the starting register address
+ // for(uint8_t i = 0; i < count; i++){
+ // dest[i] = _spi->transfer(0x00); // read the data
+ // }
+ // digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
+ // _spi->endTransaction(); // end the transaction
+ _spi.frequency(SPI_HS_CLOCK); // setup clock
+ _csPin=0; // select the MPU9250 chip
+ _spi.write(subAddress | SPI_READ); // specify the starting register address
+ for(uint8_t i = 0; i < cnt; i++){
+ dest[i] = _spi.write(0x00); // read the data
+ }
+ _csPin=1; // deselect the MPU9250 chip
+}
+
+void MPU9250_SPI::writeAK8963Byte(uint8_t subAddress, uint8_t data){
+ writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR) ; // set slave 0 to the AK8963 and set for write
+ writeByte(I2C_SLV0_REG,subAddress) ; // set the register to the desired AK8963 sub address
+ writeByte(I2C_SLV0_DO,data) ; // store the data for write
+ writeByte(I2C_SLV0_CTRL,I2C_SLV0_EN | (uint8_t)1); // enable I2C and send 1 byte
+}
+
+void MPU9250_SPI::readAK8963Bytes(uint8_t subAddress, uint8_t count, uint8_t* dest){
+ writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG) ; // set slave 0 to the AK8963 and set for read
+ writeByte(I2C_SLV0_REG,subAddress) ; // set the register to the desired AK8963 sub address
+ writeByte(I2C_SLV0_CTRL,I2C_SLV0_EN | count); // enable I2C and request the bytes
+ wait_ms(1); // takes some time for these registers to fill
+ readBytes(EXT_SENS_DATA_00,count,dest); // read the bytes off the MPU9250 EXT_SENS_DATA registers
+}
+
+uint8_t MPU9250_SPI::readAK8963Byte(uint8_t subAddress){
+ writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG) ; // set slave 0 to the AK8963 and set for read
+ writeByte(I2C_SLV0_REG,subAddress) ; // set the register to the desired AK8963 sub address
+ writeByte(I2C_SLV0_CTRL,I2C_SLV0_EN | (uint8_t)1); // enable I2C and request the bytes
+ wait_ms(11); // takes some time for these registers to fill
+ return readByte(EXT_SENS_DATA_00); // read the bytes off the MPU9250 EXT_SENS_DATA registers
+}
+void MPU9250_SPI::replaceBlock(uint8_t address, uint8_t block, uint8_t at, uint8_t sz){
+ uint8_t data=readByte(address);
+ data &= ~(((1<<sz)-1)<<at);
+ data |= block<<at;
+ writeByte(address, data );
+}
+void MPU9250_SPI::replaceBlockAK(uint8_t address, uint8_t block, uint8_t at, uint8_t sz){
+ uint8_t data=readByte(address);
+ data &= ~(((1<<sz)-1)<<at);
+ data |= block<<at;
+ writeAK8963Byte(address, data );
+}
+
--- a/MPU9250.cpp Tue Jul 01 13:59:45 2014 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,390 +0,0 @@
-/*CODED by Qiyong Mu on 21/06/2014
-kylongmu@msn.com
-*/
-
-#include <mbed.h>
-#include "MPU9250.h"
-
-mpu9250_spi::mpu9250_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs) {}
-
-unsigned int mpu9250_spi::WriteReg( uint8_t WriteAddr, uint8_t WriteData )
-{
- unsigned int temp_val;
- select();
- spi.write(WriteAddr);
- temp_val=spi.write(WriteData);
- deselect();
- wait_us(50);
- return temp_val;
-}
-unsigned int mpu9250_spi::ReadReg( uint8_t WriteAddr, uint8_t WriteData )
-{
- return WriteReg(WriteAddr | READ_FLAG,WriteData);
-}
-void mpu9250_spi::ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes )
-{
- unsigned int i = 0;
-
- select();
- spi.write(ReadAddr | READ_FLAG);
- for(i=0; i<Bytes; i++)
- ReadBuf[i] = spi.write(0x00);
- deselect();
- wait_us(50);
-}
-
-/*-----------------------------------------------------------------------------------------------
- INITIALIZATION
-usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and
-low pass filter value; suitable values are:
-BITS_DLPF_CFG_256HZ_NOLPF2
-BITS_DLPF_CFG_188HZ
-BITS_DLPF_CFG_98HZ
-BITS_DLPF_CFG_42HZ
-BITS_DLPF_CFG_20HZ
-BITS_DLPF_CFG_10HZ
-BITS_DLPF_CFG_5HZ
-BITS_DLPF_CFG_2100HZ_NOLPF
-returns 1 if an error occurred
------------------------------------------------------------------------------------------------*/
-#define MPU_InitRegNum 17
-
-bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter){
- uint8_t i = 0;
- uint8_t MPU_Init_Data[MPU_InitRegNum][2] = {
- {0x80, MPUREG_PWR_MGMT_1}, // Reset Device
- {0x01, MPUREG_PWR_MGMT_1}, // Clock Source
- {0x00, MPUREG_PWR_MGMT_2}, // Enable Acc & Gyro
- {low_pass_filter, MPUREG_CONFIG}, // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz
- {0x18, MPUREG_GYRO_CONFIG}, // +-2000dps
- {0x08, MPUREG_ACCEL_CONFIG}, // +-4G
- {0x09, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz
- {0x30, MPUREG_INT_PIN_CFG}, //
- //{0x40, MPUREG_I2C_MST_CTRL}, // I2C Speed 348 kHz
- //{0x20, MPUREG_USER_CTRL}, // Enable AUX
- {0x20, MPUREG_USER_CTRL}, // I2C Master mode
- {0x0D, MPUREG_I2C_MST_CTRL}, // I2C configuration multi-master IIC 400KHz
-
- {AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR}, //Set the I2C slave addres of AK8963 and set for write.
- //{0x09, MPUREG_I2C_SLV4_CTRL},
- //{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay
-
- {AK8963_CNTL2, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
- {0x01, MPUREG_I2C_SLV0_DO}, // Reset AK8963
- {0x81, MPUREG_I2C_SLV0_CTRL}, //Enable I2C and set 1 byte
-
- {AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
- {0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit
- {0x81, MPUREG_I2C_SLV0_CTRL} //Enable I2C and set 1 byte
-
- };
- spi.format(8,0);
- spi.frequency(1000000);
-
- for(i=0; i<MPU_InitRegNum; i++) {
- WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
- wait(0.001); //I2C must slow down the write speed, otherwise it won't work
- }
-
- set_acc_scale(2);
- set_gyro_scale(250);
-
- //AK8963_calib_Magnetometer(); //Can't load this function here , strange problem?
- return 0;
-}
-/*-----------------------------------------------------------------------------------------------
- ACCELEROMETER SCALE
-usage: call this function at startup, after initialization, to set the right range for the
-accelerometers. Suitable ranges are:
-BITS_FS_2G
-BITS_FS_4G
-BITS_FS_8G
-BITS_FS_16G
-returns the range set (2,4,8 or 16)
------------------------------------------------------------------------------------------------*/
-unsigned int mpu9250_spi::set_acc_scale(int scale){
- unsigned int temp_scale;
- WriteReg(MPUREG_ACCEL_CONFIG, scale);
-
- switch (scale){
- case BITS_FS_2G:
- acc_divider=16384;
- break;
- case BITS_FS_4G:
- acc_divider=8192;
- break;
- case BITS_FS_8G:
- acc_divider=4096;
- break;
- case BITS_FS_16G:
- acc_divider=2048;
- break;
- }
- temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
-
- switch (temp_scale){
- case BITS_FS_2G:
- temp_scale=2;
- break;
- case BITS_FS_4G:
- temp_scale=4;
- break;
- case BITS_FS_8G:
- temp_scale=8;
- break;
- case BITS_FS_16G:
- temp_scale=16;
- break;
- }
- return temp_scale;
-}
-
-
-/*-----------------------------------------------------------------------------------------------
- GYROSCOPE SCALE
-usage: call this function at startup, after initialization, to set the right range for the
-gyroscopes. Suitable ranges are:
-BITS_FS_250DPS
-BITS_FS_500DPS
-BITS_FS_1000DPS
-BITS_FS_2000DPS
-returns the range set (250,500,1000 or 2000)
------------------------------------------------------------------------------------------------*/
-unsigned int mpu9250_spi::set_gyro_scale(int scale){
- unsigned int temp_scale;
- WriteReg(MPUREG_GYRO_CONFIG, scale);
- switch (scale){
- case BITS_FS_250DPS:
- gyro_divider=131;
- break;
- case BITS_FS_500DPS:
- gyro_divider=65.5;
- break;
- case BITS_FS_1000DPS:
- gyro_divider=32.8;
- break;
- case BITS_FS_2000DPS:
- gyro_divider=16.4;
- break;
- }
- temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00);
- switch (temp_scale){
- case BITS_FS_250DPS:
- temp_scale=250;
- break;
- case BITS_FS_500DPS:
- temp_scale=500;
- break;
- case BITS_FS_1000DPS:
- temp_scale=1000;
- break;
- case BITS_FS_2000DPS:
- temp_scale=2000;
- break;
- }
- return temp_scale;
-}
-
-
-/*-----------------------------------------------------------------------------------------------
- WHO AM I?
-usage: call this function to know if SPI is working correctly. It checks the I2C address of the
-mpu9250 which should be 104 when in SPI mode.
-returns the I2C address (104)
------------------------------------------------------------------------------------------------*/
-unsigned int mpu9250_spi::whoami(){
- unsigned int response;
- response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00);
- return response;
-}
-
-
-/*-----------------------------------------------------------------------------------------------
- READ ACCELEROMETER
-usage: call this function to read accelerometer data. Axis represents selected axis:
-0 -> X axis
-1 -> Y axis
-2 -> Z axis
------------------------------------------------------------------------------------------------*/
-void mpu9250_spi::read_acc()
-{
- uint8_t response[6];
- int16_t bit_data;
- float data;
- int i;
- ReadRegs(MPUREG_ACCEL_XOUT_H,response,6);
- for(i=0; i<3; i++) {
- bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
- data=(float)bit_data;
- accelerometer_data[i]=data/acc_divider;
- }
-
-}
-
-/*-----------------------------------------------------------------------------------------------
- READ GYROSCOPE
-usage: call this function to read gyroscope data. Axis represents selected axis:
-0 -> X axis
-1 -> Y axis
-2 -> Z axis
------------------------------------------------------------------------------------------------*/
-void mpu9250_spi::read_rot()
-{
- uint8_t response[6];
- int16_t bit_data;
- float data;
- int i;
- ReadRegs(MPUREG_GYRO_XOUT_H,response,6);
- for(i=0; i<3; i++) {
- bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
- data=(float)bit_data;
- gyroscope_data[i]=data/gyro_divider;
- }
-
-}
-
-/*-----------------------------------------------------------------------------------------------
- READ TEMPERATURE
-usage: call this function to read temperature data.
-returns the value in °C
------------------------------------------------------------------------------------------------*/
-void mpu9250_spi::read_temp(){
- uint8_t response[2];
- int16_t bit_data;
- float data;
- ReadRegs(MPUREG_TEMP_OUT_H,response,2);
-
- bit_data=((int16_t)response[0]<<8)|response[1];
- data=(float)bit_data;
- Temperature=(data/340)+36.53;
- deselect();
-}
-
-/*-----------------------------------------------------------------------------------------------
- READ ACCELEROMETER CALIBRATION
-usage: call this function to read accelerometer data. Axis represents selected axis:
-0 -> X axis
-1 -> Y axis
-2 -> Z axis
-returns Factory Trim value
------------------------------------------------------------------------------------------------*/
-void mpu9250_spi::calib_acc()
-{
- uint8_t response[4];
- int temp_scale;
- //READ CURRENT ACC SCALE
- temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
- set_acc_scale(BITS_FS_8G);
- //ENABLE SELF TEST need modify
- //temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis);
-
- ReadRegs(MPUREG_SELF_TEST_X,response,4);
- calib_data[0]=((response[0]&11100000)>>3)|((response[3]&00110000)>>4);
- calib_data[1]=((response[1]&11100000)>>3)|((response[3]&00001100)>>2);
- calib_data[2]=((response[2]&11100000)>>3)|((response[3]&00000011));
-
- set_acc_scale(temp_scale);
-}
-uint8_t mpu9250_spi::AK8963_whoami(){
- uint8_t response;
- WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
- WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer
- WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Read 1 byte from the magnetometer
-
- //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes
- wait(0.001);
- response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00); //Read I2C
- //ReadRegs(MPUREG_EXT_SENS_DATA_00,response,1);
- //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
-
- return response;
-}
-void mpu9250_spi::AK8963_calib_Magnetometer(){
- uint8_t response[3];
- float data;
- int i;
-
- WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
- WriteReg(MPUREG_I2C_SLV0_REG, AK8963_ASAX); //I2C slave 0 register address from where to begin data transfer
- WriteReg(MPUREG_I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer
-
- //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes
- wait(0.001);
- //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00); //Read I2C
- ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3);
-
- //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
- for(i=0; i<3; i++) {
- data=response[i];
- Magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor;
- }
-}
-void mpu9250_spi::AK8963_read_Magnetometer(){
- uint8_t response[7];
- int16_t bit_data;
- float data;
- int i;
-
- WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
- WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
- WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 6 bytes from the magnetometer
-
- wait(0.001);
- ReadRegs(MPUREG_EXT_SENS_DATA_00,response,7);
- //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.
- for(i=0; i<3; i++) {
- bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
- data=(float)bit_data;
- Magnetometer[i]=data*Magnetometer_ASA[i];
- }
-}
-void mpu9250_spi::read_all(){
- uint8_t response[21];
- int16_t bit_data;
- float data;
- int i;
-
- //Send I2C command at first
- WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
- WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
- WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 7 bytes from the magnetometer
- //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.
-
- //wait(0.001);
- ReadRegs(MPUREG_ACCEL_XOUT_H,response,21);
- //Get accelerometer value
- for(i=0; i<3; i++) {
- bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
- data=(float)bit_data;
- accelerometer_data[i]=data/acc_divider;
- }
- //Get temperature
- bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
- data=(float)bit_data;
- Temperature=((data-21)/333.87)+21;
- //Get gyroscop value
- for(i=4; i<7; i++) {
- bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
- data=(float)bit_data;
- gyroscope_data[i-4]=data/gyro_divider;
- }
- //Get Magnetometer value
- for(i=7; i<10; i++) {
- bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
- data=(float)bit_data;
- Magnetometer[i-7]=data*Magnetometer_ASA[i-7];
- }
-}
-
-/*-----------------------------------------------------------------------------------------------
- SPI SELECT AND DESELECT
-usage: enable and disable mpu9250 communication bus
------------------------------------------------------------------------------------------------*/
-void mpu9250_spi::select() {
- //Set CS low to start transmission (interrupts conversion)
- cs = 0;
-}
-void mpu9250_spi::deselect() {
- //Set CS high to stop transmission (restarts conversion)
- cs = 1;
-}
\ No newline at end of file
--- a/MPU9250.h Tue Jul 01 13:59:45 2014 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,252 +0,0 @@
-/*CODED by Qiyong Mu on 21/06/2014
-kylongmu@msn.com
-*/
-
-
-#ifndef mpu9250_h
-#define mpu9250_h
-#include "mbed.h"
-
-
-class mpu9250_spi
-{
- SPI& spi;
- DigitalOut cs;
-
- public:
- mpu9250_spi(SPI& _spi, PinName _cs);
- unsigned int WriteReg( uint8_t WriteAddr, uint8_t WriteData );
- unsigned int ReadReg( uint8_t WriteAddr, uint8_t WriteData );
- void ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes );
-
- bool init(int sample_rate_div,int low_pass_filter);
- void read_temp();
- void read_acc();
- void read_rot();
- unsigned int set_gyro_scale(int scale);
- unsigned int set_acc_scale(int scale);
- void calib_acc();
- void AK8963_calib_Magnetometer();
- void select();
- void deselect();
- unsigned int whoami();
- uint8_t AK8963_whoami();
- void AK8963_read_Magnetometer();
- void read_all();
-
-
-
- float acc_divider;
- float gyro_divider;
-
- int calib_data[3];
- float Magnetometer_ASA[3];
-
- float accelerometer_data[3];
- float Temperature;
- float gyroscope_data[3];
- float Magnetometer[3];
-
- private:
- PinName _CS_pin;
- PinName _SO_pin;
- PinName _SCK_pin;
- float _error;
-};
-
-#endif
-
-
-
-// mpu9250 registers
-#define MPUREG_XG_OFFS_TC 0x00
-#define MPUREG_YG_OFFS_TC 0x01
-#define MPUREG_ZG_OFFS_TC 0x02
-#define MPUREG_X_FINE_GAIN 0x03
-#define MPUREG_Y_FINE_GAIN 0x04
-#define MPUREG_Z_FINE_GAIN 0x05
-#define MPUREG_XA_OFFS_H 0x06
-#define MPUREG_XA_OFFS_L 0x07
-#define MPUREG_YA_OFFS_H 0x08
-#define MPUREG_YA_OFFS_L 0x09
-#define MPUREG_ZA_OFFS_H 0x0A
-#define MPUREG_ZA_OFFS_L 0x0B
-#define MPUREG_PRODUCT_ID 0x0C
-#define MPUREG_SELF_TEST_X 0x0D
-#define MPUREG_SELF_TEST_Y 0x0E
-#define MPUREG_SELF_TEST_Z 0x0F
-#define MPUREG_SELF_TEST_A 0x10
-#define MPUREG_XG_OFFS_USRH 0x13
-#define MPUREG_XG_OFFS_USRL 0x14
-#define MPUREG_YG_OFFS_USRH 0x15
-#define MPUREG_YG_OFFS_USRL 0x16
-#define MPUREG_ZG_OFFS_USRH 0x17
-#define MPUREG_ZG_OFFS_USRL 0x18
-#define MPUREG_SMPLRT_DIV 0x19
-#define MPUREG_CONFIG 0x1A
-#define MPUREG_GYRO_CONFIG 0x1B
-#define MPUREG_ACCEL_CONFIG 0x1C
-#define MPUREG_ACCEL_CONFIG_2 0x1D
-#define MPUREG_LP_ACCEL_ODR 0x1E
-#define MPUREG_MOT_THR 0x1F
-#define MPUREG_FIFO_EN 0x23
-#define MPUREG_I2C_MST_CTRL 0x24
-#define MPUREG_I2C_SLV0_ADDR 0x25
-#define MPUREG_I2C_SLV0_REG 0x26
-#define MPUREG_I2C_SLV0_CTRL 0x27
-#define MPUREG_I2C_SLV1_ADDR 0x28
-#define MPUREG_I2C_SLV1_REG 0x29
-#define MPUREG_I2C_SLV1_CTRL 0x2A
-#define MPUREG_I2C_SLV2_ADDR 0x2B
-#define MPUREG_I2C_SLV2_REG 0x2C
-#define MPUREG_I2C_SLV2_CTRL 0x2D
-#define MPUREG_I2C_SLV3_ADDR 0x2E
-#define MPUREG_I2C_SLV3_REG 0x2F
-#define MPUREG_I2C_SLV3_CTRL 0x30
-#define MPUREG_I2C_SLV4_ADDR 0x31
-#define MPUREG_I2C_SLV4_REG 0x32
-#define MPUREG_I2C_SLV4_DO 0x33
-#define MPUREG_I2C_SLV4_CTRL 0x34
-#define MPUREG_I2C_SLV4_DI 0x35
-#define MPUREG_I2C_MST_STATUS 0x36
-#define MPUREG_INT_PIN_CFG 0x37
-#define MPUREG_INT_ENABLE 0x38
-#define MPUREG_ACCEL_XOUT_H 0x3B
-#define MPUREG_ACCEL_XOUT_L 0x3C
-#define MPUREG_ACCEL_YOUT_H 0x3D
-#define MPUREG_ACCEL_YOUT_L 0x3E
-#define MPUREG_ACCEL_ZOUT_H 0x3F
-#define MPUREG_ACCEL_ZOUT_L 0x40
-#define MPUREG_TEMP_OUT_H 0x41
-#define MPUREG_TEMP_OUT_L 0x42
-#define MPUREG_GYRO_XOUT_H 0x43
-#define MPUREG_GYRO_XOUT_L 0x44
-#define MPUREG_GYRO_YOUT_H 0x45
-#define MPUREG_GYRO_YOUT_L 0x46
-#define MPUREG_GYRO_ZOUT_H 0x47
-#define MPUREG_GYRO_ZOUT_L 0x48
-#define MPUREG_EXT_SENS_DATA_00 0x49
-#define MPUREG_EXT_SENS_DATA_01 0x4A
-#define MPUREG_EXT_SENS_DATA_02 0x4B
-#define MPUREG_EXT_SENS_DATA_03 0x4C
-#define MPUREG_EXT_SENS_DATA_04 0x4D
-#define MPUREG_EXT_SENS_DATA_05 0x4E
-#define MPUREG_EXT_SENS_DATA_06 0x4F
-#define MPUREG_EXT_SENS_DATA_07 0x50
-#define MPUREG_EXT_SENS_DATA_08 0x51
-#define MPUREG_EXT_SENS_DATA_09 0x52
-#define MPUREG_EXT_SENS_DATA_10 0x53
-#define MPUREG_EXT_SENS_DATA_11 0x54
-#define MPUREG_EXT_SENS_DATA_12 0x55
-#define MPUREG_EXT_SENS_DATA_13 0x56
-#define MPUREG_EXT_SENS_DATA_14 0x57
-#define MPUREG_EXT_SENS_DATA_15 0x58
-#define MPUREG_EXT_SENS_DATA_16 0x59
-#define MPUREG_EXT_SENS_DATA_17 0x5A
-#define MPUREG_EXT_SENS_DATA_18 0x5B
-#define MPUREG_EXT_SENS_DATA_19 0x5C
-#define MPUREG_EXT_SENS_DATA_20 0x5D
-#define MPUREG_EXT_SENS_DATA_21 0x5E
-#define MPUREG_EXT_SENS_DATA_22 0x5F
-#define MPUREG_EXT_SENS_DATA_23 0x60
-#define MPUREG_I2C_SLV0_DO 0x63
-#define MPUREG_I2C_SLV1_DO 0x64
-#define MPUREG_I2C_SLV2_DO 0x65
-#define MPUREG_I2C_SLV3_DO 0x66
-#define MPUREG_I2C_MST_DELAY_CTRL 0x67
-#define MPUREG_SIGNAL_PATH_RESET 0x68
-#define MPUREG_MOT_DETECT_CTRL 0x69
-#define MPUREG_USER_CTRL 0x6A
-#define MPUREG_PWR_MGMT_1 0x6B
-#define MPUREG_PWR_MGMT_2 0x6C
-#define MPUREG_BANK_SEL 0x6D
-#define MPUREG_MEM_START_ADDR 0x6E
-#define MPUREG_MEM_R_W 0x6F
-#define MPUREG_DMP_CFG_1 0x70
-#define MPUREG_DMP_CFG_2 0x71
-#define MPUREG_FIFO_COUNTH 0x72
-#define MPUREG_FIFO_COUNTL 0x73
-#define MPUREG_FIFO_R_W 0x74
-#define MPUREG_WHOAMI 0x75
-#define MPUREG_XA_OFFSET_H 0x77
-#define MPUREG_XA_OFFSET_L 0x78
-#define MPUREG_YA_OFFSET_H 0x7A
-#define MPUREG_YA_OFFSET_L 0x7B
-#define MPUREG_ZA_OFFSET_H 0x7D
-#define MPUREG_ZA_OFFSET_L 0x7E
-/* ---- AK8963 Reg In MPU9250 ----------------------------------------------- */
-
-#define AK8963_I2C_ADDR 0x0c//0x18
-#define AK8963_Device_ID 0x48
-
-// Read-only Reg
-#define AK8963_WIA 0x00
-#define AK8963_INFO 0x01
-#define AK8963_ST1 0x02
-#define AK8963_HXL 0x03
-#define AK8963_HXH 0x04
-#define AK8963_HYL 0x05
-#define AK8963_HYH 0x06
-#define AK8963_HZL 0x07
-#define AK8963_HZH 0x08
-#define AK8963_ST2 0x09
-// Write/Read Reg
-#define AK8963_CNTL1 0x0A
-#define AK8963_CNTL2 0x0B
-#define AK8963_ASTC 0x0C
-#define AK8963_TS1 0x0D
-#define AK8963_TS2 0x0E
-#define AK8963_I2CDIS 0x0F
-// Read-only Reg ( ROM )
-#define AK8963_ASAX 0x10
-#define AK8963_ASAY 0x11
-#define AK8963_ASAZ 0x12
-
-// Configuration bits mpu9250
-#define BIT_SLEEP 0x40
-#define BIT_H_RESET 0x80
-#define BITS_CLKSEL 0x07
-#define MPU_CLK_SEL_PLLGYROX 0x01
-#define MPU_CLK_SEL_PLLGYROZ 0x03
-#define MPU_EXT_SYNC_GYROX 0x02
-#define BITS_FS_250DPS 0x00
-#define BITS_FS_500DPS 0x08
-#define BITS_FS_1000DPS 0x10
-#define BITS_FS_2000DPS 0x18
-#define BITS_FS_2G 0x00
-#define BITS_FS_4G 0x08
-#define BITS_FS_8G 0x10
-#define BITS_FS_16G 0x18
-#define BITS_FS_MASK 0x18
-#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00
-#define BITS_DLPF_CFG_188HZ 0x01
-#define BITS_DLPF_CFG_98HZ 0x02
-#define BITS_DLPF_CFG_42HZ 0x03
-#define BITS_DLPF_CFG_20HZ 0x04
-#define BITS_DLPF_CFG_10HZ 0x05
-#define BITS_DLPF_CFG_5HZ 0x06
-#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
-#define BITS_DLPF_CFG_MASK 0x07
-#define BIT_INT_ANYRD_2CLEAR 0x10
-#define BIT_RAW_RDY_EN 0x01
-#define BIT_I2C_IF_DIS 0x10
-
-#define READ_FLAG 0x80
-
-/* ---- Sensitivity --------------------------------------------------------- */
-
-#define MPU9250A_2g ((float)0.000061035156f) // 0.000061035156 g/LSB
-#define MPU9250A_4g ((float)0.000122070312f) // 0.000122070312 g/LSB
-#define MPU9250A_8g ((float)0.000244140625f) // 0.000244140625 g/LSB
-#define MPU9250A_16g ((float)0.000488281250f) // 0.000488281250 g/LSB
-
-#define MPU9250G_250dps ((float)0.007633587786f) // 0.007633587786 dps/LSB
-#define MPU9250G_500dps ((float)0.015267175572f) // 0.015267175572 dps/LSB
-#define MPU9250G_1000dps ((float)0.030487804878f) // 0.030487804878 dps/LSB
-#define MPU9250G_2000dps ((float)0.060975609756f) // 0.060975609756 dps/LSB
-
-#define MPU9250M_4800uT ((float)0.6f) // 0.6 uT/LSB
-
-#define MPU9250T_85degC ((float)0.002995177763f) // 0.002995177763 degC/LSB
-
-#define Magnetometer_Sensitivity_Scale_Factor ((float)0.15f)
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU9250RegisterMap.h Mon May 03 07:20:25 2021 +0000 @@ -0,0 +1,164 @@ +#ifndef MPU9250REGISTERMAP_H +#define MPU9250REGISTERMAP_H +#include <cmath> +#include "mbed.h" +// MPU9250 with SPI interface library Ver. 0.98 +// Made by HeeJae Park +// 2019.05.27 +//Magnetometer Registers ================================ +#define AK8963_WHO_AM_I 0x00 // should return 0x48 +#define AK8963_INFO 0x01 +#define AK8963_ST1 0x02 // data ready status bit 0 +#define AK8963_XOUT_L 0x03 // data +#define AK8963_XOUT_H 0x04 +#define AK8963_YOUT_L 0x05 +#define AK8963_YOUT_H 0x06 +#define AK8963_ZOUT_L 0x07 +#define AK8963_ZOUT_H 0x08 +#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 +#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 +#define AK8963_CNTL2 0x0B +#define AK8963_ASTC 0x0C // Self test control +#define AK8963_I2CDIS 0x0F // I2C disable +#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value +#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value +#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value +//IMU Registers ========================================== +#define SELF_TEST_X_GYRO 0x00 +#define SELF_TEST_Y_GYRO 0x01 +#define SELF_TEST_Z_GYRO 0x02 +#define SELF_TEST_X_ACCEL 0x0D +#define SELF_TEST_Y_ACCEL 0x0E +#define SELF_TEST_Z_ACCEL 0x0F +#define SELF_TEST_A 0x10 +#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope +#define XG_OFFSET_L 0x14 +#define YG_OFFSET_H 0x15 +#define YG_OFFSET_L 0x16 +#define ZG_OFFSET_H 0x17 +#define ZG_OFFSET_L 0x18 +#define SMPLRT_DIV 0x19 +#define MPU_CONFIG 0x1A +#define GYRO_CONFIG 0x1B +#define ACCEL_CONFIG 0x1C +#define ACCEL_CONFIG2 0x1D +#define LP_ACCEL_ODR 0x1E +#define WOM_THR 0x1F +#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms +#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] +#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms +#define FIFO_EN 0x23 +#define I2C_MST_CTRL 0x24 +#define I2C_SLV0_ADDR 0x25 +#define I2C_SLV0_REG 0x26 +#define I2C_SLV0_CTRL 0x27 +#define I2C_SLV1_ADDR 0x28 +#define I2C_SLV1_REG 0x29 +#define I2C_SLV1_CTRL 0x2A +#define I2C_SLV2_ADDR 0x2B +#define I2C_SLV2_REG 0x2C +#define I2C_SLV2_CTRL 0x2D +#define I2C_SLV3_ADDR 0x2E +#define I2C_SLV3_REG 0x2F +#define I2C_SLV3_CTRL 0x30 +#define I2C_SLV4_ADDR 0x31 +#define I2C_SLV4_REG 0x32 +#define I2C_SLV4_DO 0x33 +#define I2C_SLV4_CTRL 0x34 +#define I2C_SLV4_DI 0x35 +#define I2C_MST_STATUS 0x36 +#define INT_PIN_CFG 0x37 +#define INT_ENABLE 0x38 +#define DMP_INT_STATUS 0x39 // Check DMP interrupt +#define INT_STATUS 0x3A +#define ACCEL_XOUT_H 0x3B +#define ACCEL_XOUT_L 0x3C +#define ACCEL_YOUT_H 0x3D +#define ACCEL_YOUT_L 0x3E +#define ACCEL_ZOUT_H 0x3F +#define ACCEL_ZOUT_L 0x40 +#define TEMP_OUT_H 0x41 +#define TEMP_OUT_L 0x42 +#define GYRO_XOUT_H 0x43 +#define GYRO_XOUT_L 0x44 +#define GYRO_YOUT_H 0x45 +#define GYRO_YOUT_L 0x46 +#define GYRO_ZOUT_H 0x47 +#define GYRO_ZOUT_L 0x48 +#define EXT_SENS_DATA_00 0x49 +#define EXT_SENS_DATA_01 0x4A +#define EXT_SENS_DATA_02 0x4B +#define EXT_SENS_DATA_03 0x4C +#define EXT_SENS_DATA_04 0x4D +#define EXT_SENS_DATA_05 0x4E +#define EXT_SENS_DATA_06 0x4F +#define EXT_SENS_DATA_07 0x50 +#define EXT_SENS_DATA_08 0x51 +#define EXT_SENS_DATA_09 0x52 +#define EXT_SENS_DATA_10 0x53 +#define EXT_SENS_DATA_11 0x54 +#define EXT_SENS_DATA_12 0x55 +#define EXT_SENS_DATA_13 0x56 +#define EXT_SENS_DATA_14 0x57 +#define EXT_SENS_DATA_15 0x58 +#define EXT_SENS_DATA_16 0x59 +#define EXT_SENS_DATA_17 0x5A +#define EXT_SENS_DATA_18 0x5B +#define EXT_SENS_DATA_19 0x5C +#define EXT_SENS_DATA_20 0x5D +#define EXT_SENS_DATA_21 0x5E +#define EXT_SENS_DATA_22 0x5F +#define EXT_SENS_DATA_23 0x60 +#define MOT_DETECT_STATUS 0x61 +#define I2C_SLV0_DO 0x63 +#define I2C_SLV1_DO 0x64 +#define I2C_SLV2_DO 0x65 +#define I2C_SLV3_DO 0x66 +#define I2C_MST_DELAY_CTRL 0x67 +#define SIGNAL_PATH_RESET 0x68 +#define MOT_DETECT_CTRL 0x69 +#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP +#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode +#define PWR_MGMT_2 0x6C +#define DMP_BANK 0x6D // Activates a specific bank in the DMP +#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank +#define DMP_REG 0x6F // Register in DMP from which to read or to which to write +#define DMP_REG_1 0x70 +#define DMP_REG_2 0x71 +#define FIFO_COUNTH 0x72 +#define FIFO_COUNTL 0x73 +#define FIFO_R_W 0x74 +#define WHO_AM_I_MPU9250 0x75 // Should return 0x71 +#define XA_OFFSET_H 0x77 +#define XA_OFFSET_L 0x78 +#define YA_OFFSET_H 0x7A +#define YA_OFFSET_L 0x7B +#define ZA_OFFSET_H 0x7D +#define ZA_OFFSET_L 0x7E +// =================== Importat values +#define AK8963_I2C_ADDR 0x0C +#define AK8963_RESET 0x01// @ CNTL2 +#define MPU9250_WHOAMI_DEFAULT_VALUE 0x71 // 고유번호 +#define AK8963_WHOAMI_DEFAULT_VALUE 0x48 +#define SPI_LS_CLOCK 15000000 // 1 MHz +#define SPI_HS_CLOCK 15000000 // 15 MHz +#define I2C_READ_FLAG 0x80 // for all I2C +#define SPI_READ 0x80 //SPI READ +#define I2C_MST_EN 0x20 // @ USER_CTRL +#define I2C_MST_CLK 0x0D // @I2C_MST_CTRL 400KHz +#define I2C_SLV0_EN 0x80 // @I2C_SLV0_CTRL slave 0 enable +#define CLOCK_SEL_PLL 0x01 // @ PWR_MGMNT_1 +#define PWR_RESET 0x80 // @ PWR_MGMNT_1 +#define SEN_ENABLE 0x00 // @ PWR_MGMNT_2 +// some conversion +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif +#define DEG_TO_RAD ( M_PI /180) +#define RAD_TO_DEG (180/M_PI) +#define TWO_PI (2*M_PI) +// complementary filter + + + +#endif // MPU9250REGISTERMAP_H
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250_SPI.h Mon May 03 07:20:25 2021 +0000
@@ -0,0 +1,90 @@
+#ifndef MPU9250_SPI_
+#define MPU9250_SPI_
+#include "mbed.h"
+#include <cmath>
+#include "MPU9250RegisterMap.h"
+#define DT 0.01f
+#define fc 0.5f
+#define Tc (1./(2.*M_PI*fc))
+enum GyroRange { GYRO_RANGE_250DPS, GYRO_RANGE_500DPS, GYRO_RANGE_1000DPS, GYRO_RANGE_2000DPS };
+enum AccelRange { ACCEL_RANGE_2G, ACCEL_RANGE_4G, ACCEL_RANGE_8G, ACCEL_RANGE_16G };
+enum DlpfBandwidth { DLPF_BANDWIDTH_250HZ, DLPF_BANDWIDTH_184HZ, DLPF_BANDWIDTH_92HZ, DLPF_BANDWIDTH_41HZ,
+ DLPF_BANDWIDTH_20HZ, DLPF_BANDWIDTH_10HZ, DLPF_BANDWIDTH_5HZ };
+enum MagnBits { MGN_14BITS, MGN_16BITS }; // CNTL1 offset 4 0:14bits, 1:16bits
+enum MagnMode { MGN_POWER_DN=0, MGN_SNGL_MEAS=1, MGN_CONT_MEAS1=2,MGN_CONT_MEAS2=6,MGN_EX_TRIG=4,
+ MGN_SELF_TEST=8, MGN_FUSE_ROM=15}; // CNTL1 offset 0
+enum SampleRate {SR_1000HZ=0, SR_200HZ=4, SR_100HZ=9 }; // 1kHz/(1+SRD)
+struct Vect3 {float x,y,z;};
+class MPU9250_SPI {
+ float aRes ; // 가속도 (LSB 당의 값)
+ float gRes ; // 자이로 (LSB 당의 값)
+ float mRes ; // 지자기 (LSB 당의 값)
+ AccelRange _accelRange;
+ GyroRange _gyroRange;
+ DlpfBandwidth _bandwidth;
+ MagnMode _mMode;
+ MagnBits _mBits;
+ SampleRate _srd;
+ Vect3 magCalibration ; // factory mag calibration
+ Vect3 magBias ; //지자기 바이어스
+ Vect3 magScale ; // 지자기 스케일
+ Vect3 gyroBias ; // 자이로 바이어스
+ Vect3 accelBias ; // 가속도 바이어스
+ int16_t tempCount; // 원시 온도값
+ float temperature; //실제 온도값 (도C)
+ float SelfTestResult[6]; // 자이로 가속도 실험결과
+ Vect3 a, g, m;
+ float roll, pitch, yaw;
+ float magnetic_declination; // Seoul 2019.1.2
+ SPI _spi;
+ DigitalOut _csPin;
+ InterruptIn _intPin;
+ Timer _tmr;
+ volatile static bool _dataReady;
+public:
+ // MPU9250_SPI(SPIClass& bus,uint8_t csPin,uint8_t intPin);
+ MPU9250_SPI(PinName mosi,PinName miso,PinName sclk, PinName cs, PinName intpin );
+ void setup() ;
+ void update(Vect3& _a,Vect3& _g,Vect3& _m) ;
+ Vect3 getAccBias() const { return accelBias; }
+ Vect3 getGyroBias() const { return gyroBias; }
+ Vect3 getMagBias() const { return magBias; }
+ Vect3 getMagScale() const { return magScale; }
+ void setAccBias(Vect3 v) { accelBias = v; }
+ void setGyroBias(Vect3 v) { gyroBias = v; }
+ void setMagBias(Vect3 v) { magBias = v; }
+ void setMagScale(Vect3 v) { magScale = v; }
+ void setMagneticDeclination(const float d) { magnetic_declination = d; }
+ void setAccelRange(AccelRange range) ;
+ void setGyroRange(GyroRange range);
+ void setDlpfBandwidth(DlpfBandwidth bandwidth);
+ void setSampleRate(SampleRate srd);
+ float getRoll() const { return roll; }
+ float getPitch() const { return pitch; }
+ float getYaw() const { return yaw; }
+ Vect3 getAccelVect() {return a;}
+ Vect3 getGyroVect() {return g;}
+ Vect3 getMagVect() {return m;}
+ void enableDataReadyInterrupt();
+ bool isDataReady(){return _dataReady;}
+ private:
+ uint8_t isConnectedMPU9250() ;
+ uint8_t isConnectedAK8963() ;
+ void initMPU9250() ;
+ void initAK8963() ;
+ void intService(){ _dataReady=true; }
+ void updateSensors();
+ void updateAccelGyro() ;
+ void readMPU9250Data(int16_t * destination) ;
+ void updateMag() ;
+ void readMagData(int16_t * destination) ;
+ void writeByte(uint8_t subAddress, uint8_t data);
+ uint8_t readByte(uint8_t subAddress);
+ void readBytes(uint8_t subAddress, uint8_t count, uint8_t* dest);
+ void replaceBlock(uint8_t address, uint8_t block, uint8_t at, uint8_t sz);
+ void replaceBlockAK(uint8_t address, uint8_t block, uint8_t at, uint8_t sz);
+ void writeAK8963Byte(uint8_t subAddress, uint8_t data);
+ void readAK8963Bytes(uint8_t subAddress, uint8_t count, uint8_t* dest);
+ uint8_t readAK8963Byte(uint8_t subAddress);
+};
+#endif