Alvee Ahmed / virgo3_imuHandler

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.

Fork of virgo3_imuHandler_Orion_PCB by Van Nguyen

imuHandler.cpp@10:31eee5f90d04, 2016-06-16 (annotated)

Committer:
akashvibhute
Date:
Thu Jun 16 03:27:34 2016 +0000
Revision:
10:31eee5f90d04
Parent:
9:47b6a8530868
Child:
11:fb47aa30cc7b
Child:
12:6ec427e47641
added mag read functions to mpu9150 lib

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akashvibhute 9:47b6a8530868 1 /*
akashvibhute 9:47b6a8530868 2 * Copyright (C) 2016 Akash Vibhute <akash.roboticist@gmail.com>
akashvibhute 9:47b6a8530868 3 *
akashvibhute 9:47b6a8530868 4 * Wrapper to handle all quirks of MPU9250 and BNO055 IMU on Virgo 3 robot.
akashvibhute 9:47b6a8530868 5 * Based off various user libraries
akashvibhute 9:47b6a8530868 6 *
akashvibhute 9:47b6a8530868 7 *
akashvibhute 9:47b6a8530868 8 * Initial Release: Apr/26/2016
akashvibhute 9:47b6a8530868 9 *
akashvibhute 9:47b6a8530868 10 */
akashvibhute 1:927f1b3e4bf4 11
akashvibhute 0:88de7b3bd889 12 #include "imuHandler.h"
akashvibhute 0:88de7b3bd889 13
akashvibhute 0:88de7b3bd889 14 /*** MPU6050 ***/
akashvibhute 0:88de7b3bd889 15
akashvibhute 0:88de7b3bd889 16 IMU_MPU6050::IMU_MPU6050(): imu_mpu6050(i2c_SDA, i2c_SCL)
akashvibhute 0:88de7b3bd889 17 {
akashvibhute 0:88de7b3bd889 18 unsigned int movWindowSize_Pose = Pose_MWindowSize;
akashvibhute 0:88de7b3bd889 19 unsigned int movWindowSize_GyroAcc = GyroAcc_MWindowSize;
akashvibhute 10:31eee5f90d04 20 unsigned int movWindowSize_Mag = Mag_MWindowSize;
akashvibhute 10:31eee5f90d04 21
akashvibhute 0:88de7b3bd889 22 unstable_readings = MPU6050_StabilizationReadings;
akashvibhute 10:31eee5f90d04 23
akashvibhute 10:31eee5f90d04 24 imu_stabilized[0]=0;
akashvibhute 0:88de7b3bd889 25
akashvibhute 0:88de7b3bd889 26 if(movWindowSize_Pose <= movWindow_lenMax) movWindow_len_Pose = movWindowSize_Pose;
akashvibhute 0:88de7b3bd889 27 else movWindow_len_Pose = movWindow_lenMax;
akashvibhute 0:88de7b3bd889 28
akashvibhute 0:88de7b3bd889 29 if(movWindow_len_GyroAcc <= movWindow_lenMax) movWindow_len_GyroAcc = movWindowSize_GyroAcc;
akashvibhute 0:88de7b3bd889 30 else movWindow_len_GyroAcc = movWindow_lenMax;
akashvibhute 10:31eee5f90d04 31
akashvibhute 10:31eee5f90d04 32 if(movWindowSize_Mag <= movWindow_lenMax) movWindow_len_Mag = movWindowSize_Mag;
akashvibhute 10:31eee5f90d04 33 else movWindow_len_Mag = movWindow_lenMax;
akashvibhute 0:88de7b3bd889 34
akashvibhute 0:88de7b3bd889 35 movWindow_index_Pose=0;
akashvibhute 0:88de7b3bd889 36 movWindow_index_GyroAcc=0;
akashvibhute 10:31eee5f90d04 37 movWindow_index_Mag=0;
akashvibhute 10:31eee5f90d04 38
akashvibhute 10:31eee5f90d04 39 enable_mag =0; //disable magnetometer reading by default
akashvibhute 10:31eee5f90d04 40 mag_time =0;
akashvibhute 0:88de7b3bd889 41
akashvibhute 0:88de7b3bd889 42 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 43 PoseCorrection[i]=0;
akashvibhute 0:88de7b3bd889 44 }
akashvibhute 0:88de7b3bd889 45
akashvibhute 0:88de7b3bd889 46 bool IMU_MPU6050::imuInit()
akashvibhute 0:88de7b3bd889 47 {
akashvibhute 0:88de7b3bd889 48 //debug_printf("MPU6050 initializing.\n");
akashvibhute 0:88de7b3bd889 49
akashvibhute 0:88de7b3bd889 50 imu_mpu6050.initialize();
akashvibhute 0:88de7b3bd889 51 if (imu_mpu6050.testConnection()) {
akashvibhute 0:88de7b3bd889 52 //debug_printf("MPU6050 test connection passed.\n");
akashvibhute 0:88de7b3bd889 53 } else {
akashvibhute 0:88de7b3bd889 54 //debug_printf("MPU6050 test connection failed.\n");
akashvibhute 0:88de7b3bd889 55 return false;
akashvibhute 0:88de7b3bd889 56 }
akashvibhute 0:88de7b3bd889 57 if (imu_mpu6050.dmpInitialize() == 0) {
akashvibhute 0:88de7b3bd889 58 //debug_printf("succeed in MPU6050 DMP Initializing.\n");
akashvibhute 0:88de7b3bd889 59 } else {
akashvibhute 0:88de7b3bd889 60 //debug_printf("failed in MPU6050 DMP Initializing.\n");
akashvibhute 0:88de7b3bd889 61 return false;
akashvibhute 0:88de7b3bd889 62 }
akashvibhute 0:88de7b3bd889 63 imu_mpu6050.setXAccelOffset(MPU6050_Offset_Ax);
akashvibhute 0:88de7b3bd889 64 imu_mpu6050.setYAccelOffset(MPU6050_Offset_Ay);
akashvibhute 0:88de7b3bd889 65 imu_mpu6050.setZAccelOffset(MPU6050_Offset_Az);
akashvibhute 0:88de7b3bd889 66 imu_mpu6050.setFullScaleGyroRange(MPU6050_GYRO_FS);
akashvibhute 0:88de7b3bd889 67 imu_mpu6050.setFullScaleAccelRange(MPU6050_ACCEL_FS);
akashvibhute 0:88de7b3bd889 68 imu_mpu6050.setXGyroOffsetUser(MPU6050_Offset_Gx);
akashvibhute 0:88de7b3bd889 69 imu_mpu6050.setYGyroOffsetUser(MPU6050_Offset_Gy);
akashvibhute 0:88de7b3bd889 70 imu_mpu6050.setZGyroOffsetUser(MPU6050_Offset_Gz);
akashvibhute 0:88de7b3bd889 71 imu_mpu6050.setDMPEnabled(true); // Enable DMP
akashvibhute 0:88de7b3bd889 72 packetSize = imu_mpu6050.dmpGetFIFOPacketSize();
akashvibhute 0:88de7b3bd889 73
akashvibhute 0:88de7b3bd889 74 //debug_printf("Init finish!\n");
akashvibhute 0:88de7b3bd889 75
akashvibhute 0:88de7b3bd889 76 return true;
akashvibhute 0:88de7b3bd889 77 }
akashvibhute 0:88de7b3bd889 78
akashvibhute 0:88de7b3bd889 79 void IMU_MPU6050::imuUpdate()
akashvibhute 0:88de7b3bd889 80 {
akashvibhute 0:88de7b3bd889 81 mpuIntStatus = imu_mpu6050.getIntStatus();
akashvibhute 0:88de7b3bd889 82 fifoCount = imu_mpu6050.getFIFOCount();
akashvibhute 0:88de7b3bd889 83
akashvibhute 0:88de7b3bd889 84 // Check that this interrupt is a FIFO buffer overflow interrupt.
akashvibhute 0:88de7b3bd889 85 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
akashvibhute 0:88de7b3bd889 86
akashvibhute 10:31eee5f90d04 87 imu_mpu6050.resetFIFO();
akashvibhute 10:31eee5f90d04 88 /*debug_printf("FIFO overflow!\n");*/
akashvibhute 0:88de7b3bd889 89 return;
akashvibhute 0:88de7b3bd889 90
akashvibhute 0:88de7b3bd889 91 // Check that this interrupt is a Data Ready interrupt.
akashvibhute 0:88de7b3bd889 92 }
akashvibhute 0:88de7b3bd889 93
akashvibhute 0:88de7b3bd889 94 else if (mpuIntStatus & 0x02) {
akashvibhute 0:88de7b3bd889 95 while (fifoCount < packetSize) fifoCount = imu_mpu6050.getFIFOCount();
akashvibhute 0:88de7b3bd889 96
akashvibhute 0:88de7b3bd889 97 if(unstable_readings >= 1) {
akashvibhute 0:88de7b3bd889 98
akashvibhute 0:88de7b3bd889 99 imu_mpu6050.getFIFOBytes(fifoBuffer, packetSize);
akashvibhute 0:88de7b3bd889 100
akashvibhute 0:88de7b3bd889 101 imu_mpu6050.dmpGetQuaternion(&imu_Data.q, fifoBuffer);
akashvibhute 0:88de7b3bd889 102 imu_mpu6050.dmpGetGravity(&imu_Data.gravity, &imu_Data.q);
akashvibhute 0:88de7b3bd889 103
akashvibhute 0:88de7b3bd889 104 imu_mpu6050.dmpGetAccel(&imu_Data.aa, fifoBuffer);
akashvibhute 0:88de7b3bd889 105 imu_mpu6050.dmpGetLinearAccel(&imu_Data.aaReal, &imu_Data.aa, &imu_Data.gravity);
akashvibhute 0:88de7b3bd889 106
akashvibhute 0:88de7b3bd889 107 imu_mpu6050.dmpGetLinearAccelInWorld(&imu_Data.aaWorld, &imu_Data.aaReal, &imu_Data.q);
akashvibhute 0:88de7b3bd889 108
akashvibhute 0:88de7b3bd889 109 if((generalFunctions::abs_f(imu_Data.aaWorld.x) <= 8) && (generalFunctions::abs_f(imu_Data.aaWorld.y) <= 8)) {
akashvibhute 0:88de7b3bd889 110 if(unstable_readings > 1)
akashvibhute 0:88de7b3bd889 111 unstable_readings --;
akashvibhute 0:88de7b3bd889 112 else {
akashvibhute 0:88de7b3bd889 113 imu_mpu6050.dmpGetYawPitchRoll(imu_initialAngles, &imu_Data.q, &imu_Data.gravity);
akashvibhute 0:88de7b3bd889 114 mpu_timer.start();
akashvibhute 0:88de7b3bd889 115 unstable_readings --;
akashvibhute 0:88de7b3bd889 116
akashvibhute 10:31eee5f90d04 117 imu_stabilized[0] = 1;
akashvibhute 0:88de7b3bd889 118 //debug_printf("\nIMU stabilized\n");
akashvibhute 0:88de7b3bd889 119 }
akashvibhute 0:88de7b3bd889 120 }
akashvibhute 0:88de7b3bd889 121 }
akashvibhute 0:88de7b3bd889 122
akashvibhute 0:88de7b3bd889 123 else {
akashvibhute 0:88de7b3bd889 124 float yawPitchRoll[3];
akashvibhute 0:88de7b3bd889 125 int16_t raw_gyro[3];
akashvibhute 0:88de7b3bd889 126 imu_mpu6050.getFIFOBytes(fifoBuffer, packetSize);
akashvibhute 0:88de7b3bd889 127 imu_mpu6050.dmpGetQuaternion(&imu_Data.q, fifoBuffer);
akashvibhute 0:88de7b3bd889 128 imu_mpu6050.dmpGetGravity(&imu_Data.gravity, &imu_Data.q);
akashvibhute 0:88de7b3bd889 129 imu_mpu6050.dmpGetYawPitchRoll(yawPitchRoll, &imu_Data.q, &imu_Data.gravity);
akashvibhute 10:31eee5f90d04 130
akashvibhute 10:31eee5f90d04 131 if((enable_mag == 1) && ((time_s - mag_time)*1000 >= mpu_mag_readms) ){
akashvibhute 10:31eee5f90d04 132 int16_t mx,my,mz;
akashvibhute 10:31eee5f90d04 133
akashvibhute 10:31eee5f90d04 134 imu_mpu6050.getMag(&mx, &my, &mz);
akashvibhute 10:31eee5f90d04 135 mag_time = time_s;
akashvibhute 10:31eee5f90d04 136
akashvibhute 10:31eee5f90d04 137 imu_Data.magnetometer[0] = (float)mx /10.0;
akashvibhute 10:31eee5f90d04 138 imu_Data.magnetometer[1] = (float)my /10.0;
akashvibhute 10:31eee5f90d04 139 imu_Data.magnetometer[2] = (float)mz /10.0;
akashvibhute 10:31eee5f90d04 140
akashvibhute 10:31eee5f90d04 141 //moving window average of pose angle
akashvibhute 10:31eee5f90d04 142 for(int i=0; i<3; i++) {
akashvibhute 10:31eee5f90d04 143 movWindow_Mag[i][movWindow_index_Mag] = imu_Data.magnetometer[i];
akashvibhute 10:31eee5f90d04 144 Mag[i] = generalFunctions::moving_window(movWindow_Mag[i], movWindow_len_Mag);
akashvibhute 10:31eee5f90d04 145 }
akashvibhute 10:31eee5f90d04 146
akashvibhute 10:31eee5f90d04 147 }
akashvibhute 10:31eee5f90d04 148
akashvibhute 0:88de7b3bd889 149
akashvibhute 0:88de7b3bd889 150 //debug_printf("DEBUG>> got YPR data\n");
akashvibhute 0:88de7b3bd889 151
akashvibhute 0:88de7b3bd889 152 if(mpu_timer.read() > 2100) { //reset as timer is close to overflow
akashvibhute 0:88de7b3bd889 153 mpu_timer.reset();
akashvibhute 0:88de7b3bd889 154 imuTime_s[0] = imuTime_s[1];
akashvibhute 0:88de7b3bd889 155 } else
akashvibhute 0:88de7b3bd889 156 imuTime_s[1] = imuTime_s[0] + mpu_timer.read();
akashvibhute 0:88de7b3bd889 157
akashvibhute 0:88de7b3bd889 158 time_s = imuTime_s[1]; //imu timestamp
akashvibhute 0:88de7b3bd889 159
akashvibhute 0:88de7b3bd889 160 imu_Data.posePRY[2] = -1*(yawPitchRoll[0] - imu_initialAngles[0]- DRIFT_CORRECTION(imuTime_s[1])) - PoseCorrection[0];
akashvibhute 0:88de7b3bd889 161 imu_Data.posePRY[1] = yawPitchRoll[1] - imu_initialAngles[1] - PoseCorrection[1];
akashvibhute 0:88de7b3bd889 162 imu_Data.posePRY[0] = yawPitchRoll[2] - imu_initialAngles[2] - PoseCorrection[2];
akashvibhute 0:88de7b3bd889 163
akashvibhute 0:88de7b3bd889 164 //convert angles to 0 to 2pi range
akashvibhute 0:88de7b3bd889 165 for(int i=0; i<3; i++) {
akashvibhute 10:31eee5f90d04 166 if(imu_Data.posePRY[i] > 2*RAD_TO_DEG(M_PI))
akashvibhute 10:31eee5f90d04 167 imu_Data.posePRY[i] -= 2*RAD_TO_DEG(M_PI);
akashvibhute 10:31eee5f90d04 168 if(imu_Data.posePRY[i] < 0.0)
akashvibhute 10:31eee5f90d04 169 imu_Data.posePRY[i] += 2*RAD_TO_DEG(M_PI);
akashvibhute 0:88de7b3bd889 170 }
akashvibhute 0:88de7b3bd889 171
akashvibhute 10:31eee5f90d04 172
akashvibhute 0:88de7b3bd889 173 //moving window average of pose angle
akashvibhute 0:88de7b3bd889 174 for(int i=0; i<3; i++) {
akashvibhute 10:31eee5f90d04 175 movWindow_Pose[i][movWindow_index_Pose] = sin(DEG_TO_RAD(imu_Data.posePRY[i]));
akashvibhute 10:31eee5f90d04 176 Pose[i] = RAD_TO_DEG(asin(generalFunctions::moving_window(movWindow_Pose[i], movWindow_len_Pose)));
akashvibhute 10:31eee5f90d04 177 }
akashvibhute 10:31eee5f90d04 178
akashvibhute 10:31eee5f90d04 179 //convert angles to 0 to 2pi range
akashvibhute 10:31eee5f90d04 180 for(int i=0; i<3; i++) {
akashvibhute 10:31eee5f90d04 181 if(Pose[i] > 2*RAD_TO_DEG(M_PI))
akashvibhute 10:31eee5f90d04 182 Pose[i] -= 2*RAD_TO_DEG(M_PI);
akashvibhute 10:31eee5f90d04 183 if(Pose[i] < 0.0)
akashvibhute 10:31eee5f90d04 184 Pose[i] += 2*RAD_TO_DEG(M_PI);
akashvibhute 0:88de7b3bd889 185 }
akashvibhute 0:88de7b3bd889 186
akashvibhute 0:88de7b3bd889 187 /** Todo: offset->filter quaternion, use this quaternion to generate gravity vector, and consequently aaReal and aaWorld **/
akashvibhute 0:88de7b3bd889 188
akashvibhute 0:88de7b3bd889 189 //convert filtered pose to quaternion here
akashvibhute 0:88de7b3bd889 190
akashvibhute 0:88de7b3bd889 191 Quat[0]= imu_Data.q.w;
akashvibhute 0:88de7b3bd889 192 Quat[1]= imu_Data.q.x;
akashvibhute 0:88de7b3bd889 193 Quat[2]= imu_Data.q.y;
akashvibhute 0:88de7b3bd889 194 Quat[3]= imu_Data.q.z;
akashvibhute 0:88de7b3bd889 195
akashvibhute 0:88de7b3bd889 196 //imu_mpu6050.dmpGetGravity(&imu_Data.gravity, &imu_Data.q /* filtered quaternion */);
akashvibhute 0:88de7b3bd889 197
akashvibhute 0:88de7b3bd889 198 /***************************************/
akashvibhute 0:88de7b3bd889 199
akashvibhute 0:88de7b3bd889 200 imu_mpu6050.dmpGetAccel(&imu_Data.aa, fifoBuffer);
akashvibhute 0:88de7b3bd889 201 imu_mpu6050.dmpGetGyro(raw_gyro, fifoBuffer);
akashvibhute 0:88de7b3bd889 202 imu_mpu6050.dmpGetLinearAccel(&imu_Data.aaReal, &imu_Data.aa, &imu_Data.gravity);
akashvibhute 0:88de7b3bd889 203 imu_mpu6050.dmpGetLinearAccelInWorld(&imu_Data.aaWorld, &imu_Data.aaReal, &imu_Data.q);
akashvibhute 0:88de7b3bd889 204
akashvibhute 0:88de7b3bd889 205 imu_Data.accW.x = (float) imu_Data.aaWorld.x * (9806.0/8192);
akashvibhute 0:88de7b3bd889 206 imu_Data.accW.y = (float) imu_Data.aaWorld.y * (9806.0/8192);
akashvibhute 0:88de7b3bd889 207 imu_Data.accW.z = (float) imu_Data.aaWorld.z * (9806.0/8192);
akashvibhute 0:88de7b3bd889 208
akashvibhute 0:88de7b3bd889 209 imu_Data.gyro.x = (float) raw_gyro[0];
akashvibhute 0:88de7b3bd889 210 imu_Data.gyro.y = (float) raw_gyro[1];
akashvibhute 0:88de7b3bd889 211 imu_Data.gyro.z = (float) raw_gyro[2];
akashvibhute 0:88de7b3bd889 212
akashvibhute 0:88de7b3bd889 213
akashvibhute 4:599d5ac25b73 214
akashvibhute 0:88de7b3bd889 215 //moving window average of gyro velocities
akashvibhute 0:88de7b3bd889 216 movWindow_AngVel[0][movWindow_index_GyroAcc] = imu_Data.gyro.x;
akashvibhute 0:88de7b3bd889 217 movWindow_AngVel[1][movWindow_index_GyroAcc] = imu_Data.gyro.y;
akashvibhute 0:88de7b3bd889 218 movWindow_AngVel[2][movWindow_index_GyroAcc] = imu_Data.gyro.z;
akashvibhute 0:88de7b3bd889 219
akashvibhute 0:88de7b3bd889 220 movWindow_LinAcc[0][movWindow_index_GyroAcc] = imu_Data.accW.x;
akashvibhute 0:88de7b3bd889 221 movWindow_LinAcc[1][movWindow_index_GyroAcc] = imu_Data.accW.y;
akashvibhute 0:88de7b3bd889 222 movWindow_LinAcc[2][movWindow_index_GyroAcc] = imu_Data.accW.z;
akashvibhute 0:88de7b3bd889 223
akashvibhute 0:88de7b3bd889 224 for(int i=0; i<3; i++) {
akashvibhute 0:88de7b3bd889 225 AngVel[i] = generalFunctions::moving_window(movWindow_AngVel[i]/**/, movWindow_index_GyroAcc); /****** PROBABLY WILL BREAK HERE!!! ******/
akashvibhute 0:88de7b3bd889 226 LinAcc[i] = generalFunctions::moving_window(movWindow_LinAcc[i]/**/, movWindow_index_GyroAcc); /****** PROBABLY WILL BREAK HERE!!! ******/
akashvibhute 0:88de7b3bd889 227 }
akashvibhute 0:88de7b3bd889 228
akashvibhute 0:88de7b3bd889 229 movWindow_index_Pose++;
akashvibhute 0:88de7b3bd889 230 if(movWindow_index_Pose >= movWindow_len_Pose) movWindow_index_Pose=0;
akashvibhute 0:88de7b3bd889 231
akashvibhute 0:88de7b3bd889 232 movWindow_index_GyroAcc++;
akashvibhute 0:88de7b3bd889 233 if(movWindow_index_GyroAcc >= movWindow_len_GyroAcc) movWindow_index_GyroAcc=0;
akashvibhute 0:88de7b3bd889 234
akashvibhute 0:88de7b3bd889 235 //debug_printf("Roll:%6.2fdeg, Pitch:%6.2fdeg, Yaw:%6.2fdeg\n", RAD_TO_DEG(imu_Data.roll), RAD_TO_DEG(imu_Data.pitch), RAD_TO_DEG(imu_Data.yaw));
akashvibhute 0:88de7b3bd889 236 }
akashvibhute 0:88de7b3bd889 237 }
akashvibhute 0:88de7b3bd889 238 }
akashvibhute 0:88de7b3bd889 239
akashvibhute 0:88de7b3bd889 240 void IMU_MPU6050::getPose(float *pose[3])
akashvibhute 0:88de7b3bd889 241 {
akashvibhute 10:31eee5f90d04 242 //pitch about X, roll about Y, yaw about Z
akashvibhute 10:31eee5f90d04 243 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 244 *pose[i]=Pose[i];
akashvibhute 0:88de7b3bd889 245 }
akashvibhute 0:88de7b3bd889 246
akashvibhute 0:88de7b3bd889 247 void IMU_MPU6050::getQuat(float *quaternion[4])
akashvibhute 0:88de7b3bd889 248 {
akashvibhute 0:88de7b3bd889 249 for(int i=0; i<4; i++)
akashvibhute 0:88de7b3bd889 250 *quaternion[i]=Quat[i];
akashvibhute 0:88de7b3bd889 251 }
akashvibhute 0:88de7b3bd889 252
akashvibhute 0:88de7b3bd889 253 void IMU_MPU6050::getAngVel(float *angVelocity[3])
akashvibhute 0:88de7b3bd889 254 {
akashvibhute 0:88de7b3bd889 255 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 256 *angVelocity[i]=AngVel[i];
akashvibhute 0:88de7b3bd889 257 }
akashvibhute 0:88de7b3bd889 258
akashvibhute 0:88de7b3bd889 259 void IMU_MPU6050::getLinAcc(float *linAcc[3])
akashvibhute 0:88de7b3bd889 260 {
akashvibhute 0:88de7b3bd889 261 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 262 *linAcc[i]=LinAcc[i];
akashvibhute 0:88de7b3bd889 263 }
akashvibhute 0:88de7b3bd889 264
akashvibhute 0:88de7b3bd889 265 float IMU_MPU6050::getTime()
akashvibhute 0:88de7b3bd889 266 {
akashvibhute 0:88de7b3bd889 267 return(imuTime_s[1]);
akashvibhute 0:88de7b3bd889 268 }
akashvibhute 0:88de7b3bd889 269
akashvibhute 0:88de7b3bd889 270 void IMU_MPU6050::setPose(float pose_in[3])
akashvibhute 0:88de7b3bd889 271 {
akashvibhute 0:88de7b3bd889 272 IMU_MPU6050::imuUpdate(); //refresh imu before updating Pose
akashvibhute 0:88de7b3bd889 273
akashvibhute 10:31eee5f90d04 274 //pitch about X, roll about Y, yaw about Z
akashvibhute 10:31eee5f90d04 275 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 276 PoseCorrection[i]=(Pose[i] - pose_in[i]);
akashvibhute 0:88de7b3bd889 277 }
akashvibhute 0:88de7b3bd889 278
akashvibhute 0:88de7b3bd889 279 /*** ***/
akashvibhute 0:88de7b3bd889 280
akashvibhute 0:88de7b3bd889 281 /*** BNO055 ***/
akashvibhute 0:88de7b3bd889 282
akashvibhute 0:88de7b3bd889 283 IMU_BNO055::IMU_BNO055(): imu_BNO055(i2c_SDA, i2c_SCL)
akashvibhute 0:88de7b3bd889 284 {
akashvibhute 0:88de7b3bd889 285 unsigned int movWindowSize_Pose = Pose_MWindowSize;
akashvibhute 0:88de7b3bd889 286 unsigned int movWindowSize_GyroAcc = GyroAcc_MWindowSize;
akashvibhute 0:88de7b3bd889 287
akashvibhute 0:88de7b3bd889 288 unstable_readings = BNO055_StabilizationReadings;
akashvibhute 0:88de7b3bd889 289 //imu_stabilized[0] =0;
akashvibhute 0:88de7b3bd889 290 imu_stabilized[0] =1; //for imu mode no need to calibrate
akashvibhute 0:88de7b3bd889 291 imu_stabilized[1] =0;
akashvibhute 0:88de7b3bd889 292
akashvibhute 0:88de7b3bd889 293 if(movWindowSize_Pose <= movWindow_lenMax) movWindow_len_Pose = movWindowSize_Pose;
akashvibhute 0:88de7b3bd889 294 else movWindow_len_Pose = movWindow_lenMax;
akashvibhute 0:88de7b3bd889 295
akashvibhute 0:88de7b3bd889 296 if(movWindow_len_GyroAcc <= movWindow_lenMax) movWindow_len_GyroAcc = movWindowSize_GyroAcc;
akashvibhute 0:88de7b3bd889 297 else movWindow_len_GyroAcc = movWindow_lenMax;
akashvibhute 0:88de7b3bd889 298
akashvibhute 0:88de7b3bd889 299 movWindow_index_Pose=0;
akashvibhute 0:88de7b3bd889 300 movWindow_index_GyroAcc=0;
akashvibhute 0:88de7b3bd889 301
akashvibhute 0:88de7b3bd889 302 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 303 PoseCorrection[i]=0;
akashvibhute 9:47b6a8530868 304
akashvibhute 9:47b6a8530868 305 bno_timer.start();
akashvibhute 0:88de7b3bd889 306 }
akashvibhute 0:88de7b3bd889 307
akashvibhute 0:88de7b3bd889 308 bool IMU_BNO055::imuInit()
akashvibhute 0:88de7b3bd889 309 {
akashvibhute 0:88de7b3bd889 310 imu_BNO055.reset();
akashvibhute 0:88de7b3bd889 311
akashvibhute 0:88de7b3bd889 312 if (imu_BNO055.check()) {
akashvibhute 9:47b6a8530868 313
akashvibhute 0:88de7b3bd889 314 //load default calib parameters
akashvibhute 0:88de7b3bd889 315 offset_acc[0] =10;
akashvibhute 0:88de7b3bd889 316 offset_acc[1] =509;
akashvibhute 0:88de7b3bd889 317 offset_acc[2] =500;
akashvibhute 0:88de7b3bd889 318
akashvibhute 0:88de7b3bd889 319 offset_mag[0] =409;
akashvibhute 0:88de7b3bd889 320 offset_mag[1] =487;
akashvibhute 0:88de7b3bd889 321 offset_mag[2] =290;
akashvibhute 0:88de7b3bd889 322
akashvibhute 0:88de7b3bd889 323 offset_gyr[0] =4;
akashvibhute 0:88de7b3bd889 324 offset_gyr[0] =0;
akashvibhute 0:88de7b3bd889 325 offset_gyr[2] =1;
akashvibhute 0:88de7b3bd889 326
akashvibhute 0:88de7b3bd889 327 radius_acc = 235;
akashvibhute 0:88de7b3bd889 328 radius_mag = 165;
akashvibhute 0:88de7b3bd889 329
akashvibhute 0:88de7b3bd889 330 writeCalibrationData();
akashvibhute 0:88de7b3bd889 331
akashvibhute 0:88de7b3bd889 332 imu_BNO055.set_mapping(2);
akashvibhute 0:88de7b3bd889 333 imu_BNO055.setmode(OPERATION_MODE_IMUPLUS);
akashvibhute 0:88de7b3bd889 334
akashvibhute 0:88de7b3bd889 335 imu_BNO055.get_calib();
akashvibhute 0:88de7b3bd889 336 calibration_status = (imu_BNO055.calib & 0xc0) >> 6; //upper 2 MSB's denoting calibration status
akashvibhute 0:88de7b3bd889 337
akashvibhute 0:88de7b3bd889 338 return(true);
akashvibhute 0:88de7b3bd889 339 } else return(false);
akashvibhute 0:88de7b3bd889 340
akashvibhute 0:88de7b3bd889 341 //debug_printf("Init finish!\n");
akashvibhute 0:88de7b3bd889 342 }
akashvibhute 0:88de7b3bd889 343
akashvibhute 0:88de7b3bd889 344 void IMU_BNO055::imuUpdate()
akashvibhute 0:88de7b3bd889 345 {
akashvibhute 0:88de7b3bd889 346 imu_BNO055.get_calib();
akashvibhute 0:88de7b3bd889 347 imu_BNO055.get_angles();
akashvibhute 0:88de7b3bd889 348
akashvibhute 0:88de7b3bd889 349 imu_BNO055.get_lia(); //imu_BNO055.get_accel();
akashvibhute 0:88de7b3bd889 350 imu_BNO055.get_gyro();
akashvibhute 0:88de7b3bd889 351 //imu_BNO055.get_mag();
akashvibhute 0:88de7b3bd889 352 imu_BNO055.get_quat();
akashvibhute 0:88de7b3bd889 353 //imu_BNO055.get_grv();
akashvibhute 0:88de7b3bd889 354
akashvibhute 0:88de7b3bd889 355 float posePRY[3];
akashvibhute 0:88de7b3bd889 356
akashvibhute 0:88de7b3bd889 357 //debug_printf("DEBUG>> got YPR data\n");
akashvibhute 0:88de7b3bd889 358
akashvibhute 0:88de7b3bd889 359 if(bno_timer.read() > 2100) { //reset as timer is close to overflow
akashvibhute 0:88de7b3bd889 360 bno_timer.reset();
akashvibhute 0:88de7b3bd889 361 imuTime_s[0] = imuTime_s[1];
akashvibhute 0:88de7b3bd889 362 } else
akashvibhute 0:88de7b3bd889 363 imuTime_s[1] = imuTime_s[0] + bno_timer.read();
akashvibhute 0:88de7b3bd889 364
akashvibhute 0:88de7b3bd889 365 time_s = imuTime_s[1]; //imu timestamp
akashvibhute 0:88de7b3bd889 366
akashvibhute 0:88de7b3bd889 367 calib_stat[4] = imu_BNO055.calib;
akashvibhute 0:88de7b3bd889 368 calib_stat[3] = (calib_stat[4] & 0xc0) >> 6;
akashvibhute 0:88de7b3bd889 369 calib_stat[2] = (calib_stat[4] & 0x30) >> 4;
akashvibhute 0:88de7b3bd889 370 calib_stat[1] = (calib_stat[4] & 0x0c) >> 2;
akashvibhute 0:88de7b3bd889 371 calib_stat[0] = (calib_stat[4] & 0x03);
akashvibhute 0:88de7b3bd889 372
akashvibhute 0:88de7b3bd889 373 calibration_status = (imu_BNO055.calib & 0xc0) >> 6; //upper 2 MSB's denoting calibration status
akashvibhute 0:88de7b3bd889 374
akashvibhute 0:88de7b3bd889 375 //if((calib_stat[4] >= 193) && (imu_stabilized[1] == 0)) {
akashvibhute 0:88de7b3bd889 376 if(imu_stabilized[1] == 0) { //for imu only mode
akashvibhute 0:88de7b3bd889 377 if(imu_stabilized[0] == 0)
akashvibhute 0:88de7b3bd889 378 imu_stabilized[0] = 1;
akashvibhute 9:47b6a8530868 379
akashvibhute 0:88de7b3bd889 380 if( ((generalFunctions::abs_f(imu_BNO055.lia.x) - initialAcc[0])* 9.81*1000 <= 5.0) && ((generalFunctions::abs_f(imu_BNO055.lia.y) - initialAcc[1])* 9.81*1000 <= 5.0) && ((generalFunctions::abs_f(imu_BNO055.lia.z) - initialAcc[2])* 9.81*1000 <= 5.0)) {
akashvibhute 0:88de7b3bd889 381
akashvibhute 0:88de7b3bd889 382 unstable_readings--;
akashvibhute 0:88de7b3bd889 383 }
akashvibhute 0:88de7b3bd889 384
akashvibhute 0:88de7b3bd889 385 initialAcc[0] = generalFunctions::abs_f(imu_BNO055.lia.x);
akashvibhute 0:88de7b3bd889 386 initialAcc[1] = generalFunctions::abs_f(imu_BNO055.lia.y);
akashvibhute 0:88de7b3bd889 387 initialAcc[2] = generalFunctions::abs_f(imu_BNO055.lia.z);
akashvibhute 0:88de7b3bd889 388
akashvibhute 0:88de7b3bd889 389 if(unstable_readings <= 1) {
akashvibhute 0:88de7b3bd889 390 //imu_initialAngles[0] = imu_BNO055.euler.pitch;
akashvibhute 0:88de7b3bd889 391 //imu_initialAngles[1] = imu_BNO055.euler.roll;
akashvibhute 0:88de7b3bd889 392 imu_initialAngles[2] = 2*RAD_TO_DEG(M_PI) - imu_BNO055.euler.yaw;
akashvibhute 0:88de7b3bd889 393
akashvibhute 0:88de7b3bd889 394 imu_initialAngles[0] = 0;
akashvibhute 0:88de7b3bd889 395 imu_initialAngles[1] = 0;
akashvibhute 0:88de7b3bd889 396
akashvibhute 0:88de7b3bd889 397 imu_stabilized[1] =1;
akashvibhute 0:88de7b3bd889 398
akashvibhute 0:88de7b3bd889 399 bno_timer.reset();
akashvibhute 0:88de7b3bd889 400 }
akashvibhute 0:88de7b3bd889 401 }
akashvibhute 0:88de7b3bd889 402
akashvibhute 0:88de7b3bd889 403 posePRY[0] = imu_BNO055.euler.pitch - imu_initialAngles[0] - PoseCorrection[0];
akashvibhute 0:88de7b3bd889 404 posePRY[1] = imu_BNO055.euler.roll - imu_initialAngles[1] - PoseCorrection[1];
akashvibhute 0:88de7b3bd889 405 //posePRY[2] = -1.0*(imu_BNO055.euler.yaw - imu_initialAngles[0]- DRIFT_CORRECTION(imuTime_s[1])) - PoseCorrection[2];
akashvibhute 0:88de7b3bd889 406 //posePRY[2] = -1.0*(imu_BNO055.euler.yaw - imu_initialAngles[0]) - PoseCorrection[2];
akashvibhute 5:42b4f2fb593f 407 posePRY[2] = (2*RAD_TO_DEG(M_PI) - imu_BNO055.euler.yaw) - imu_initialAngles[2] - PoseCorrection[2];
akashvibhute 0:88de7b3bd889 408
akashvibhute 0:88de7b3bd889 409 //convert angles to 0 to 2pi range
akashvibhute 9:47b6a8530868 410
akashvibhute 0:88de7b3bd889 411 for(int i=0; i<3; i++) {
akashvibhute 0:88de7b3bd889 412 if(posePRY[i] > 2*RAD_TO_DEG(M_PI))
akashvibhute 0:88de7b3bd889 413 posePRY[i] -= 2*RAD_TO_DEG(M_PI);
akashvibhute 0:88de7b3bd889 414 if(posePRY[i] < 0.0)
akashvibhute 0:88de7b3bd889 415 posePRY[i] += 2*RAD_TO_DEG(M_PI);
akashvibhute 0:88de7b3bd889 416 }
akashvibhute 0:88de7b3bd889 417
akashvibhute 0:88de7b3bd889 418
akashvibhute 0:88de7b3bd889 419 //moving window average of pose angle
akashvibhute 0:88de7b3bd889 420 for(int i=0; i<3; i++) {
akashvibhute 9:47b6a8530868 421 movWindow_Pose[i][movWindow_index_Pose] = posePRY[i];
akashvibhute 9:47b6a8530868 422 Pose[i] = generalFunctions::moving_window(movWindow_Pose[i], movWindow_len_Pose);
akashvibhute 0:88de7b3bd889 423 }
akashvibhute 0:88de7b3bd889 424
akashvibhute 0:88de7b3bd889 425 /** Todo: offset->filter quaternion, use this quaternion to generate gravity vector, and consequently aaReal and aaWorld **/
akashvibhute 0:88de7b3bd889 426
akashvibhute 0:88de7b3bd889 427 Quat[0]= imu_BNO055.quat.w;
akashvibhute 0:88de7b3bd889 428 Quat[1]= imu_BNO055.quat.x;
akashvibhute 0:88de7b3bd889 429 Quat[2]= imu_BNO055.quat.y;
akashvibhute 0:88de7b3bd889 430 Quat[3]= imu_BNO055.quat.z;
akashvibhute 0:88de7b3bd889 431
akashvibhute 0:88de7b3bd889 432 //moving window average of gyro velocities
akashvibhute 0:88de7b3bd889 433 movWindow_AngVel[0][movWindow_index_GyroAcc] = imu_BNO055.gyro.x;
akashvibhute 0:88de7b3bd889 434 movWindow_AngVel[1][movWindow_index_GyroAcc] = imu_BNO055.gyro.y;
akashvibhute 0:88de7b3bd889 435 movWindow_AngVel[2][movWindow_index_GyroAcc] = imu_BNO055.gyro.z;
akashvibhute 0:88de7b3bd889 436
akashvibhute 3:d7a72ce26117 437 movWindow_LinAcc[0][movWindow_index_GyroAcc] = imu_BNO055.lia.x * 9.81 * 1000; //convert to mm/s2
akashvibhute 3:d7a72ce26117 438 movWindow_LinAcc[1][movWindow_index_GyroAcc] = imu_BNO055.lia.y * 9.81 * 1000; //convert to mm/s2
akashvibhute 3:d7a72ce26117 439 movWindow_LinAcc[2][movWindow_index_GyroAcc] = imu_BNO055.lia.z * 9.81 * 1000; //convert to mm/s2
akashvibhute 0:88de7b3bd889 440
akashvibhute 0:88de7b3bd889 441 for(int i=0; i<3; i++) {
akashvibhute 0:88de7b3bd889 442 AngVel[i] = generalFunctions::moving_window(movWindow_AngVel[i]/**/, movWindow_index_GyroAcc); /****** PROBABLY WILL BREAK HERE!!! ******/
akashvibhute 0:88de7b3bd889 443 LinAcc[i] = generalFunctions::moving_window(movWindow_LinAcc[i]/**/, movWindow_index_GyroAcc); /****** PROBABLY WILL BREAK HERE!!! ******/
akashvibhute 0:88de7b3bd889 444 }
akashvibhute 0:88de7b3bd889 445
akashvibhute 0:88de7b3bd889 446 movWindow_index_Pose++;
akashvibhute 0:88de7b3bd889 447 if(movWindow_index_Pose >= movWindow_len_Pose) movWindow_index_Pose=0;
akashvibhute 0:88de7b3bd889 448
akashvibhute 0:88de7b3bd889 449 movWindow_index_GyroAcc++;
akashvibhute 0:88de7b3bd889 450 if(movWindow_index_GyroAcc >= movWindow_len_GyroAcc) movWindow_index_GyroAcc=0;
akashvibhute 0:88de7b3bd889 451
akashvibhute 0:88de7b3bd889 452 //debug_printf("Roll:%6.2fdeg, Pitch:%6.2fdeg, Yaw:%6.2fdeg\n", RAD_TO_DEG(imu_Data.roll), RAD_TO_DEG(imu_Data.pitch), RAD_TO_DEG(imu_Data.yaw));
akashvibhute 0:88de7b3bd889 453 }
akashvibhute 0:88de7b3bd889 454
akashvibhute 0:88de7b3bd889 455 void IMU_BNO055::getPose(float *pose[3])
akashvibhute 0:88de7b3bd889 456 {
akashvibhute 9:47b6a8530868 457 //pitch about X, roll about Y, yaw about Z
akashvibhute 9:47b6a8530868 458 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 459 *pose[i]=Pose[i];
akashvibhute 0:88de7b3bd889 460 }
akashvibhute 0:88de7b3bd889 461
akashvibhute 0:88de7b3bd889 462 void IMU_BNO055::getQuat(float *quaternion[4])
akashvibhute 0:88de7b3bd889 463 {
akashvibhute 0:88de7b3bd889 464 for(int i=0; i<4; i++)
akashvibhute 0:88de7b3bd889 465 *quaternion[i]=Quat[i];
akashvibhute 0:88de7b3bd889 466 }
akashvibhute 0:88de7b3bd889 467
akashvibhute 0:88de7b3bd889 468 void IMU_BNO055::getAngVel(float *angVelocity[3])
akashvibhute 0:88de7b3bd889 469 {
akashvibhute 0:88de7b3bd889 470 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 471 *angVelocity[i]=AngVel[i];
akashvibhute 0:88de7b3bd889 472 }
akashvibhute 0:88de7b3bd889 473
akashvibhute 0:88de7b3bd889 474 void IMU_BNO055::getLinAcc(float *linAcc[3])
akashvibhute 0:88de7b3bd889 475 {
akashvibhute 0:88de7b3bd889 476 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 477 *linAcc[i]=LinAcc[i];
akashvibhute 0:88de7b3bd889 478 }
akashvibhute 0:88de7b3bd889 479
akashvibhute 0:88de7b3bd889 480 float IMU_BNO055::getTime()
akashvibhute 0:88de7b3bd889 481 {
akashvibhute 0:88de7b3bd889 482 return(imuTime_s[1]);
akashvibhute 0:88de7b3bd889 483 }
akashvibhute 0:88de7b3bd889 484
akashvibhute 0:88de7b3bd889 485 void IMU_BNO055::setPose(float pose_in[3])
akashvibhute 0:88de7b3bd889 486 {
akashvibhute 0:88de7b3bd889 487 IMU_BNO055::imuUpdate(); //refresh imu before updating Pose
akashvibhute 9:47b6a8530868 488
akashvibhute 0:88de7b3bd889 489 //pitch about X, roll about Y, yaw about Z
akashvibhute 9:47b6a8530868 490 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 491 PoseCorrection[i]=(Pose[i] - pose_in[i]);
akashvibhute 0:88de7b3bd889 492 }
akashvibhute 0:88de7b3bd889 493
akashvibhute 0:88de7b3bd889 494 void IMU_BNO055::readCalibrationData()
akashvibhute 0:88de7b3bd889 495 {
akashvibhute 0:88de7b3bd889 496 //imu_BNO055.get_calib();
akashvibhute 0:88de7b3bd889 497 imu_BNO055.read_calibration_data();
akashvibhute 9:47b6a8530868 498
akashvibhute 0:88de7b3bd889 499 for(int i=0; i<22; i++)
akashvibhute 0:88de7b3bd889 500 calibration_regs[i]=imu_BNO055.calibration[i];
akashvibhute 9:47b6a8530868 501
akashvibhute 0:88de7b3bd889 502 offset_acc[0] = (calibration_regs[1] + calibration_regs[0]);
akashvibhute 0:88de7b3bd889 503 offset_acc[1] = (calibration_regs[3] + calibration_regs[2]);
akashvibhute 0:88de7b3bd889 504 offset_acc[2] = (calibration_regs[5] + calibration_regs[4]);
akashvibhute 9:47b6a8530868 505
akashvibhute 0:88de7b3bd889 506 offset_mag[0] = (calibration_regs[7] + calibration_regs[6]);
akashvibhute 0:88de7b3bd889 507 offset_mag[1] = (calibration_regs[9] + calibration_regs[8]);
akashvibhute 0:88de7b3bd889 508 offset_mag[2] = (calibration_regs[11] + calibration_regs[10]);
akashvibhute 9:47b6a8530868 509
akashvibhute 0:88de7b3bd889 510 offset_gyr[0] = (calibration_regs[13] + calibration_regs[12]);
akashvibhute 0:88de7b3bd889 511 offset_gyr[1] = (calibration_regs[15] + calibration_regs[14]);
akashvibhute 0:88de7b3bd889 512 offset_gyr[2] = (calibration_regs[17] + calibration_regs[16]);
akashvibhute 9:47b6a8530868 513
akashvibhute 0:88de7b3bd889 514 radius_acc = (calibration_regs[19] + calibration_regs[18]);
akashvibhute 9:47b6a8530868 515 radius_mag = (calibration_regs[21] + calibration_regs[20]);
akashvibhute 0:88de7b3bd889 516 }
akashvibhute 0:88de7b3bd889 517
akashvibhute 0:88de7b3bd889 518 void IMU_BNO055::writeCalibrationData()
akashvibhute 0:88de7b3bd889 519 {
akashvibhute 0:88de7b3bd889 520 //acc
akashvibhute 0:88de7b3bd889 521 calibration_regs[0] = offset_acc[0] & 0b00001111;
akashvibhute 0:88de7b3bd889 522 calibration_regs[1] = offset_acc[0] & 0b11110000;
akashvibhute 9:47b6a8530868 523
akashvibhute 0:88de7b3bd889 524 calibration_regs[2] = offset_acc[1] & 0b00001111;
akashvibhute 0:88de7b3bd889 525 calibration_regs[3] = offset_acc[1] & 0b11110000;
akashvibhute 9:47b6a8530868 526
akashvibhute 0:88de7b3bd889 527 calibration_regs[4] = offset_acc[2] & 0b00001111;
akashvibhute 0:88de7b3bd889 528 calibration_regs[5] = offset_acc[2] & 0b11110000;
akashvibhute 9:47b6a8530868 529
akashvibhute 0:88de7b3bd889 530 //mag
akashvibhute 0:88de7b3bd889 531 calibration_regs[6] = offset_mag[0] & 0b00001111;
akashvibhute 0:88de7b3bd889 532 calibration_regs[7] = offset_mag[0] & 0b11110000;
akashvibhute 9:47b6a8530868 533
akashvibhute 0:88de7b3bd889 534 calibration_regs[8] = offset_mag[1] & 0b00001111;
akashvibhute 0:88de7b3bd889 535 calibration_regs[9] = offset_mag[1] & 0b11110000;
akashvibhute 9:47b6a8530868 536
akashvibhute 0:88de7b3bd889 537 calibration_regs[10] = offset_mag[2] & 0b00001111;
akashvibhute 0:88de7b3bd889 538 calibration_regs[11] = offset_mag[2] & 0b11110000;
akashvibhute 9:47b6a8530868 539
akashvibhute 0:88de7b3bd889 540 //gyro
akashvibhute 0:88de7b3bd889 541 calibration_regs[12] = offset_gyr[0] & 0b00001111;
akashvibhute 0:88de7b3bd889 542 calibration_regs[13] = offset_gyr[0] & 0b11110000;
akashvibhute 9:47b6a8530868 543
akashvibhute 0:88de7b3bd889 544 calibration_regs[14] = offset_gyr[1] & 0b00001111;
akashvibhute 0:88de7b3bd889 545 calibration_regs[15] = offset_gyr[1] & 0b11110000;
akashvibhute 9:47b6a8530868 546
akashvibhute 0:88de7b3bd889 547 calibration_regs[16] = offset_gyr[2] & 0b00001111;
akashvibhute 0:88de7b3bd889 548 calibration_regs[17] = offset_gyr[2] & 0b11110000;
akashvibhute 9:47b6a8530868 549
akashvibhute 0:88de7b3bd889 550 //acc_radius
akashvibhute 0:88de7b3bd889 551 calibration_regs[18] = radius_acc & 0b00001111;
akashvibhute 0:88de7b3bd889 552 calibration_regs[19] = radius_acc & 0b11110000;
akashvibhute 9:47b6a8530868 553
akashvibhute 0:88de7b3bd889 554 //mag_radius
akashvibhute 0:88de7b3bd889 555 calibration_regs[20] = radius_mag & 0b00001111;
akashvibhute 0:88de7b3bd889 556 calibration_regs[21] = radius_mag & 0b11110000;
akashvibhute 9:47b6a8530868 557
akashvibhute 0:88de7b3bd889 558 for(int i=0; i<22; i++)
akashvibhute 0:88de7b3bd889 559 imu_BNO055.calibration[i] = calibration_regs[i];
akashvibhute 9:47b6a8530868 560
akashvibhute 0:88de7b3bd889 561 imu_BNO055.write_calibration_data();
akashvibhute 0:88de7b3bd889 562 }
akashvibhute 0:88de7b3bd889 563
akashvibhute 0:88de7b3bd889 564 /*** ***/