주 형진 / ai_car

ai_car

MPU9205_SPI.cpp@12:f3816a0f498e, 2021-05-03 (annotated)

Committer:
wngudwls000
Date:
Mon May 03 07:20:25 2021 +0000
Revision:
12:f3816a0f498e
adas

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wngudwls000 12:f3816a0f498e 1 #include "mbed.h"
wngudwls000 12:f3816a0f498e 2 #include "MPU9250_SPI.h"
wngudwls000 12:f3816a0f498e 3 #include "MPU9250RegisterMap.h"
wngudwls000 12:f3816a0f498e 4 #include <cmath>
wngudwls000 12:f3816a0f498e 5 // MPU9250 with SPI interface library Ver. 0.98
wngudwls000 12:f3816a0f498e 6 // Made by HeeJae Park
wngudwls000 12:f3816a0f498e 7 // 2019.05.27
wngudwls000 12:f3816a0f498e 8 //extern Serial pc;
wngudwls000 12:f3816a0f498e 9 volatile bool MPU9250_SPI::_dataReady=false;
wngudwls000 12:f3816a0f498e 10 MPU9250_SPI::MPU9250_SPI(PinName mosi,PinName miso,PinName sclk, PinName cs, PinName intpin)
wngudwls000 12:f3816a0f498e 11 : _spi(mosi,miso,sclk), _csPin(cs), _intPin(intpin),_mMode(MGN_CONT_MEAS2),_mBits( MGN_16BITS),_srd(SR_100HZ) {
wngudwls000 12:f3816a0f498e 12 magCalibration.x=0;magCalibration.y=0;magCalibration.z=0;
wngudwls000 12:f3816a0f498e 13 magBias.x=0; magBias.y=0; magBias.z=0;
wngudwls000 12:f3816a0f498e 14 magScale.x=1;magScale.y=1;magScale.z=1;
wngudwls000 12:f3816a0f498e 15 gyroBias.x =0; gyroBias.y =0; gyroBias.z =0;
wngudwls000 12:f3816a0f498e 16 accelBias.x=0; accelBias.y=0; accelBias.z=0;
wngudwls000 12:f3816a0f498e 17 magnetic_declination = 8.5;
wngudwls000 12:f3816a0f498e 18 _csPin=1;
wngudwls000 12:f3816a0f498e 19 }
wngudwls000 12:f3816a0f498e 20 void MPU9250_SPI::setup() {
wngudwls000 12:f3816a0f498e 21 _csPin=1; // setting CS pin high
wngudwls000 12:f3816a0f498e 22 _spi.format(8,3); // SPI mode 3
wngudwls000 12:f3816a0f498e 23 _spi.frequency(SPI_HS_CLOCK); // 1Mega
wngudwls000 12:f3816a0f498e 24 uint8_t m_whoami = 0x00;
wngudwls000 12:f3816a0f498e 25 uint8_t a_whoami = 0x00;
wngudwls000 12:f3816a0f498e 26 m_whoami = isConnectedMPU9250();
wngudwls000 12:f3816a0f498e 27 if (m_whoami==MPU9250_WHOAMI_DEFAULT_VALUE) {
wngudwls000 12:f3816a0f498e 28 initMPU9250();
wngudwls000 12:f3816a0f498e 29 a_whoami = isConnectedAK8963();
wngudwls000 12:f3816a0f498e 30 if (a_whoami == AK8963_WHOAMI_DEFAULT_VALUE){
wngudwls000 12:f3816a0f498e 31 initAK8963();
wngudwls000 12:f3816a0f498e 32 }
wngudwls000 12:f3816a0f498e 33 else {
wngudwls000 12:f3816a0f498e 34 while(1);
wngudwls000 12:f3816a0f498e 35 }
wngudwls000 12:f3816a0f498e 36 }
wngudwls000 12:f3816a0f498e 37 else {
wngudwls000 12:f3816a0f498e 38 while(1);
wngudwls000 12:f3816a0f498e 39 }
wngudwls000 12:f3816a0f498e 40 _intPin.rise(callback(this, &MPU9250_SPI::intService));
wngudwls000 12:f3816a0f498e 41 _tmr.start();
wngudwls000 12:f3816a0f498e 42 }
wngudwls000 12:f3816a0f498e 43 void MPU9250_SPI::update(Vect3& _a,Vect3& _g,Vect3& _m) {
wngudwls000 12:f3816a0f498e 44 if (_dataReady){ // On interrupt, check if data ready interrupt
wngudwls000 12:f3816a0f498e 45 updateSensors();
wngudwls000 12:f3816a0f498e 46 _a=a;_g=g;_m=m;
wngudwls000 12:f3816a0f498e 47 }
wngudwls000 12:f3816a0f498e 48 }
wngudwls000 12:f3816a0f498e 49
wngudwls000 12:f3816a0f498e 50 uint8_t MPU9250_SPI::isConnectedMPU9250() {
wngudwls000 12:f3816a0f498e 51 uint8_t c = readByte(WHO_AM_I_MPU9250);
wngudwls000 12:f3816a0f498e 52 return c; // (c == MPU9250_WHOAMI_DEFAULT_VALUE);
wngudwls000 12:f3816a0f498e 53 }
wngudwls000 12:f3816a0f498e 54 uint8_t MPU9250_SPI::isConnectedAK8963() {
wngudwls000 12:f3816a0f498e 55 uint8_t c = readAK8963Byte(AK8963_WHO_AM_I);
wngudwls000 12:f3816a0f498e 56 return c; // (c == AK8963_WHOAMI_DEFAULT_VALUE);
wngudwls000 12:f3816a0f498e 57 }
wngudwls000 12:f3816a0f498e 58
wngudwls000 12:f3816a0f498e 59 void MPU9250_SPI::initMPU9250() {
wngudwls000 12:f3816a0f498e 60 wait_ms(100);
wngudwls000 12:f3816a0f498e 61 writeByte(PWR_MGMT_1, CLOCK_SEL_PLL);
wngudwls000 12:f3816a0f498e 62 writeByte(USER_CTRL,I2C_MST_EN); // Master enable
wngudwls000 12:f3816a0f498e 63 writeByte(I2C_MST_CTRL,I2C_MST_CLK); // I2C master clock =400HZ
wngudwls000 12:f3816a0f498e 64 replaceBlockAK(AK8963_CNTL,MGN_POWER_DN,0,4); // Power down
wngudwls000 12:f3816a0f498e 65 writeByte(PWR_MGMT_1, PWR_RESET); // Clear sleep mode bit (6), enable all sensors
wngudwls000 12:f3816a0f498e 66 wait_ms(100);
wngudwls000 12:f3816a0f498e 67 writeByte(PWR_MGMT_1, CLOCK_SEL_PLL);
wngudwls000 12:f3816a0f498e 68 setDlpfBandwidth( DLPF_BANDWIDTH_5HZ);
wngudwls000 12:f3816a0f498e 69 writeByte(SMPLRT_DIV, SR_100HZ); //{SR_1000HZ=0, SR_200HZ=4, SR_100HZ=9 }
wngudwls000 12:f3816a0f498e 70 setGyroRange(GYRO_RANGE_2000DPS);
wngudwls000 12:f3816a0f498e 71 writeByte(PWR_MGMT_2,SEN_ENABLE);
wngudwls000 12:f3816a0f498e 72 setAccelRange(ACCEL_RANGE_16G);//{ _2G, _4G, _8G, _16G }
wngudwls000 12:f3816a0f498e 73 setDlpfBandwidth(DLPF_BANDWIDTH_184HZ); // [250HZ, 184HZ, 92HZ, 41HZ, 20HZ, 10HZ, 5HZ]
wngudwls000 12:f3816a0f498e 74 writeByte(INT_PIN_CFG, 0x20); // LATCH_INT_EN=1, BYPASS_EN=1-->0 (0x22)
wngudwls000 12:f3816a0f498e 75 writeByte(INT_ENABLE, 0x01); // Enable raw data ready (bit 0) interrupt
wngudwls000 12:f3816a0f498e 76 writeByte(USER_CTRL,I2C_MST_EN);
wngudwls000 12:f3816a0f498e 77 wait_ms(100);
wngudwls000 12:f3816a0f498e 78 writeByte(I2C_MST_CTRL,I2C_MST_CLK);
wngudwls000 12:f3816a0f498e 79 wait_ms(100);
wngudwls000 12:f3816a0f498e 80 }
wngudwls000 12:f3816a0f498e 81
wngudwls000 12:f3816a0f498e 82 void MPU9250_SPI::initAK8963() {
wngudwls000 12:f3816a0f498e 83 uint8_t rawData[3]; // x/y/z gyro calibration data stored here
wngudwls000 12:f3816a0f498e 84 replaceBlockAK(AK8963_CNTL,MGN_POWER_DN,0,4); // Power down magnetometer
wngudwls000 12:f3816a0f498e 85 wait_ms(50);
wngudwls000 12:f3816a0f498e 86 replaceBlockAK(AK8963_CNTL,MGN_FUSE_ROM,0,4);
wngudwls000 12:f3816a0f498e 87 wait_ms(50);
wngudwls000 12:f3816a0f498e 88 readAK8963Bytes( AK8963_ASAX, 3, rawData); // Read the x-, y-, and z-axis calibration values
wngudwls000 12:f3816a0f498e 89 magCalibration.x = (float)(rawData[0] - 128)/256.f + 1.f; // Return x-axis sensitivity adjustment values, etc.
wngudwls000 12:f3816a0f498e 90 magCalibration.y = (float)(rawData[1] - 128)/256.f + 1.f;
wngudwls000 12:f3816a0f498e 91 magCalibration.z = (float)(rawData[2] - 128)/256.f + 1.f;
wngudwls000 12:f3816a0f498e 92 replaceBlockAK(AK8963_CNTL,MGN_POWER_DN,0,4); // Power down magnetometer
wngudwls000 12:f3816a0f498e 93 wait_ms(50);
wngudwls000 12:f3816a0f498e 94 replaceBlockAK(AK8963_CNTL,((_mBits << 4 )| _mMode),0,5); // Set measurment mode, mMode[0:3]
wngudwls000 12:f3816a0f498e 95 writeByte(PWR_MGMT_1,CLOCK_SEL_PLL);
wngudwls000 12:f3816a0f498e 96 wait_ms(50);
wngudwls000 12:f3816a0f498e 97 mRes=10. * 4912. / 32760.0; // for Magenetometer 16BITS
wngudwls000 12:f3816a0f498e 98 }
wngudwls000 12:f3816a0f498e 99
wngudwls000 12:f3816a0f498e 100 void MPU9250_SPI::setAccelRange(AccelRange range) {
wngudwls000 12:f3816a0f498e 101 switch(range) {
wngudwls000 12:f3816a0f498e 102 case ACCEL_RANGE_2G:
wngudwls000 12:f3816a0f498e 103 aRes = 2.0f/32767.5f; break;
wngudwls000 12:f3816a0f498e 104 case ACCEL_RANGE_4G:
wngudwls000 12:f3816a0f498e 105 aRes = 4.0f/32767.5f; break;
wngudwls000 12:f3816a0f498e 106 case ACCEL_RANGE_8G:
wngudwls000 12:f3816a0f498e 107 aRes = 8.0f/32767.5f; break;
wngudwls000 12:f3816a0f498e 108 case ACCEL_RANGE_16G:
wngudwls000 12:f3816a0f498e 109 aRes = 16.0f/32767.5f; // setting the accel scale to 16G
wngudwls000 12:f3816a0f498e 110 break;
wngudwls000 12:f3816a0f498e 111 }
wngudwls000 12:f3816a0f498e 112 replaceBlock(ACCEL_CONFIG,range,3,2); // addr, value, at, size
wngudwls000 12:f3816a0f498e 113 _accelRange = range;
wngudwls000 12:f3816a0f498e 114 }
wngudwls000 12:f3816a0f498e 115 void MPU9250_SPI::setGyroRange(GyroRange range) {
wngudwls000 12:f3816a0f498e 116 switch(range) {
wngudwls000 12:f3816a0f498e 117 case GYRO_RANGE_250DPS:
wngudwls000 12:f3816a0f498e 118 gRes = 250.0f/32767.5f; break;
wngudwls000 12:f3816a0f498e 119 case GYRO_RANGE_500DPS:
wngudwls000 12:f3816a0f498e 120 gRes = 500.0f/32767.5f; break;
wngudwls000 12:f3816a0f498e 121 case GYRO_RANGE_1000DPS:
wngudwls000 12:f3816a0f498e 122 gRes = 1000.0f/32767.5f; break;
wngudwls000 12:f3816a0f498e 123 case GYRO_RANGE_2000DPS:
wngudwls000 12:f3816a0f498e 124 gRes = 2000.0f/32767.5f ; break;
wngudwls000 12:f3816a0f498e 125 }
wngudwls000 12:f3816a0f498e 126 replaceBlock(GYRO_CONFIG,range,3,2);
wngudwls000 12:f3816a0f498e 127 _gyroRange = range;
wngudwls000 12:f3816a0f498e 128 }
wngudwls000 12:f3816a0f498e 129 void MPU9250_SPI::setDlpfBandwidth(DlpfBandwidth bandwidth) {
wngudwls000 12:f3816a0f498e 130 replaceBlock(ACCEL_CONFIG2,bandwidth,0,4); //Accel DLPF [0:2]
wngudwls000 12:f3816a0f498e 131 replaceBlock(MPU_CONFIG,bandwidth,0,3); //Gyro DLPF [0:2]
wngudwls000 12:f3816a0f498e 132 _bandwidth = bandwidth;
wngudwls000 12:f3816a0f498e 133 }
wngudwls000 12:f3816a0f498e 134
wngudwls000 12:f3816a0f498e 135 void MPU9250_SPI::setSampleRate(SampleRate srd){
wngudwls000 12:f3816a0f498e 136 writeByte(SMPLRT_DIV, srd); // sampling rate set
wngudwls000 12:f3816a0f498e 137 _srd = srd;
wngudwls000 12:f3816a0f498e 138 }
wngudwls000 12:f3816a0f498e 139
wngudwls000 12:f3816a0f498e 140 void MPU9250_SPI::enableDataReadyInterrupt() {
wngudwls000 12:f3816a0f498e 141 writeByte(INT_PIN_CFG,0x00); // setup interrupt, 50 us pulse
wngudwls000 12:f3816a0f498e 142 writeByte(INT_ENABLE,0x01) ; // set to data ready
wngudwls000 12:f3816a0f498e 143 }
wngudwls000 12:f3816a0f498e 144
wngudwls000 12:f3816a0f498e 145 void MPU9250_SPI::updateSensors(){
wngudwls000 12:f3816a0f498e 146 int16_t MPU9250Data[10]; // MPU9250 accel/gyro 에서 16비트 정수로 7개 저장
wngudwls000 12:f3816a0f498e 147 uint8_t rawData[21]; // 가속도 자이로 원시 데이터 보관
wngudwls000 12:f3816a0f498e 148 writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR|SPI_READ); // Set the I2C slave addres of AK8963 and set for read.
wngudwls000 12:f3816a0f498e 149 writeByte(I2C_SLV0_REG,AK8963_XOUT_L); // I2C slave 0 register address from where to begin data transfer
wngudwls000 12:f3816a0f498e 150 writeByte(I2C_SLV0_CTRL, 0x87); // Read 7 bytes from the magnetometer
wngudwls000 12:f3816a0f498e 151 readBytes(ACCEL_XOUT_H, 21, rawData); // 16비트 정수로 7개 저장--> 14byte
wngudwls000 12:f3816a0f498e 152 MPU9250Data[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // signed 16-bit (MSB + LSB)
wngudwls000 12:f3816a0f498e 153 MPU9250Data[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;
wngudwls000 12:f3816a0f498e 154 MPU9250Data[2] = ((int16_t)rawData[4] << 8) | rawData[5] ;
wngudwls000 12:f3816a0f498e 155 MPU9250Data[3] = ((int16_t)rawData[6] << 8) | rawData[7] ;
wngudwls000 12:f3816a0f498e 156 MPU9250Data[4] = ((int16_t)rawData[8] << 8) | rawData[9] ;
wngudwls000 12:f3816a0f498e 157 MPU9250Data[5] = ((int16_t)rawData[10] << 8) | rawData[11] ;
wngudwls000 12:f3816a0f498e 158 MPU9250Data[6] = ((int16_t)rawData[12] << 8) | rawData[13] ;
wngudwls000 12:f3816a0f498e 159 MPU9250Data[7] = (((int16_t)rawData[15]) << 8) |rawData[14];
wngudwls000 12:f3816a0f498e 160 MPU9250Data[8] = (((int16_t)rawData[17]) << 8) |rawData[16];
wngudwls000 12:f3816a0f498e 161 MPU9250Data[9] = (((int16_t)rawData[19]) << 8) |rawData[18];
wngudwls000 12:f3816a0f498e 162 a.x = (float)MPU9250Data[0] * aRes - accelBias.x; // 가속도 해상도와 바이어스 보정
wngudwls000 12:f3816a0f498e 163 a.y = (float)MPU9250Data[1] * aRes - accelBias.y;
wngudwls000 12:f3816a0f498e 164 a.z = (float)MPU9250Data[2] * aRes - accelBias.z;
wngudwls000 12:f3816a0f498e 165 g.x = (float)MPU9250Data[4] * gRes - gyroBias.x; // 자이로 해상도 보정
wngudwls000 12:f3816a0f498e 166 g.y = (float)MPU9250Data[5] * gRes - gyroBias.y; // 자이로 바이어스는 칩내부에서 보정함!!!
wngudwls000 12:f3816a0f498e 167 g.z = (float)MPU9250Data[6] * gRes - gyroBias.z;
wngudwls000 12:f3816a0f498e 168 m.x = (float)(MPU9250Data[7] * mRes * magCalibration.x - magBias.x) * magScale.x;
wngudwls000 12:f3816a0f498e 169 m.y = (float)(MPU9250Data[8] * mRes * magCalibration.y - magBias.y) * magScale.y;
wngudwls000 12:f3816a0f498e 170 m.z = (float)(MPU9250Data[9] * mRes * magCalibration.z - magBias.z) * magScale.z;
wngudwls000 12:f3816a0f498e 171 }
wngudwls000 12:f3816a0f498e 172 void MPU9250_SPI::updateAccelGyro() {
wngudwls000 12:f3816a0f498e 173 int16_t MPU9250Data[7]; // MPU9250 accel/gyro 에서 16비트 정수로 7개 저장
wngudwls000 12:f3816a0f498e 174 readMPU9250Data(MPU9250Data); // 읽으면 INT 핀 해제
wngudwls000 12:f3816a0f498e 175 a.x = (float)MPU9250Data[0] * aRes - accelBias.x; // 가속도 해상도와 바이어스 보정
wngudwls000 12:f3816a0f498e 176 a.y = (float)MPU9250Data[1] * aRes - accelBias.y;
wngudwls000 12:f3816a0f498e 177 a.z = (float)MPU9250Data[2] * aRes - accelBias.z;
wngudwls000 12:f3816a0f498e 178 g.x = (float)MPU9250Data[4] * gRes - gyroBias.x; // 자이로 해상도 보정
wngudwls000 12:f3816a0f498e 179 g.y = (float)MPU9250Data[5] * gRes - gyroBias.y; //
wngudwls000 12:f3816a0f498e 180 g.z = (float)MPU9250Data[6] * gRes - gyroBias.z;
wngudwls000 12:f3816a0f498e 181 }
wngudwls000 12:f3816a0f498e 182
wngudwls000 12:f3816a0f498e 183 void MPU9250_SPI::readMPU9250Data(int16_t * destination) {
wngudwls000 12:f3816a0f498e 184 uint8_t rawData[14]; // 가속도 자이로 원시 데이터 보관
wngudwls000 12:f3816a0f498e 185 readBytes(ACCEL_XOUT_H, 14, rawData); // 16비트 정수로 7개 저장--> 14byte
wngudwls000 12:f3816a0f498e 186 destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // signed 16-bit (MSB + LSB)
wngudwls000 12:f3816a0f498e 187 destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;
wngudwls000 12:f3816a0f498e 188 destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ;
wngudwls000 12:f3816a0f498e 189 destination[3] = ((int16_t)rawData[6] << 8) | rawData[7] ;
wngudwls000 12:f3816a0f498e 190 destination[4] = ((int16_t)rawData[8] << 8) | rawData[9] ;
wngudwls000 12:f3816a0f498e 191 destination[5] = ((int16_t)rawData[10] << 8) | rawData[11] ;
wngudwls000 12:f3816a0f498e 192 destination[6] = ((int16_t)rawData[12] << 8) | rawData[13] ;
wngudwls000 12:f3816a0f498e 193 }
wngudwls000 12:f3816a0f498e 194
wngudwls000 12:f3816a0f498e 195 void MPU9250_SPI::updateMag() {
wngudwls000 12:f3816a0f498e 196 int16_t magCount[3] = {0, 0, 0}; // 16-bit 지자기 데이터
wngudwls000 12:f3816a0f498e 197 readMagData(magCount); // 지자기 데이터 읽기
wngudwls000 12:f3816a0f498e 198 // 지자기 해상도, 검정값, 바이어스 보정, 검정값 (magCalibration[] )은 칩의 ROM에서
wngudwls000 12:f3816a0f498e 199 m.x = (float)(magCount[0] * mRes * magCalibration.x - magBias.x) * magScale.x;
wngudwls000 12:f3816a0f498e 200 m.y = (float)(magCount[1] * mRes * magCalibration.y - magBias.y) * magScale.y;
wngudwls000 12:f3816a0f498e 201 m.z = (float)(magCount[2] * mRes * magCalibration.z - magBias.z) * magScale.z;
wngudwls000 12:f3816a0f498e 202 }
wngudwls000 12:f3816a0f498e 203 void MPU9250_SPI::readMagData(int16_t * destination) {
wngudwls000 12:f3816a0f498e 204 uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
wngudwls000 12:f3816a0f498e 205 if(readAK8963Byte(AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
wngudwls000 12:f3816a0f498e 206 readAK8963Bytes(AK8963_XOUT_L, 7,rawData); // Read the six raw data and ST2 registers sequentially into data array
wngudwls000 12:f3816a0f498e 207 uint8_t c = rawData[6]; // End data read by reading ST2 register
wngudwls000 12:f3816a0f498e 208 if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
wngudwls000 12:f3816a0f498e 209 destination[0] = ((int16_t)rawData[1] << 8) | rawData[0]; // Turn the MSB and LSB into a signed 16-bit value
wngudwls000 12:f3816a0f498e 210 destination[1] = ((int16_t)rawData[3] << 8) | rawData[2]; // Data stored as little Endian
wngudwls000 12:f3816a0f498e 211 destination[2] = ((int16_t)rawData[5] << 8) | rawData[4];
wngudwls000 12:f3816a0f498e 212 }
wngudwls000 12:f3816a0f498e 213 }
wngudwls000 12:f3816a0f498e 214 }
wngudwls000 12:f3816a0f498e 215
wngudwls000 12:f3816a0f498e 216 void MPU9250_SPI::writeByte(uint8_t subAddress, uint8_t data){ /* write data to device */
wngudwls000 12:f3816a0f498e 217 // _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction
wngudwls000 12:f3816a0f498e 218 // digitalWrite(_csPin,LOW); // select the MPU9250 chip
wngudwls000 12:f3816a0f498e 219 // _spi->transfer(subAddress); // write the register address
wngudwls000 12:f3816a0f498e 220 // _spi->transfer(data); // write the data
wngudwls000 12:f3816a0f498e 221 // digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
wngudwls000 12:f3816a0f498e 222 // _spi->endTransaction(); // end the transaction
wngudwls000 12:f3816a0f498e 223 _spi.frequency(SPI_LS_CLOCK); // setup clock
wngudwls000 12:f3816a0f498e 224 _csPin=0; // select the MPU9250 chip
wngudwls000 12:f3816a0f498e 225 _spi.write(subAddress); // write the register address
wngudwls000 12:f3816a0f498e 226 _spi.write(data); // write the data
wngudwls000 12:f3816a0f498e 227 _csPin=1; // deselect the MPU9250 chip
wngudwls000 12:f3816a0f498e 228 }
wngudwls000 12:f3816a0f498e 229 uint8_t MPU9250_SPI::readByte(uint8_t subAddress){
wngudwls000 12:f3816a0f498e 230 // _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3));
wngudwls000 12:f3816a0f498e 231 // digitalWrite(_csPin,LOW); // select the MPU9250 chip
wngudwls000 12:f3816a0f498e 232 // _spi->transfer(subAddress | SPI_READ); // specify the starting register address
wngudwls000 12:f3816a0f498e 233 // uint8_t data = _spi->transfer(0x00); // read the data
wngudwls000 12:f3816a0f498e 234 // digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
wngudwls000 12:f3816a0f498e 235 // _spi->endTransaction(); // end the transaction
wngudwls000 12:f3816a0f498e 236
wngudwls000 12:f3816a0f498e 237 _spi.frequency(SPI_LS_CLOCK); // setup clock
wngudwls000 12:f3816a0f498e 238 _csPin=0; // select the MPU9250 chip
wngudwls000 12:f3816a0f498e 239 _spi.write(subAddress| SPI_READ); // use READ MASK
wngudwls000 12:f3816a0f498e 240 uint8_t data =_spi.write(0); // write any to get data
wngudwls000 12:f3816a0f498e 241 _csPin=1; // deselect the MPU9250 chip
wngudwls000 12:f3816a0f498e 242 return data;
wngudwls000 12:f3816a0f498e 243 }
wngudwls000 12:f3816a0f498e 244
wngudwls000 12:f3816a0f498e 245 void MPU9250_SPI::readBytes(uint8_t subAddress, uint8_t cnt, uint8_t* dest){
wngudwls000 12:f3816a0f498e 246 // _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3));
wngudwls000 12:f3816a0f498e 247 // digitalWrite(_csPin,LOW); // select the MPU9250 chip
wngudwls000 12:f3816a0f498e 248 // _spi->transfer(subAddress | SPI_READ); // specify the starting register address
wngudwls000 12:f3816a0f498e 249 // for(uint8_t i = 0; i < count; i++){
wngudwls000 12:f3816a0f498e 250 // dest[i] = _spi->transfer(0x00); // read the data
wngudwls000 12:f3816a0f498e 251 // }
wngudwls000 12:f3816a0f498e 252 // digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
wngudwls000 12:f3816a0f498e 253 // _spi->endTransaction(); // end the transaction
wngudwls000 12:f3816a0f498e 254 _spi.frequency(SPI_HS_CLOCK); // setup clock
wngudwls000 12:f3816a0f498e 255 _csPin=0; // select the MPU9250 chip
wngudwls000 12:f3816a0f498e 256 _spi.write(subAddress | SPI_READ); // specify the starting register address
wngudwls000 12:f3816a0f498e 257 for(uint8_t i = 0; i < cnt; i++){
wngudwls000 12:f3816a0f498e 258 dest[i] = _spi.write(0x00); // read the data
wngudwls000 12:f3816a0f498e 259 }
wngudwls000 12:f3816a0f498e 260 _csPin=1; // deselect the MPU9250 chip
wngudwls000 12:f3816a0f498e 261 }
wngudwls000 12:f3816a0f498e 262
wngudwls000 12:f3816a0f498e 263 void MPU9250_SPI::writeAK8963Byte(uint8_t subAddress, uint8_t data){
wngudwls000 12:f3816a0f498e 264 writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR) ; // set slave 0 to the AK8963 and set for write
wngudwls000 12:f3816a0f498e 265 writeByte(I2C_SLV0_REG,subAddress) ; // set the register to the desired AK8963 sub address
wngudwls000 12:f3816a0f498e 266 writeByte(I2C_SLV0_DO,data) ; // store the data for write
wngudwls000 12:f3816a0f498e 267 writeByte(I2C_SLV0_CTRL,I2C_SLV0_EN | (uint8_t)1); // enable I2C and send 1 byte
wngudwls000 12:f3816a0f498e 268 }
wngudwls000 12:f3816a0f498e 269
wngudwls000 12:f3816a0f498e 270 void MPU9250_SPI::readAK8963Bytes(uint8_t subAddress, uint8_t count, uint8_t* dest){
wngudwls000 12:f3816a0f498e 271 writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG) ; // set slave 0 to the AK8963 and set for read
wngudwls000 12:f3816a0f498e 272 writeByte(I2C_SLV0_REG,subAddress) ; // set the register to the desired AK8963 sub address
wngudwls000 12:f3816a0f498e 273 writeByte(I2C_SLV0_CTRL,I2C_SLV0_EN | count); // enable I2C and request the bytes
wngudwls000 12:f3816a0f498e 274 wait_ms(1); // takes some time for these registers to fill
wngudwls000 12:f3816a0f498e 275 readBytes(EXT_SENS_DATA_00,count,dest); // read the bytes off the MPU9250 EXT_SENS_DATA registers
wngudwls000 12:f3816a0f498e 276 }
wngudwls000 12:f3816a0f498e 277
wngudwls000 12:f3816a0f498e 278 uint8_t MPU9250_SPI::readAK8963Byte(uint8_t subAddress){
wngudwls000 12:f3816a0f498e 279 writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG) ; // set slave 0 to the AK8963 and set for read
wngudwls000 12:f3816a0f498e 280 writeByte(I2C_SLV0_REG,subAddress) ; // set the register to the desired AK8963 sub address
wngudwls000 12:f3816a0f498e 281 writeByte(I2C_SLV0_CTRL,I2C_SLV0_EN | (uint8_t)1); // enable I2C and request the bytes
wngudwls000 12:f3816a0f498e 282 wait_ms(11); // takes some time for these registers to fill
wngudwls000 12:f3816a0f498e 283 return readByte(EXT_SENS_DATA_00); // read the bytes off the MPU9250 EXT_SENS_DATA registers
wngudwls000 12:f3816a0f498e 284 }
wngudwls000 12:f3816a0f498e 285 void MPU9250_SPI::replaceBlock(uint8_t address, uint8_t block, uint8_t at, uint8_t sz){
wngudwls000 12:f3816a0f498e 286 uint8_t data=readByte(address);
wngudwls000 12:f3816a0f498e 287 data &= ~(((1<<sz)-1)<<at);
wngudwls000 12:f3816a0f498e 288 data |= block<<at;
wngudwls000 12:f3816a0f498e 289 writeByte(address, data );
wngudwls000 12:f3816a0f498e 290 }
wngudwls000 12:f3816a0f498e 291 void MPU9250_SPI::replaceBlockAK(uint8_t address, uint8_t block, uint8_t at, uint8_t sz){
wngudwls000 12:f3816a0f498e 292 uint8_t data=readByte(address);
wngudwls000 12:f3816a0f498e 293 data &= ~(((1<<sz)-1)<<at);
wngudwls000 12:f3816a0f498e 294 data |= block<<at;
wngudwls000 12:f3816a0f498e 295 writeAK8963Byte(address, data );
wngudwls000 12:f3816a0f498e 296 }
wngudwls000 12:f3816a0f498e 297