semin ahn / Mbed OS zeta_stm_kinetic

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BufferedSerial

MPU9250/MPU9250_SPI.cpp@3:a4677501ae87, 2021-11-30 (annotated)

Committer:
_seminahn
Date:
Tue Nov 30 08:13:05 2021 +0000
Revision:
3:a4677501ae87
v1.2.5, change imu freq

Who changed what in which revision?

UserRevisionLine numberNew contents of line
_seminahn 3:a4677501ae87 1 /*
_seminahn 3:a4677501ae87 2 * 20201024 - (MBED code) MPU-9250 AHRS
_seminahn 3:a4677501ae87 3 *
_seminahn 3:a4677501ae87 4 */
_seminahn 3:a4677501ae87 5 #include "mbed.h"
_seminahn 3:a4677501ae87 6 #include "MPU9250_SPI.h"
_seminahn 3:a4677501ae87 7 #include "MPU9250RegisterMap.h"
_seminahn 3:a4677501ae87 8 #include <cmath>
_seminahn 3:a4677501ae87 9
_seminahn 3:a4677501ae87 10
_seminahn 3:a4677501ae87 11 //define NO_ROS 0 // need also match define in "main.cpp"
_seminahn 3:a4677501ae87 12
_seminahn 3:a4677501ae87 13
_seminahn 3:a4677501ae87 14 #if (NO_ROS) // use Serial pc only when not using ROS
_seminahn 3:a4677501ae87 15 extern Serial pc;
_seminahn 3:a4677501ae87 16 #else
_seminahn 3:a4677501ae87 17 #include <ros.h>
_seminahn 3:a4677501ae87 18 extern ros::NodeHandle nh;
_seminahn 3:a4677501ae87 19 #endif
_seminahn 3:a4677501ae87 20
_seminahn 3:a4677501ae87 21 volatile bool MPU9250_SPI::_dataReady=false;
_seminahn 3:a4677501ae87 22
_seminahn 3:a4677501ae87 23 MPU9250_SPI::MPU9250_SPI(PinName mosi,PinName miso,PinName sclk, PinName cs, PinName intpin)
_seminahn 3:a4677501ae87 24 : _spi(mosi,miso,sclk), _csPin(cs), _intPin(intpin),_mMode(MGN_CONT_MEAS2),_mBits( MGN_16BITS),_srd(SR_200HZ)
_seminahn 3:a4677501ae87 25 {
_seminahn 3:a4677501ae87 26 magCalibration.x=0;magCalibration.y=0;magCalibration.z=0;
_seminahn 3:a4677501ae87 27 magBias.x=0; magBias.y=0; magBias.z=0;
_seminahn 3:a4677501ae87 28 magScale.x=1;magScale.y=1;magScale.z=1;
_seminahn 3:a4677501ae87 29 gyroBias.x =0; gyroBias.y =0; gyroBias.z =0;
_seminahn 3:a4677501ae87 30 accelBias.x=0; accelBias.y=0; accelBias.z=0;
_seminahn 3:a4677501ae87 31 magnetic_declination = 8.5;
_seminahn 3:a4677501ae87 32 q[0]=1.0f; q[1]=0.0f; q[2]=0.0f; q[3]=0.0f;
_seminahn 3:a4677501ae87 33 _csPin=1;
_seminahn 3:a4677501ae87 34 //qFilter.begin(100);
_seminahn 3:a4677501ae87 35 }
_seminahn 3:a4677501ae87 36
_seminahn 3:a4677501ae87 37 void MPU9250_SPI::setup()
_seminahn 3:a4677501ae87 38 {
_seminahn 3:a4677501ae87 39
_seminahn 3:a4677501ae87 40 _csPin=1; // setting CS pin high
_seminahn 3:a4677501ae87 41 _spi.format(8,3); // SPI mode 3
_seminahn 3:a4677501ae87 42 _spi.frequency(SPI_HS_CLOCK); // 1Mega
_seminahn 3:a4677501ae87 43 uint8_t m_whoami = 0x00;
_seminahn 3:a4677501ae87 44 uint8_t a_whoami = 0x00;
_seminahn 3:a4677501ae87 45 // while(true){
_seminahn 3:a4677501ae87 46 // _csPin = 0; // select device
_seminahn 3:a4677501ae87 47 // _spi.write(0x75 | 0x80); // to read slave data
_seminahn 3:a4677501ae87 48 // int whoami = _spi.write(0x33); // send any byte to read
_seminahn 3:a4677501ae87 49 // _csPin = 1; // Deselect the device
_seminahn 3:a4677501ae87 50 // pc.printf("WHOAMI register = 0x%X\n", whoami);
_seminahn 3:a4677501ae87 51 // wait(0.1);
_seminahn 3:a4677501ae87 52 // }
_seminahn 3:a4677501ae87 53 m_whoami = isConnectedMPU9250();
_seminahn 3:a4677501ae87 54 if (m_whoami==MPU9250_WHOAMI_DEFAULT_VALUE) {
_seminahn 3:a4677501ae87 55 //nh.loginfo("MPU9250 is online...");
_seminahn 3:a4677501ae87 56 #ifdef PRINT_DETAILS
_seminahn 3:a4677501ae87 57 #if (NO_ROS)
_seminahn 3:a4677501ae87 58 pc.printf("MPU9250 is online...\n");
_seminahn 3:a4677501ae87 59 #else
_seminahn 3:a4677501ae87 60
_seminahn 3:a4677501ae87 61 #endif
_seminahn 3:a4677501ae87 62 #endif
_seminahn 3:a4677501ae87 63 initMPU9250();
_seminahn 3:a4677501ae87 64 a_whoami = isConnectedAK8963();
_seminahn 3:a4677501ae87 65 if (a_whoami == AK8963_WHOAMI_DEFAULT_VALUE) {
_seminahn 3:a4677501ae87 66 initAK8963();
_seminahn 3:a4677501ae87 67 }
_seminahn 3:a4677501ae87 68 else {
_seminahn 3:a4677501ae87 69 #if (NO_ROS)
_seminahn 3:a4677501ae87 70 pc.printf("Could not connect to AK8963: 0x%x\n",a_whoami);
_seminahn 3:a4677501ae87 71 #endif
_seminahn 3:a4677501ae87 72 while(1);
_seminahn 3:a4677501ae87 73 }
_seminahn 3:a4677501ae87 74 }
_seminahn 3:a4677501ae87 75 else {
_seminahn 3:a4677501ae87 76 #if (NO_ROS)
_seminahn 3:a4677501ae87 77 pc.printf("Could not connect to MPU9250: 0x%x\n",m_whoami);
_seminahn 3:a4677501ae87 78 #endif
_seminahn 3:a4677501ae87 79 while(1);
_seminahn 3:a4677501ae87 80 }
_seminahn 3:a4677501ae87 81 _intPin.rise(callback(this, &MPU9250_SPI::intService));
_seminahn 3:a4677501ae87 82 _tmr.start();
_seminahn 3:a4677501ae87 83 }
_seminahn 3:a4677501ae87 84
_seminahn 3:a4677501ae87 85 void MPU9250_SPI::update(FusionMethod method)
_seminahn 3:a4677501ae87 86 {
_seminahn 3:a4677501ae87 87 if (_dataReady){
_seminahn 3:a4677501ae87 88 updateSensors();
_seminahn 3:a4677501ae87 89 float deltat = (_tmr.read_us()/ 1000000.0f); // set integration time by time elapsed since last filter update
_seminahn 3:a4677501ae87 90 _tmr.reset();
_seminahn 3:a4677501ae87 91 if (method == MADGWICK){
_seminahn 3:a4677501ae87 92 //qFilter.update(g.x,g.y,g.z,a.x,a.y,a.z,m.x,m.y,m.z); // dot의 방향이 전진인 경우
_seminahn 3:a4677501ae87 93 //pc.printf("gx: %.3f, gy: %.3f, gz: %.3f, ax: %.3f, ay: %.3f, az: %.3f\n\r",g.x,g.y,g.z,a.x,a.y,a.z);
_seminahn 3:a4677501ae87 94 //pc.printf("%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t\n\r",g.x,g.y,g.z,a.x,a.y,a.z);
_seminahn 3:a4677501ae87 95 qFilter.invSampleFreq = deltat;
_seminahn 3:a4677501ae87 96
_seminahn 3:a4677501ae87 97
_seminahn 3:a4677501ae87 98 a.x = 0.0;
_seminahn 3:a4677501ae87 99 a.y = 0.0;
_seminahn 3:a4677501ae87 100 a.z = 1.0;
_seminahn 3:a4677501ae87 101
_seminahn 3:a4677501ae87 102
_seminahn 3:a4677501ae87 103 qFilter.updateIMU(g.x,g.y,g.z,a.x,a.y,a.z);
_seminahn 3:a4677501ae87 104 q[0] = qFilter.q0;
_seminahn 3:a4677501ae87 105 q[1] = qFilter.q1;
_seminahn 3:a4677501ae87 106 q[2] = qFilter.q2;
_seminahn 3:a4677501ae87 107 q[3] = qFilter.q3;
_seminahn 3:a4677501ae87 108 //pc.printf("q0: %.3f, q1: %.3f, q2: %.3f, q3: %.3f\n\r",q[0],q[1],q[2],q[3]);
_seminahn 3:a4677501ae87 109 //pc.printf("%.3f\t%.3f\t%.3f\t%.3f\t\n\r",q[0],q[1],q[2],q[3]);
_seminahn 3:a4677501ae87 110 updateRPY();
_seminahn 3:a4677501ae87 111 _dataReady = false;
_seminahn 3:a4677501ae87 112 }
_seminahn 3:a4677501ae87 113 else{
_seminahn 3:a4677501ae87 114 compFilter(deltat);
_seminahn 3:a4677501ae87 115 //pc.printf("i entered compFilter!!!\n\r");
_seminahn 3:a4677501ae87 116 }
_seminahn 3:a4677501ae87 117 }
_seminahn 3:a4677501ae87 118 }
_seminahn 3:a4677501ae87 119
_seminahn 3:a4677501ae87 120 void MPU9250_SPI::compFilter(float dt)
_seminahn 3:a4677501ae87 121 {
_seminahn 3:a4677501ae87 122 Vect3 _a, _g, _m;
_seminahn 3:a4677501ae87 123 _a.x= a.y; _a.y= a.x; _a.z= -a.z;
_seminahn 3:a4677501ae87 124 _g.x= DEG_TO_RAD*g.y; _g.y= DEG_TO_RAD*g.x; _g.z= -DEG_TO_RAD*g.z;
_seminahn 3:a4677501ae87 125 float ayz= sqrt(_a.y*_a.y+_a.z*_a.z);
_seminahn 3:a4677501ae87 126 float rollAcc= atan2(-_a.y,-_a.z);
_seminahn 3:a4677501ae87 127 float pitchAcc= atan2(_a.x,ayz);
_seminahn 3:a4677501ae87 128 roll= ALPHA*(roll+_g.x*dt)+BETA*rollAcc;
_seminahn 3:a4677501ae87 129 pitch= ALPHA*(pitch+_g.y*dt)+BETA*pitchAcc;
_seminahn 3:a4677501ae87 130 float c_th=cos(pitch), s_th=sin(pitch), c_pi=cos(roll), s_pi=sin(roll);
_seminahn 3:a4677501ae87 131 _m.x= m.x*c_th+m.y*s_pi*c_th+m.z*c_pi*s_th;
_seminahn 3:a4677501ae87 132 _m.y= m.y*c_pi-m.z*s_pi;
_seminahn 3:a4677501ae87 133 float heading=-atan2(_m.y, _m.x);
_seminahn 3:a4677501ae87 134 if ((yaw-heading)>M_PI) heading+=TWO_PI;
_seminahn 3:a4677501ae87 135 else if ((yaw-heading)<-M_PI) yaw+=TWO_PI;
_seminahn 3:a4677501ae87 136 yaw= ALPHA*(yaw+_g.z*dt)+BETA*heading;
_seminahn 3:a4677501ae87 137 yaw= (yaw> M_PI) ? (yaw - TWO_PI) : ((yaw < -M_PI) ? (yaw +TWO_PI) : yaw);
_seminahn 3:a4677501ae87 138 }
_seminahn 3:a4677501ae87 139
_seminahn 3:a4677501ae87 140 void MPU9250_SPI::update(Vect3& _a,Vect3& _g,Vect3& _m)
_seminahn 3:a4677501ae87 141 {
_seminahn 3:a4677501ae87 142 if (_dataReady) { // On interrupt, check if data ready interrupt
_seminahn 3:a4677501ae87 143 updateSensors();
_seminahn 3:a4677501ae87 144 _a=a;_g=g;_m=m;
_seminahn 3:a4677501ae87 145 }
_seminahn 3:a4677501ae87 146 }
_seminahn 3:a4677501ae87 147
_seminahn 3:a4677501ae87 148 uint8_t MPU9250_SPI::isConnectedMPU9250()
_seminahn 3:a4677501ae87 149 {
_seminahn 3:a4677501ae87 150 uint8_t c = readByte(WHO_AM_I_MPU9250);
_seminahn 3:a4677501ae87 151 #ifdef PRINT_DETAILS
_seminahn 3:a4677501ae87 152 pc.printf("MPU9250 WHO AM I = 0x%x\n",c);
_seminahn 3:a4677501ae87 153 #endif
_seminahn 3:a4677501ae87 154 return c; // (c == MPU9250_WHOAMI_DEFAULT_VALUE);
_seminahn 3:a4677501ae87 155 }
_seminahn 3:a4677501ae87 156
_seminahn 3:a4677501ae87 157 uint8_t MPU9250_SPI::isConnectedAK8963()
_seminahn 3:a4677501ae87 158 {
_seminahn 3:a4677501ae87 159 uint8_t c = readAK8963Byte(AK8963_WHO_AM_I);
_seminahn 3:a4677501ae87 160 #ifdef PRINT_DETAILS
_seminahn 3:a4677501ae87 161 pc.printf("AK8963 WHO AM I = 0x%x\n ",c);
_seminahn 3:a4677501ae87 162 #endif
_seminahn 3:a4677501ae87 163 return c; // (c == AK8963_WHOAMI_DEFAULT_VALUE);
_seminahn 3:a4677501ae87 164 }
_seminahn 3:a4677501ae87 165
_seminahn 3:a4677501ae87 166 void MPU9250_SPI::initMPU9250()
_seminahn 3:a4677501ae87 167 {
_seminahn 3:a4677501ae87 168 wait_ms(100);
_seminahn 3:a4677501ae87 169 writeByte(PWR_MGMT_1, CLOCK_SEL_PLL);
_seminahn 3:a4677501ae87 170 writeByte(USER_CTRL,I2C_MST_EN); // Master enable
_seminahn 3:a4677501ae87 171 writeByte(I2C_MST_CTRL,I2C_MST_CLK); // I2C master clock =400HZ
_seminahn 3:a4677501ae87 172 replaceBlockAK(AK8963_CNTL,MGN_POWER_DN,0,4); // Power down
_seminahn 3:a4677501ae87 173 writeByte(PWR_MGMT_1, PWR_RESET); // Clear sleep mode bit (6), enable all sensors
_seminahn 3:a4677501ae87 174 wait_ms(100);
_seminahn 3:a4677501ae87 175 writeByte(PWR_MGMT_1, CLOCK_SEL_PLL);
_seminahn 3:a4677501ae87 176 //setDlpfBandwidth( DLPF_BANDWIDTH_41HZ);
_seminahn 3:a4677501ae87 177 writeByte(SMPLRT_DIV, SR_100HZ); //{SR_1000HZ=0, SR_200HZ=4, SR_100HZ=9 }
_seminahn 3:a4677501ae87 178 // writeByte(SMPLRT_DIV, SR_100HZ);
_seminahn 3:a4677501ae87 179 setGyroRange(GYRO_RANGE_2000DPS);
_seminahn 3:a4677501ae87 180 writeByte(PWR_MGMT_2,SEN_ENABLE);
_seminahn 3:a4677501ae87 181 setAccelRange(ACCEL_RANGE_16G);//{ _2G, _4G, _8G, _16G }
_seminahn 3:a4677501ae87 182 setDlpfBandwidth(DLPF_BANDWIDTH_41HZ); // [250HZ, 184HZ, 92HZ, 41HZ, 20HZ, 10HZ, 5HZ]
_seminahn 3:a4677501ae87 183 writeByte(INT_PIN_CFG, 0x20); // LATCH_INT_EN=1, BYPASS_EN=1-->0 (0x22)
_seminahn 3:a4677501ae87 184 writeByte(INT_ENABLE, 0x01); // Enable raw data ready (bit 0) interrupt
_seminahn 3:a4677501ae87 185 writeByte(USER_CTRL,I2C_MST_EN);
_seminahn 3:a4677501ae87 186 wait_ms(100);
_seminahn 3:a4677501ae87 187 writeByte(I2C_MST_CTRL,I2C_MST_CLK);
_seminahn 3:a4677501ae87 188 wait_ms(100);
_seminahn 3:a4677501ae87 189 }
_seminahn 3:a4677501ae87 190
_seminahn 3:a4677501ae87 191 void MPU9250_SPI::initAK8963()
_seminahn 3:a4677501ae87 192 {
_seminahn 3:a4677501ae87 193 uint8_t rawData[3]; // x/y/z gyro calibration data stored here
_seminahn 3:a4677501ae87 194 replaceBlockAK(AK8963_CNTL,MGN_POWER_DN,0,4); // Power down magnetometer
_seminahn 3:a4677501ae87 195 wait_ms(50);
_seminahn 3:a4677501ae87 196 replaceBlockAK(AK8963_CNTL,MGN_FUSE_ROM,0,4);
_seminahn 3:a4677501ae87 197 wait_ms(50);
_seminahn 3:a4677501ae87 198 readAK8963Bytes( AK8963_ASAX, 3, rawData); // Read the x-, y-, and z-axis calibration values
_seminahn 3:a4677501ae87 199 magCalibration.x = (float)(rawData[0] - 128)/256.f + 1.f; // Return x-axis sensitivity adjustment values, etc.
_seminahn 3:a4677501ae87 200 magCalibration.y = (float)(rawData[1] - 128)/256.f + 1.f;
_seminahn 3:a4677501ae87 201 magCalibration.z = (float)(rawData[2] - 128)/256.f + 1.f;
_seminahn 3:a4677501ae87 202 replaceBlockAK(AK8963_CNTL,MGN_POWER_DN,0,4); // Power down magnetometer
_seminahn 3:a4677501ae87 203 wait_ms(50);
_seminahn 3:a4677501ae87 204 replaceBlockAK(AK8963_CNTL,((_mBits << 4 )| _mMode),0,5); // Set measurment mode, mMode[0:3]
_seminahn 3:a4677501ae87 205 writeByte(PWR_MGMT_1,CLOCK_SEL_PLL);
_seminahn 3:a4677501ae87 206 wait_ms(50);
_seminahn 3:a4677501ae87 207 mRes=10. * 4912. / 32760.0; // for Magenetometer 16BITS
_seminahn 3:a4677501ae87 208 #ifdef PRINT_DETAILS
_seminahn 3:a4677501ae87 209 pc.printf("Calibration values:\n");
_seminahn 3:a4677501ae87 210 pc.printf("X-Axis sensitivity adjustment value %7.2f \n",magCalibration.x);
_seminahn 3:a4677501ae87 211 pc.printf("Y-Axis sensitivity adjustment value %7.2f \n",magCalibration.y);
_seminahn 3:a4677501ae87 212 pc.printf("Z-Axis sensitivity adjustment value %7.2f \n",magCalibration.z);
_seminahn 3:a4677501ae87 213 pc.printf("X-Axis sensitivity offset value %7.2f \n",magBias.x);
_seminahn 3:a4677501ae87 214 pc.printf("Y-Axis sensitivity offset value %7.2f \n",magBias.y);
_seminahn 3:a4677501ae87 215 pc.printf("Z-Axis sensitivity offset value %7.2f \n",magBias.z);
_seminahn 3:a4677501ae87 216 #endif
_seminahn 3:a4677501ae87 217 }
_seminahn 3:a4677501ae87 218
_seminahn 3:a4677501ae87 219 void MPU9250_SPI::setAccelRange(AccelRange range)
_seminahn 3:a4677501ae87 220 {
_seminahn 3:a4677501ae87 221 switch(range) {
_seminahn 3:a4677501ae87 222 case ACCEL_RANGE_2G:
_seminahn 3:a4677501ae87 223 aRes = 2.0f/32767.5f; break;
_seminahn 3:a4677501ae87 224 case ACCEL_RANGE_4G:
_seminahn 3:a4677501ae87 225 aRes = 4.0f/32767.5f; break;
_seminahn 3:a4677501ae87 226 case ACCEL_RANGE_8G:
_seminahn 3:a4677501ae87 227 aRes = 8.0f/32767.5f; break;
_seminahn 3:a4677501ae87 228 case ACCEL_RANGE_16G:
_seminahn 3:a4677501ae87 229 aRes = 16.0f/32767.5f; // setting the accel scale to 16G
_seminahn 3:a4677501ae87 230 break;
_seminahn 3:a4677501ae87 231 }
_seminahn 3:a4677501ae87 232 replaceBlock(ACCEL_CONFIG,range,3,2); // addr, value, at, size
_seminahn 3:a4677501ae87 233 _accelRange = range;
_seminahn 3:a4677501ae87 234 }
_seminahn 3:a4677501ae87 235
_seminahn 3:a4677501ae87 236 void MPU9250_SPI::setGyroRange(GyroRange range)
_seminahn 3:a4677501ae87 237 {
_seminahn 3:a4677501ae87 238 switch(range) {
_seminahn 3:a4677501ae87 239 case GYRO_RANGE_250DPS:
_seminahn 3:a4677501ae87 240 gRes = 250.0f/32767.5f; break;
_seminahn 3:a4677501ae87 241 case GYRO_RANGE_500DPS:
_seminahn 3:a4677501ae87 242 gRes = 500.0f/32767.5f; break;
_seminahn 3:a4677501ae87 243 case GYRO_RANGE_1000DPS:
_seminahn 3:a4677501ae87 244 gRes = 1000.0f/32767.5f; break;
_seminahn 3:a4677501ae87 245 case GYRO_RANGE_2000DPS:
_seminahn 3:a4677501ae87 246 gRes = 2000.0f/32767.5f ; break;
_seminahn 3:a4677501ae87 247 }
_seminahn 3:a4677501ae87 248 replaceBlock(GYRO_CONFIG,range,3,2);
_seminahn 3:a4677501ae87 249 _gyroRange = range;
_seminahn 3:a4677501ae87 250 }
_seminahn 3:a4677501ae87 251
_seminahn 3:a4677501ae87 252 void MPU9250_SPI::setDlpfBandwidth(DlpfBandwidth bandwidth)
_seminahn 3:a4677501ae87 253 {
_seminahn 3:a4677501ae87 254 replaceBlock(ACCEL_CONFIG2,bandwidth,0,4); //Accel DLPF [0:2]
_seminahn 3:a4677501ae87 255 replaceBlock(MPU_CONFIG,bandwidth,0,3); //Gyro DLPF [0:2]
_seminahn 3:a4677501ae87 256 _bandwidth = bandwidth;
_seminahn 3:a4677501ae87 257 }
_seminahn 3:a4677501ae87 258
_seminahn 3:a4677501ae87 259 void MPU9250_SPI::setSampleRate(SampleRate srd)
_seminahn 3:a4677501ae87 260 {
_seminahn 3:a4677501ae87 261 writeByte(SMPLRT_DIV, srd); // sampling rate set
_seminahn 3:a4677501ae87 262 _srd = srd;
_seminahn 3:a4677501ae87 263 }
_seminahn 3:a4677501ae87 264
_seminahn 3:a4677501ae87 265 void MPU9250_SPI::calibrateMag()
_seminahn 3:a4677501ae87 266 {
_seminahn 3:a4677501ae87 267 magCalMPU9250();
_seminahn 3:a4677501ae87 268 }
_seminahn 3:a4677501ae87 269
_seminahn 3:a4677501ae87 270 void MPU9250_SPI::enableDataReadyInterrupt()
_seminahn 3:a4677501ae87 271 {
_seminahn 3:a4677501ae87 272 writeByte(INT_PIN_CFG,0x00); // setup interrupt, 50 us pulse
_seminahn 3:a4677501ae87 273 writeByte(INT_ENABLE,0x01) ; // set to data ready
_seminahn 3:a4677501ae87 274 }
_seminahn 3:a4677501ae87 275
_seminahn 3:a4677501ae87 276 void MPU9250_SPI::updateSensors()
_seminahn 3:a4677501ae87 277 {
_seminahn 3:a4677501ae87 278 int16_t MPU9250Data[10]; // MPU9250 accel/gyro 에서 16비트 정수로 7개 저장
_seminahn 3:a4677501ae87 279 uint8_t rawData[21]; // 가속도 자이로 원시 데이터 보관
_seminahn 3:a4677501ae87 280 writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR|SPI_READ); // Set the I2C slave addres of AK8963 and set for read.
_seminahn 3:a4677501ae87 281 writeByte(I2C_SLV0_REG,AK8963_XOUT_L); // I2C slave 0 register address from where to begin data transfer
_seminahn 3:a4677501ae87 282 writeByte(I2C_SLV0_CTRL, 0x87); // Read 7 bytes from the magnetometer
_seminahn 3:a4677501ae87 283 readBytes(ACCEL_XOUT_H, 21, rawData); // 16비트 정수로 7개 저장--> 14byte
_seminahn 3:a4677501ae87 284 MPU9250Data[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // signed 16-bit (MSB + LSB)
_seminahn 3:a4677501ae87 285 MPU9250Data[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;
_seminahn 3:a4677501ae87 286 MPU9250Data[2] = ((int16_t)rawData[4] << 8) | rawData[5] ;
_seminahn 3:a4677501ae87 287 MPU9250Data[3] = ((int16_t)rawData[6] << 8) | rawData[7] ;
_seminahn 3:a4677501ae87 288 MPU9250Data[4] = ((int16_t)rawData[8] << 8) | rawData[9] ;
_seminahn 3:a4677501ae87 289 MPU9250Data[5] = ((int16_t)rawData[10] << 8) | rawData[11] ;
_seminahn 3:a4677501ae87 290 MPU9250Data[6] = ((int16_t)rawData[12] << 8) | rawData[13] ;
_seminahn 3:a4677501ae87 291 MPU9250Data[7] = (((int16_t)rawData[15]) << 8) |rawData[14];
_seminahn 3:a4677501ae87 292 MPU9250Data[8] = (((int16_t)rawData[17]) << 8) |rawData[16];
_seminahn 3:a4677501ae87 293 MPU9250Data[9] = (((int16_t)rawData[19]) << 8) |rawData[18];
_seminahn 3:a4677501ae87 294 a.x = (float)MPU9250Data[0] * aRes - accelBias.x; // 가속도 해상도와 바이어스 보정
_seminahn 3:a4677501ae87 295 a.y = (float)MPU9250Data[1] * aRes - accelBias.y;
_seminahn 3:a4677501ae87 296 a.z = (float)MPU9250Data[2] * aRes - accelBias.z;
_seminahn 3:a4677501ae87 297 g.x = (float)MPU9250Data[4] * gRes- gyroBias.x; // 자이로 해상도 보정
_seminahn 3:a4677501ae87 298 g.y = (float)MPU9250Data[5] * gRes- gyroBias.y; // 자이로 바이어스는 칩내부에서 보정함!!!
_seminahn 3:a4677501ae87 299 g.z = (float)MPU9250Data[6] * gRes- gyroBias.z;
_seminahn 3:a4677501ae87 300 if(abs(g.x) < 1.0) g.x = 0.0;
_seminahn 3:a4677501ae87 301 if(abs(g.y) < 1.0) g.y = 0.0;
_seminahn 3:a4677501ae87 302 if(abs(g.z) < 1.0) g.z = 0.0;
_seminahn 3:a4677501ae87 303 m.x = (float)(MPU9250Data[7] * mRes * magCalibration.x - magBias.x) * magScale.x;
_seminahn 3:a4677501ae87 304 m.y = (float)(MPU9250Data[8] * mRes * magCalibration.y - magBias.y) * magScale.y;
_seminahn 3:a4677501ae87 305 m.z = (float)(MPU9250Data[9] * mRes * magCalibration.z - magBias.z) * magScale.z;
_seminahn 3:a4677501ae87 306 }
_seminahn 3:a4677501ae87 307
_seminahn 3:a4677501ae87 308 void MPU9250_SPI::updateAccelGyro()
_seminahn 3:a4677501ae87 309 {
_seminahn 3:a4677501ae87 310 int16_t MPU9250Data[7]; // MPU9250 accel/gyro 에서 16비트 정수로 7개 저장
_seminahn 3:a4677501ae87 311 readMPU9250Data(MPU9250Data); // 읽으면 INT 핀 해제
_seminahn 3:a4677501ae87 312 a.x = (float)MPU9250Data[0] * aRes - accelBias.x; // 가속도 해상도와 바이어스 보정
_seminahn 3:a4677501ae87 313 a.y = (float)MPU9250Data[1] * aRes - accelBias.y;
_seminahn 3:a4677501ae87 314 a.z = (float)MPU9250Data[2] * aRes - accelBias.z;
_seminahn 3:a4677501ae87 315 g.x = (float)MPU9250Data[4] * gRes - gyroBias.x; // 자이로 해상도 보정
_seminahn 3:a4677501ae87 316 g.y = (float)MPU9250Data[5] * gRes - gyroBias.y; //
_seminahn 3:a4677501ae87 317 g.z = (float)MPU9250Data[6] * gRes - gyroBias.z;
_seminahn 3:a4677501ae87 318 }
_seminahn 3:a4677501ae87 319
_seminahn 3:a4677501ae87 320 void MPU9250_SPI::readMPU9250Data(int16_t * destination)
_seminahn 3:a4677501ae87 321 {
_seminahn 3:a4677501ae87 322 uint8_t rawData[14]; // 가속도 자이로 원시 데이터 보관
_seminahn 3:a4677501ae87 323 readBytes(ACCEL_XOUT_H, 14, rawData); // 16비트 정수로 7개 저장--> 14byte
_seminahn 3:a4677501ae87 324 destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // signed 16-bit (MSB + LSB)
_seminahn 3:a4677501ae87 325 destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;
_seminahn 3:a4677501ae87 326 destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ;
_seminahn 3:a4677501ae87 327 destination[3] = ((int16_t)rawData[6] << 8) | rawData[7] ;
_seminahn 3:a4677501ae87 328 destination[4] = ((int16_t)rawData[8] << 8) | rawData[9] ;
_seminahn 3:a4677501ae87 329 destination[5] = ((int16_t)rawData[10] << 8) | rawData[11] ;
_seminahn 3:a4677501ae87 330 destination[6] = ((int16_t)rawData[12] << 8) | rawData[13] ;
_seminahn 3:a4677501ae87 331 }
_seminahn 3:a4677501ae87 332
_seminahn 3:a4677501ae87 333 void MPU9250_SPI::updateMag()
_seminahn 3:a4677501ae87 334 {
_seminahn 3:a4677501ae87 335 int16_t magCount[3] = {0, 0, 0}; // 16-bit 지자기 데이터
_seminahn 3:a4677501ae87 336 readMagData(magCount); // 지자기 데이터 읽기
_seminahn 3:a4677501ae87 337 // 지자기 해상도, 검정값, 바이어스 보정, 검정값 (magCalibration[] )은 칩의 ROM에서
_seminahn 3:a4677501ae87 338 m.x = (float)(magCount[0] * mRes * magCalibration.x - magBias.x) * magScale.x;
_seminahn 3:a4677501ae87 339 m.y = (float)(magCount[1] * mRes * magCalibration.y - magBias.y) * magScale.y;
_seminahn 3:a4677501ae87 340 m.z = (float)(magCount[2] * mRes * magCalibration.z - magBias.z) * magScale.z;
_seminahn 3:a4677501ae87 341 }
_seminahn 3:a4677501ae87 342
_seminahn 3:a4677501ae87 343 void MPU9250_SPI::readMagData(int16_t * destination)
_seminahn 3:a4677501ae87 344 {
_seminahn 3:a4677501ae87 345 uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
_seminahn 3:a4677501ae87 346 if(readAK8963Byte(AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
_seminahn 3:a4677501ae87 347 readAK8963Bytes(AK8963_XOUT_L, 7,rawData); // Read the six raw data and ST2 registers sequentially into data array
_seminahn 3:a4677501ae87 348 uint8_t c = rawData[6]; // End data read by reading ST2 register
_seminahn 3:a4677501ae87 349 if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
_seminahn 3:a4677501ae87 350 destination[0] = ((int16_t)rawData[1] << 8) | rawData[0]; // Turn the MSB and LSB into a signed 16-bit value
_seminahn 3:a4677501ae87 351 destination[1] = ((int16_t)rawData[3] << 8) | rawData[2]; // Data stored as little Endian
_seminahn 3:a4677501ae87 352 destination[2] = ((int16_t)rawData[5] << 8) | rawData[4];
_seminahn 3:a4677501ae87 353 }
_seminahn 3:a4677501ae87 354 }
_seminahn 3:a4677501ae87 355 }
_seminahn 3:a4677501ae87 356
_seminahn 3:a4677501ae87 357 void MPU9250_SPI::calibrateGyro()
_seminahn 3:a4677501ae87 358 {
_seminahn 3:a4677501ae87 359 size_t _numSamples=128;
_seminahn 3:a4677501ae87 360 Vect3 gyroSum;gyroSum.x=0;gyroSum.y=0;gyroSum.z=0;
_seminahn 3:a4677501ae87 361 // added semin
_seminahn 3:a4677501ae87 362 Vect3 accSum;accSum.x=0;accSum.y=0;accSum.z=0;
_seminahn 3:a4677501ae87 363 #if (NO_ROS)
_seminahn 3:a4677501ae87 364 pc.printf("Please, don't move!\n");
_seminahn 3:a4677501ae87 365 #endif
_seminahn 3:a4677501ae87 366 wait_ms(100);
_seminahn 3:a4677501ae87 367 for (size_t i=0; i < _numSamples; i++) {
_seminahn 3:a4677501ae87 368 updateAccelGyro();
_seminahn 3:a4677501ae87 369 gyroSum.x+= (g.x + gyroBias.x)/(( float)_numSamples);
_seminahn 3:a4677501ae87 370 gyroSum.y+= (g.y + gyroBias.y)/(( float)_numSamples);
_seminahn 3:a4677501ae87 371 gyroSum.z+= (g.z + gyroBias.z)/(( float)_numSamples);
_seminahn 3:a4677501ae87 372 accSum.x+= (a.x + accelBias.x)/(( float)_numSamples);
_seminahn 3:a4677501ae87 373 accSum.y+= (a.y + accelBias.y)/(( float)_numSamples);
_seminahn 3:a4677501ae87 374 accSum.z+= (a.z + accelBias.z)/(( float)_numSamples);
_seminahn 3:a4677501ae87 375 wait_ms(20);
_seminahn 3:a4677501ae87 376 }
_seminahn 3:a4677501ae87 377 accelBias = accSum;
_seminahn 3:a4677501ae87 378 accelBias.z = accelBias.z - 1.0;
_seminahn 3:a4677501ae87 379 gyroBias = gyroSum;
_seminahn 3:a4677501ae87 380 #if (NO_ROS)
_seminahn 3:a4677501ae87 381 pc.printf("Gyro Calibration done!\n");
_seminahn 3:a4677501ae87 382 pc.printf("MPU9250 Gyro biases (deg/s)\n");
_seminahn 3:a4677501ae87 383 pc.printf("%7.3f, %7.3f , %7.3f\n", gyroBias.x, gyroBias.y, gyroBias.z);
_seminahn 3:a4677501ae87 384 #endif
_seminahn 3:a4677501ae87 385 }
_seminahn 3:a4677501ae87 386
_seminahn 3:a4677501ae87 387 void MPU9250_SPI::magCalMPU9250()
_seminahn 3:a4677501ae87 388 {
_seminahn 3:a4677501ae87 389 uint16_t ii = 0, sample_count = 0;
_seminahn 3:a4677501ae87 390 int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0};
_seminahn 3:a4677501ae87 391 int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0};
_seminahn 3:a4677501ae87 392 #if (NO_ROS)
_seminahn 3:a4677501ae87 393 pc.printf("Mag Calibration: Wave device in a figure eight until done!\n");
_seminahn 3:a4677501ae87 394 #endif
_seminahn 3:a4677501ae87 395 wait_ms(3000);
_seminahn 3:a4677501ae87 396 if (_mMode == MGN_CONT_MEAS1) sample_count = 128; // at 8 Hz ODR, new mag data is available every 125 ms
_seminahn 3:a4677501ae87 397 else if (_mMode == MGN_CONT_MEAS2) sample_count = 1500; // at 100 Hz ODR, new mag data is available every 10 ms
_seminahn 3:a4677501ae87 398 for(ii = 0; ii < sample_count; ii++) {
_seminahn 3:a4677501ae87 399 readMagData(mag_temp); // Read the mag data
_seminahn 3:a4677501ae87 400 for (int jj = 0; jj < 3; jj++) {
_seminahn 3:a4677501ae87 401 if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
_seminahn 3:a4677501ae87 402 if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
_seminahn 3:a4677501ae87 403 }
_seminahn 3:a4677501ae87 404 if(_mMode == MGN_CONT_MEAS1) wait_ms(135); // at 8 Hz ODR, new mag data is available every 125 ms
_seminahn 3:a4677501ae87 405 if(_mMode == MGN_CONT_MEAS2) wait_ms(12); // at 100 Hz ODR, new mag data is available every 10 ms
_seminahn 3:a4677501ae87 406 }
_seminahn 3:a4677501ae87 407 #if (NO_ROS)
_seminahn 3:a4677501ae87 408 pc.printf("mag x min/max: %d /%d \n",mag_min[0],mag_max[0]);
_seminahn 3:a4677501ae87 409 pc.printf("mag y min/max: %d /%d \n",mag_min[1],mag_max[1]);
_seminahn 3:a4677501ae87 410 pc.printf("mag z min/max: %d /%d \n",mag_min[2],mag_max[2]);
_seminahn 3:a4677501ae87 411 #endif
_seminahn 3:a4677501ae87 412 // Get hard iron correction
_seminahn 3:a4677501ae87 413 mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts
_seminahn 3:a4677501ae87 414 mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts
_seminahn 3:a4677501ae87 415 mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts
_seminahn 3:a4677501ae87 416
_seminahn 3:a4677501ae87 417 magBias.x = (float) mag_bias[0]*mRes*magCalibration.x; // save mag biases in G for main program
_seminahn 3:a4677501ae87 418 magBias.y = (float) mag_bias[1]*mRes*magCalibration.y;
_seminahn 3:a4677501ae87 419 magBias.z = (float) mag_bias[2]*mRes*magCalibration.z;
_seminahn 3:a4677501ae87 420
_seminahn 3:a4677501ae87 421 // Get soft iron correction estimate
_seminahn 3:a4677501ae87 422 mag_scale[0] = (mag_max[0] - mag_min[0])/2; // get average x axis max chord length in counts
_seminahn 3:a4677501ae87 423 mag_scale[1] = (mag_max[1] - mag_min[1])/2; // get average y axis max chord length in counts
_seminahn 3:a4677501ae87 424 mag_scale[2] = (mag_max[2] - mag_min[2])/2; // get average z axis max chord length in counts
_seminahn 3:a4677501ae87 425
_seminahn 3:a4677501ae87 426 float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
_seminahn 3:a4677501ae87 427 avg_rad /= 3.0;
_seminahn 3:a4677501ae87 428
_seminahn 3:a4677501ae87 429 magScale.x = avg_rad/((float)mag_scale[0]);
_seminahn 3:a4677501ae87 430 magScale.y = avg_rad/((float)mag_scale[1]);
_seminahn 3:a4677501ae87 431 magScale.z = avg_rad/((float)mag_scale[2]);
_seminahn 3:a4677501ae87 432 #if (NO_ROS)
_seminahn 3:a4677501ae87 433 pc.printf("Mag Calibration done!\n");
_seminahn 3:a4677501ae87 434 pc.printf("AK8963 mag biases (mG)\_numSamples\n");
_seminahn 3:a4677501ae87 435 pc.printf("%7.3f, %7.3f , %7.3f\n", magBias.x, magBias.y, magBias.z);
_seminahn 3:a4677501ae87 436 pc.printf("AK8963 mag scale (mG)\n");
_seminahn 3:a4677501ae87 437 pc.printf("%7.3f, %7.3f , %7.3f\n", magScale.x, magScale.y,magScale.z);
_seminahn 3:a4677501ae87 438 #endif
_seminahn 3:a4677501ae87 439 }
_seminahn 3:a4677501ae87 440
_seminahn 3:a4677501ae87 441 /* write data to device */
_seminahn 3:a4677501ae87 442 void MPU9250_SPI::writeByte(uint8_t subAddress, uint8_t data)
_seminahn 3:a4677501ae87 443 {
_seminahn 3:a4677501ae87 444 // _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction
_seminahn 3:a4677501ae87 445 // digitalWrite(_csPin,LOW); // select the MPU9250 chip
_seminahn 3:a4677501ae87 446 // _spi->transfer(subAddress); // write the register address
_seminahn 3:a4677501ae87 447 // _spi->transfer(data); // write the data
_seminahn 3:a4677501ae87 448 // digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
_seminahn 3:a4677501ae87 449 // _spi->endTransaction(); // end the transaction
_seminahn 3:a4677501ae87 450 _spi.frequency(SPI_LS_CLOCK); // setup clock
_seminahn 3:a4677501ae87 451 _csPin=0; // select the MPU9250 chip
_seminahn 3:a4677501ae87 452 _spi.write(subAddress); // write the register address
_seminahn 3:a4677501ae87 453 _spi.write(data); // write the data
_seminahn 3:a4677501ae87 454 _csPin=1; // deselect the MPU9250 chip
_seminahn 3:a4677501ae87 455 }
_seminahn 3:a4677501ae87 456
_seminahn 3:a4677501ae87 457 uint8_t MPU9250_SPI::readByte(uint8_t subAddress)
_seminahn 3:a4677501ae87 458 {
_seminahn 3:a4677501ae87 459 // _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3));
_seminahn 3:a4677501ae87 460 // digitalWrite(_csPin,LOW); // select the MPU9250 chip
_seminahn 3:a4677501ae87 461 // _spi->transfer(subAddress | SPI_READ); // specify the starting register address
_seminahn 3:a4677501ae87 462 // uint8_t data = _spi->transfer(0x00); // read the data
_seminahn 3:a4677501ae87 463 // digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
_seminahn 3:a4677501ae87 464 // _spi->endTransaction(); // end the transaction
_seminahn 3:a4677501ae87 465
_seminahn 3:a4677501ae87 466 _spi.frequency(SPI_LS_CLOCK); // setup clock
_seminahn 3:a4677501ae87 467 _csPin=0; // select the MPU9250 chip
_seminahn 3:a4677501ae87 468 _spi.write(subAddress| SPI_READ); // use READ MASK
_seminahn 3:a4677501ae87 469 uint8_t data =_spi.write(0); // write any to get data
_seminahn 3:a4677501ae87 470 _csPin=1; // deselect the MPU9250 chip
_seminahn 3:a4677501ae87 471 return data;
_seminahn 3:a4677501ae87 472 }
_seminahn 3:a4677501ae87 473
_seminahn 3:a4677501ae87 474 void MPU9250_SPI::readBytes(uint8_t subAddress, uint8_t cnt, uint8_t* dest)
_seminahn 3:a4677501ae87 475 {
_seminahn 3:a4677501ae87 476 // _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3));
_seminahn 3:a4677501ae87 477 // digitalWrite(_csPin,LOW); // select the MPU9250 chip
_seminahn 3:a4677501ae87 478 // _spi->transfer(subAddress | SPI_READ); // specify the starting register address
_seminahn 3:a4677501ae87 479 // for(uint8_t i = 0; i < count; i++){
_seminahn 3:a4677501ae87 480 // dest[i] = _spi->transfer(0x00); // read the data
_seminahn 3:a4677501ae87 481 // }
_seminahn 3:a4677501ae87 482 // digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
_seminahn 3:a4677501ae87 483 // _spi->endTransaction(); // end the transaction
_seminahn 3:a4677501ae87 484 _spi.frequency(SPI_HS_CLOCK); // setup clock
_seminahn 3:a4677501ae87 485 _csPin=0; // select the MPU9250 chip
_seminahn 3:a4677501ae87 486 _spi.write(subAddress | SPI_READ); // specify the starting register address
_seminahn 3:a4677501ae87 487 for(uint8_t i = 0; i < cnt; i++){
_seminahn 3:a4677501ae87 488 dest[i] = _spi.write(0x00); // read the data
_seminahn 3:a4677501ae87 489 }
_seminahn 3:a4677501ae87 490 _csPin=1; // deselect the MPU9250 chip
_seminahn 3:a4677501ae87 491 }
_seminahn 3:a4677501ae87 492
_seminahn 3:a4677501ae87 493 void MPU9250_SPI::writeAK8963Byte(uint8_t subAddress, uint8_t data)
_seminahn 3:a4677501ae87 494 {
_seminahn 3:a4677501ae87 495 writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR) ; // set slave 0 to the AK8963 and set for write
_seminahn 3:a4677501ae87 496 writeByte(I2C_SLV0_REG,subAddress) ; // set the register to the desired AK8963 sub address
_seminahn 3:a4677501ae87 497 writeByte(I2C_SLV0_DO,data) ; // store the data for write
_seminahn 3:a4677501ae87 498 writeByte(I2C_SLV0_CTRL,I2C_SLV0_EN | (uint8_t)1); // enable I2C and send 1 byte
_seminahn 3:a4677501ae87 499 }
_seminahn 3:a4677501ae87 500
_seminahn 3:a4677501ae87 501 void MPU9250_SPI::readAK8963Bytes(uint8_t subAddress, uint8_t count, uint8_t* dest)
_seminahn 3:a4677501ae87 502 {
_seminahn 3:a4677501ae87 503 writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG) ; // set slave 0 to the AK8963 and set for read
_seminahn 3:a4677501ae87 504 writeByte(I2C_SLV0_REG,subAddress) ; // set the register to the desired AK8963 sub address
_seminahn 3:a4677501ae87 505 writeByte(I2C_SLV0_CTRL,I2C_SLV0_EN | count); // enable I2C and request the bytes
_seminahn 3:a4677501ae87 506 wait_ms(1); // takes some time for these registers to fill
_seminahn 3:a4677501ae87 507 readBytes(EXT_SENS_DATA_00,count,dest); // read the bytes off the MPU9250 EXT_SENS_DATA registers
_seminahn 3:a4677501ae87 508 }
_seminahn 3:a4677501ae87 509
_seminahn 3:a4677501ae87 510 uint8_t MPU9250_SPI::readAK8963Byte(uint8_t subAddress)
_seminahn 3:a4677501ae87 511 {
_seminahn 3:a4677501ae87 512 writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG) ; // set slave 0 to the AK8963 and set for read
_seminahn 3:a4677501ae87 513 writeByte(I2C_SLV0_REG,subAddress) ; // set the register to the desired AK8963 sub address
_seminahn 3:a4677501ae87 514 writeByte(I2C_SLV0_CTRL,I2C_SLV0_EN | (uint8_t)1); // enable I2C and request the bytes
_seminahn 3:a4677501ae87 515 wait_ms(11); // takes some time for these registers to fill
_seminahn 3:a4677501ae87 516 return readByte(EXT_SENS_DATA_00); // read the bytes off the MPU9250 EXT_SENS_DATA registers
_seminahn 3:a4677501ae87 517 }
_seminahn 3:a4677501ae87 518
_seminahn 3:a4677501ae87 519 void MPU9250_SPI::replaceBlock(uint8_t address, uint8_t block, uint8_t at, uint8_t sz)
_seminahn 3:a4677501ae87 520 {
_seminahn 3:a4677501ae87 521 uint8_t data=readByte(address);
_seminahn 3:a4677501ae87 522 data &= ~(((1<<sz)-1)<<at);
_seminahn 3:a4677501ae87 523 data |= block<<at;
_seminahn 3:a4677501ae87 524 writeByte(address, data );
_seminahn 3:a4677501ae87 525 }
_seminahn 3:a4677501ae87 526
_seminahn 3:a4677501ae87 527 void MPU9250_SPI::replaceBlockAK(uint8_t address, uint8_t block, uint8_t at, uint8_t sz)
_seminahn 3:a4677501ae87 528 {
_seminahn 3:a4677501ae87 529 uint8_t data=readByte(address);
_seminahn 3:a4677501ae87 530 data &= ~(((1<<sz)-1)<<at);
_seminahn 3:a4677501ae87 531 data |= block<<at;
_seminahn 3:a4677501ae87 532 writeAK8963Byte(address, data );
_seminahn 3:a4677501ae87 533 }
_seminahn 3:a4677501ae87 534
_seminahn 3:a4677501ae87 535 void MPU9250_SPI::updateRPY() // RPY : Roll Pitch Yaw
_seminahn 3:a4677501ae87 536 {
_seminahn 3:a4677501ae87 537 a12 = 2.0f * (q[1] * q[2] + q[0] * q[3]);
_seminahn 3:a4677501ae87 538 a22 = q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3];
_seminahn 3:a4677501ae87 539 a31 = 2.0f * (q[0] * q[1] + q[2] * q[3]);
_seminahn 3:a4677501ae87 540 a32 = 2.0f * (q[1] * q[3] - q[0] * q[2]);
_seminahn 3:a4677501ae87 541 a33 = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
_seminahn 3:a4677501ae87 542 pitch = -asinf(a32);
_seminahn 3:a4677501ae87 543 roll = atan2f(a31, a33);
_seminahn 3:a4677501ae87 544 yaw = atan2f(a12, a22);
_seminahn 3:a4677501ae87 545 yaw += magnetic_declination*DEG_TO_RAD;
_seminahn 3:a4677501ae87 546 yaw= (yaw> M_PI) ? (yaw - TWO_PI) : ((yaw < - M_PI) ? (yaw +TWO_PI) : yaw);
_seminahn 3:a4677501ae87 547 }
_seminahn 3:a4677501ae87 548
_seminahn 3:a4677501ae87 549
_seminahn 3:a4677501ae87 550 /***** EOF *****/