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@11:fb47aa30cc7b, 2016-06-16 (annotated)

Committer:
akashvibhute
Date:
Thu Jun 16 05:42:21 2016 +0000
Revision:
11:fb47aa30cc7b
Parent:
10:31eee5f90d04
Child:
14:7586410ac0e6
wufang version virgo 32m; with comm and purepursuit

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 11:fb47aa30cc7b 114
akashvibhute 11:fb47aa30cc7b 115 for(int i=0; i<3; i++)
akashvibhute 11:fb47aa30cc7b 116 imu_initialAngles[i] = RAD_TO_DEG(imu_initialAngles[i]);
akashvibhute 11:fb47aa30cc7b 117
akashvibhute 0:88de7b3bd889 118 mpu_timer.start();
akashvibhute 0:88de7b3bd889 119 unstable_readings --;
akashvibhute 0:88de7b3bd889 120
akashvibhute 10:31eee5f90d04 121 imu_stabilized[0] = 1;
akashvibhute 0:88de7b3bd889 122 //debug_printf("\nIMU stabilized\n");
akashvibhute 0:88de7b3bd889 123 }
akashvibhute 0:88de7b3bd889 124 }
akashvibhute 0:88de7b3bd889 125 }
akashvibhute 0:88de7b3bd889 126
akashvibhute 0:88de7b3bd889 127 else {
akashvibhute 0:88de7b3bd889 128 float yawPitchRoll[3];
akashvibhute 0:88de7b3bd889 129 int16_t raw_gyro[3];
akashvibhute 0:88de7b3bd889 130 imu_mpu6050.getFIFOBytes(fifoBuffer, packetSize);
akashvibhute 0:88de7b3bd889 131 imu_mpu6050.dmpGetQuaternion(&imu_Data.q, fifoBuffer);
akashvibhute 0:88de7b3bd889 132 imu_mpu6050.dmpGetGravity(&imu_Data.gravity, &imu_Data.q);
akashvibhute 0:88de7b3bd889 133 imu_mpu6050.dmpGetYawPitchRoll(yawPitchRoll, &imu_Data.q, &imu_Data.gravity);
akashvibhute 10:31eee5f90d04 134
akashvibhute 10:31eee5f90d04 135 if((enable_mag == 1) && ((time_s - mag_time)*1000 >= mpu_mag_readms) ){
akashvibhute 10:31eee5f90d04 136 int16_t mx,my,mz;
akashvibhute 10:31eee5f90d04 137
akashvibhute 10:31eee5f90d04 138 imu_mpu6050.getMag(&mx, &my, &mz);
akashvibhute 10:31eee5f90d04 139 mag_time = time_s;
akashvibhute 10:31eee5f90d04 140
akashvibhute 10:31eee5f90d04 141 imu_Data.magnetometer[0] = (float)mx /10.0;
akashvibhute 10:31eee5f90d04 142 imu_Data.magnetometer[1] = (float)my /10.0;
akashvibhute 10:31eee5f90d04 143 imu_Data.magnetometer[2] = (float)mz /10.0;
akashvibhute 10:31eee5f90d04 144
akashvibhute 10:31eee5f90d04 145 //moving window average of pose angle
akashvibhute 10:31eee5f90d04 146 for(int i=0; i<3; i++) {
akashvibhute 10:31eee5f90d04 147 movWindow_Mag[i][movWindow_index_Mag] = imu_Data.magnetometer[i];
akashvibhute 10:31eee5f90d04 148 Mag[i] = generalFunctions::moving_window(movWindow_Mag[i], movWindow_len_Mag);
akashvibhute 10:31eee5f90d04 149 }
akashvibhute 10:31eee5f90d04 150
akashvibhute 10:31eee5f90d04 151 }
akashvibhute 10:31eee5f90d04 152
akashvibhute 0:88de7b3bd889 153
akashvibhute 0:88de7b3bd889 154 //debug_printf("DEBUG>> got YPR data\n");
akashvibhute 0:88de7b3bd889 155
akashvibhute 0:88de7b3bd889 156 if(mpu_timer.read() > 2100) { //reset as timer is close to overflow
akashvibhute 0:88de7b3bd889 157 mpu_timer.reset();
akashvibhute 0:88de7b3bd889 158 imuTime_s[0] = imuTime_s[1];
akashvibhute 0:88de7b3bd889 159 } else
akashvibhute 0:88de7b3bd889 160 imuTime_s[1] = imuTime_s[0] + mpu_timer.read();
akashvibhute 0:88de7b3bd889 161
akashvibhute 0:88de7b3bd889 162 time_s = imuTime_s[1]; //imu timestamp
akashvibhute 11:fb47aa30cc7b 163
akashvibhute 11:fb47aa30cc7b 164 for(int i=0; i<3; i++)
akashvibhute 11:fb47aa30cc7b 165 yawPitchRoll[i] = RAD_TO_DEG(yawPitchRoll[i]);
akashvibhute 0:88de7b3bd889 166
akashvibhute 0:88de7b3bd889 167 imu_Data.posePRY[2] = -1*(yawPitchRoll[0] - imu_initialAngles[0]- DRIFT_CORRECTION(imuTime_s[1])) - PoseCorrection[0];
akashvibhute 0:88de7b3bd889 168 imu_Data.posePRY[1] = yawPitchRoll[1] - imu_initialAngles[1] - PoseCorrection[1];
akashvibhute 0:88de7b3bd889 169 imu_Data.posePRY[0] = yawPitchRoll[2] - imu_initialAngles[2] - PoseCorrection[2];
akashvibhute 0:88de7b3bd889 170
akashvibhute 0:88de7b3bd889 171 //convert angles to 0 to 2pi range
akashvibhute 0:88de7b3bd889 172 for(int i=0; i<3; i++) {
akashvibhute 10:31eee5f90d04 173 if(imu_Data.posePRY[i] > 2*RAD_TO_DEG(M_PI))
akashvibhute 10:31eee5f90d04 174 imu_Data.posePRY[i] -= 2*RAD_TO_DEG(M_PI);
akashvibhute 10:31eee5f90d04 175 if(imu_Data.posePRY[i] < 0.0)
akashvibhute 10:31eee5f90d04 176 imu_Data.posePRY[i] += 2*RAD_TO_DEG(M_PI);
akashvibhute 0:88de7b3bd889 177 }
akashvibhute 0:88de7b3bd889 178
akashvibhute 10:31eee5f90d04 179
akashvibhute 0:88de7b3bd889 180 //moving window average of pose angle
akashvibhute 0:88de7b3bd889 181 for(int i=0; i<3; i++) {
akashvibhute 10:31eee5f90d04 182 movWindow_Pose[i][movWindow_index_Pose] = sin(DEG_TO_RAD(imu_Data.posePRY[i]));
akashvibhute 10:31eee5f90d04 183 Pose[i] = RAD_TO_DEG(asin(generalFunctions::moving_window(movWindow_Pose[i], movWindow_len_Pose)));
akashvibhute 10:31eee5f90d04 184 }
akashvibhute 10:31eee5f90d04 185
akashvibhute 10:31eee5f90d04 186 //convert angles to 0 to 2pi range
akashvibhute 10:31eee5f90d04 187 for(int i=0; i<3; i++) {
akashvibhute 10:31eee5f90d04 188 if(Pose[i] > 2*RAD_TO_DEG(M_PI))
akashvibhute 10:31eee5f90d04 189 Pose[i] -= 2*RAD_TO_DEG(M_PI);
akashvibhute 10:31eee5f90d04 190 if(Pose[i] < 0.0)
akashvibhute 10:31eee5f90d04 191 Pose[i] += 2*RAD_TO_DEG(M_PI);
akashvibhute 0:88de7b3bd889 192 }
akashvibhute 0:88de7b3bd889 193
akashvibhute 0:88de7b3bd889 194 /** Todo: offset->filter quaternion, use this quaternion to generate gravity vector, and consequently aaReal and aaWorld **/
akashvibhute 0:88de7b3bd889 195
akashvibhute 0:88de7b3bd889 196 //convert filtered pose to quaternion here
akashvibhute 0:88de7b3bd889 197
akashvibhute 0:88de7b3bd889 198 Quat[0]= imu_Data.q.w;
akashvibhute 0:88de7b3bd889 199 Quat[1]= imu_Data.q.x;
akashvibhute 0:88de7b3bd889 200 Quat[2]= imu_Data.q.y;
akashvibhute 0:88de7b3bd889 201 Quat[3]= imu_Data.q.z;
akashvibhute 0:88de7b3bd889 202
akashvibhute 0:88de7b3bd889 203 //imu_mpu6050.dmpGetGravity(&imu_Data.gravity, &imu_Data.q /* filtered quaternion */);
akashvibhute 0:88de7b3bd889 204
akashvibhute 0:88de7b3bd889 205 /***************************************/
akashvibhute 0:88de7b3bd889 206
akashvibhute 0:88de7b3bd889 207 imu_mpu6050.dmpGetAccel(&imu_Data.aa, fifoBuffer);
akashvibhute 0:88de7b3bd889 208 imu_mpu6050.dmpGetGyro(raw_gyro, fifoBuffer);
akashvibhute 0:88de7b3bd889 209 imu_mpu6050.dmpGetLinearAccel(&imu_Data.aaReal, &imu_Data.aa, &imu_Data.gravity);
akashvibhute 0:88de7b3bd889 210 imu_mpu6050.dmpGetLinearAccelInWorld(&imu_Data.aaWorld, &imu_Data.aaReal, &imu_Data.q);
akashvibhute 0:88de7b3bd889 211
akashvibhute 0:88de7b3bd889 212 imu_Data.accW.x = (float) imu_Data.aaWorld.x * (9806.0/8192);
akashvibhute 0:88de7b3bd889 213 imu_Data.accW.y = (float) imu_Data.aaWorld.y * (9806.0/8192);
akashvibhute 0:88de7b3bd889 214 imu_Data.accW.z = (float) imu_Data.aaWorld.z * (9806.0/8192);
akashvibhute 0:88de7b3bd889 215
akashvibhute 0:88de7b3bd889 216 imu_Data.gyro.x = (float) raw_gyro[0];
akashvibhute 0:88de7b3bd889 217 imu_Data.gyro.y = (float) raw_gyro[1];
akashvibhute 0:88de7b3bd889 218 imu_Data.gyro.z = (float) raw_gyro[2];
akashvibhute 0:88de7b3bd889 219
akashvibhute 0:88de7b3bd889 220
akashvibhute 4:599d5ac25b73 221
akashvibhute 0:88de7b3bd889 222 //moving window average of gyro velocities
akashvibhute 0:88de7b3bd889 223 movWindow_AngVel[0][movWindow_index_GyroAcc] = imu_Data.gyro.x;
akashvibhute 0:88de7b3bd889 224 movWindow_AngVel[1][movWindow_index_GyroAcc] = imu_Data.gyro.y;
akashvibhute 0:88de7b3bd889 225 movWindow_AngVel[2][movWindow_index_GyroAcc] = imu_Data.gyro.z;
akashvibhute 0:88de7b3bd889 226
akashvibhute 0:88de7b3bd889 227 movWindow_LinAcc[0][movWindow_index_GyroAcc] = imu_Data.accW.x;
akashvibhute 0:88de7b3bd889 228 movWindow_LinAcc[1][movWindow_index_GyroAcc] = imu_Data.accW.y;
akashvibhute 0:88de7b3bd889 229 movWindow_LinAcc[2][movWindow_index_GyroAcc] = imu_Data.accW.z;
akashvibhute 0:88de7b3bd889 230
akashvibhute 0:88de7b3bd889 231 for(int i=0; i<3; i++) {
akashvibhute 0:88de7b3bd889 232 AngVel[i] = generalFunctions::moving_window(movWindow_AngVel[i]/**/, movWindow_index_GyroAcc); /****** PROBABLY WILL BREAK HERE!!! ******/
akashvibhute 0:88de7b3bd889 233 LinAcc[i] = generalFunctions::moving_window(movWindow_LinAcc[i]/**/, movWindow_index_GyroAcc); /****** PROBABLY WILL BREAK HERE!!! ******/
akashvibhute 0:88de7b3bd889 234 }
akashvibhute 0:88de7b3bd889 235
akashvibhute 0:88de7b3bd889 236 movWindow_index_Pose++;
akashvibhute 0:88de7b3bd889 237 if(movWindow_index_Pose >= movWindow_len_Pose) movWindow_index_Pose=0;
akashvibhute 0:88de7b3bd889 238
akashvibhute 0:88de7b3bd889 239 movWindow_index_GyroAcc++;
akashvibhute 0:88de7b3bd889 240 if(movWindow_index_GyroAcc >= movWindow_len_GyroAcc) movWindow_index_GyroAcc=0;
akashvibhute 0:88de7b3bd889 241
akashvibhute 0:88de7b3bd889 242 //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 243 }
akashvibhute 0:88de7b3bd889 244 }
akashvibhute 0:88de7b3bd889 245 }
akashvibhute 0:88de7b3bd889 246
akashvibhute 0:88de7b3bd889 247 void IMU_MPU6050::getPose(float *pose[3])
akashvibhute 0:88de7b3bd889 248 {
akashvibhute 10:31eee5f90d04 249 //pitch about X, roll about Y, yaw about Z
akashvibhute 10:31eee5f90d04 250 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 251 *pose[i]=Pose[i];
akashvibhute 0:88de7b3bd889 252 }
akashvibhute 0:88de7b3bd889 253
akashvibhute 0:88de7b3bd889 254 void IMU_MPU6050::getQuat(float *quaternion[4])
akashvibhute 0:88de7b3bd889 255 {
akashvibhute 0:88de7b3bd889 256 for(int i=0; i<4; i++)
akashvibhute 0:88de7b3bd889 257 *quaternion[i]=Quat[i];
akashvibhute 0:88de7b3bd889 258 }
akashvibhute 0:88de7b3bd889 259
akashvibhute 0:88de7b3bd889 260 void IMU_MPU6050::getAngVel(float *angVelocity[3])
akashvibhute 0:88de7b3bd889 261 {
akashvibhute 0:88de7b3bd889 262 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 263 *angVelocity[i]=AngVel[i];
akashvibhute 0:88de7b3bd889 264 }
akashvibhute 0:88de7b3bd889 265
akashvibhute 0:88de7b3bd889 266 void IMU_MPU6050::getLinAcc(float *linAcc[3])
akashvibhute 0:88de7b3bd889 267 {
akashvibhute 0:88de7b3bd889 268 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 269 *linAcc[i]=LinAcc[i];
akashvibhute 0:88de7b3bd889 270 }
akashvibhute 0:88de7b3bd889 271
akashvibhute 0:88de7b3bd889 272 float IMU_MPU6050::getTime()
akashvibhute 0:88de7b3bd889 273 {
akashvibhute 0:88de7b3bd889 274 return(imuTime_s[1]);
akashvibhute 0:88de7b3bd889 275 }
akashvibhute 0:88de7b3bd889 276
akashvibhute 0:88de7b3bd889 277 void IMU_MPU6050::setPose(float pose_in[3])
akashvibhute 0:88de7b3bd889 278 {
akashvibhute 0:88de7b3bd889 279 IMU_MPU6050::imuUpdate(); //refresh imu before updating Pose
akashvibhute 0:88de7b3bd889 280
akashvibhute 10:31eee5f90d04 281 //pitch about X, roll about Y, yaw about Z
akashvibhute 10:31eee5f90d04 282 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 283 PoseCorrection[i]=(Pose[i] - pose_in[i]);
akashvibhute 0:88de7b3bd889 284 }
akashvibhute 0:88de7b3bd889 285
akashvibhute 0:88de7b3bd889 286 /*** ***/
akashvibhute 0:88de7b3bd889 287
akashvibhute 0:88de7b3bd889 288 /*** BNO055 ***/
akashvibhute 0:88de7b3bd889 289
akashvibhute 0:88de7b3bd889 290 IMU_BNO055::IMU_BNO055(): imu_BNO055(i2c_SDA, i2c_SCL)
akashvibhute 0:88de7b3bd889 291 {
akashvibhute 0:88de7b3bd889 292 unsigned int movWindowSize_Pose = Pose_MWindowSize;
akashvibhute 0:88de7b3bd889 293 unsigned int movWindowSize_GyroAcc = GyroAcc_MWindowSize;
akashvibhute 0:88de7b3bd889 294
akashvibhute 0:88de7b3bd889 295 unstable_readings = BNO055_StabilizationReadings;
akashvibhute 0:88de7b3bd889 296 //imu_stabilized[0] =0;
akashvibhute 0:88de7b3bd889 297 imu_stabilized[0] =1; //for imu mode no need to calibrate
akashvibhute 0:88de7b3bd889 298 imu_stabilized[1] =0;
akashvibhute 0:88de7b3bd889 299
akashvibhute 0:88de7b3bd889 300 if(movWindowSize_Pose <= movWindow_lenMax) movWindow_len_Pose = movWindowSize_Pose;
akashvibhute 0:88de7b3bd889 301 else movWindow_len_Pose = movWindow_lenMax;
akashvibhute 0:88de7b3bd889 302
akashvibhute 0:88de7b3bd889 303 if(movWindow_len_GyroAcc <= movWindow_lenMax) movWindow_len_GyroAcc = movWindowSize_GyroAcc;
akashvibhute 0:88de7b3bd889 304 else movWindow_len_GyroAcc = movWindow_lenMax;
akashvibhute 0:88de7b3bd889 305
akashvibhute 0:88de7b3bd889 306 movWindow_index_Pose=0;
akashvibhute 0:88de7b3bd889 307 movWindow_index_GyroAcc=0;
akashvibhute 0:88de7b3bd889 308
akashvibhute 0:88de7b3bd889 309 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 310 PoseCorrection[i]=0;
akashvibhute 9:47b6a8530868 311
akashvibhute 9:47b6a8530868 312 bno_timer.start();
akashvibhute 0:88de7b3bd889 313 }
akashvibhute 0:88de7b3bd889 314
akashvibhute 0:88de7b3bd889 315 bool IMU_BNO055::imuInit()
akashvibhute 0:88de7b3bd889 316 {
akashvibhute 0:88de7b3bd889 317 imu_BNO055.reset();
akashvibhute 0:88de7b3bd889 318
akashvibhute 0:88de7b3bd889 319 if (imu_BNO055.check()) {
akashvibhute 9:47b6a8530868 320
akashvibhute 0:88de7b3bd889 321 //load default calib parameters
akashvibhute 0:88de7b3bd889 322 offset_acc[0] =10;
akashvibhute 0:88de7b3bd889 323 offset_acc[1] =509;
akashvibhute 0:88de7b3bd889 324 offset_acc[2] =500;
akashvibhute 0:88de7b3bd889 325
akashvibhute 0:88de7b3bd889 326 offset_mag[0] =409;
akashvibhute 0:88de7b3bd889 327 offset_mag[1] =487;
akashvibhute 0:88de7b3bd889 328 offset_mag[2] =290;
akashvibhute 0:88de7b3bd889 329
akashvibhute 0:88de7b3bd889 330 offset_gyr[0] =4;
akashvibhute 0:88de7b3bd889 331 offset_gyr[0] =0;
akashvibhute 0:88de7b3bd889 332 offset_gyr[2] =1;
akashvibhute 0:88de7b3bd889 333
akashvibhute 0:88de7b3bd889 334 radius_acc = 235;
akashvibhute 0:88de7b3bd889 335 radius_mag = 165;
akashvibhute 0:88de7b3bd889 336
akashvibhute 0:88de7b3bd889 337 writeCalibrationData();
akashvibhute 0:88de7b3bd889 338
akashvibhute 0:88de7b3bd889 339 imu_BNO055.set_mapping(2);
akashvibhute 0:88de7b3bd889 340 imu_BNO055.setmode(OPERATION_MODE_IMUPLUS);
akashvibhute 0:88de7b3bd889 341
akashvibhute 0:88de7b3bd889 342 imu_BNO055.get_calib();
akashvibhute 0:88de7b3bd889 343 calibration_status = (imu_BNO055.calib & 0xc0) >> 6; //upper 2 MSB's denoting calibration status
akashvibhute 0:88de7b3bd889 344
akashvibhute 0:88de7b3bd889 345 return(true);
akashvibhute 0:88de7b3bd889 346 } else return(false);
akashvibhute 0:88de7b3bd889 347
akashvibhute 0:88de7b3bd889 348 //debug_printf("Init finish!\n");
akashvibhute 0:88de7b3bd889 349 }
akashvibhute 0:88de7b3bd889 350
akashvibhute 0:88de7b3bd889 351 void IMU_BNO055::imuUpdate()
akashvibhute 0:88de7b3bd889 352 {
akashvibhute 0:88de7b3bd889 353 imu_BNO055.get_calib();
akashvibhute 0:88de7b3bd889 354 imu_BNO055.get_angles();
akashvibhute 0:88de7b3bd889 355
akashvibhute 0:88de7b3bd889 356 imu_BNO055.get_lia(); //imu_BNO055.get_accel();
akashvibhute 0:88de7b3bd889 357 imu_BNO055.get_gyro();
akashvibhute 0:88de7b3bd889 358 //imu_BNO055.get_mag();
akashvibhute 0:88de7b3bd889 359 imu_BNO055.get_quat();
akashvibhute 0:88de7b3bd889 360 //imu_BNO055.get_grv();
akashvibhute 0:88de7b3bd889 361
akashvibhute 0:88de7b3bd889 362 float posePRY[3];
akashvibhute 0:88de7b3bd889 363
akashvibhute 0:88de7b3bd889 364 //debug_printf("DEBUG>> got YPR data\n");
akashvibhute 0:88de7b3bd889 365
akashvibhute 0:88de7b3bd889 366 if(bno_timer.read() > 2100) { //reset as timer is close to overflow
akashvibhute 0:88de7b3bd889 367 bno_timer.reset();
akashvibhute 0:88de7b3bd889 368 imuTime_s[0] = imuTime_s[1];
akashvibhute 0:88de7b3bd889 369 } else
akashvibhute 0:88de7b3bd889 370 imuTime_s[1] = imuTime_s[0] + bno_timer.read();
akashvibhute 0:88de7b3bd889 371
akashvibhute 0:88de7b3bd889 372 time_s = imuTime_s[1]; //imu timestamp
akashvibhute 0:88de7b3bd889 373
akashvibhute 0:88de7b3bd889 374 calib_stat[4] = imu_BNO055.calib;
akashvibhute 0:88de7b3bd889 375 calib_stat[3] = (calib_stat[4] & 0xc0) >> 6;
akashvibhute 0:88de7b3bd889 376 calib_stat[2] = (calib_stat[4] & 0x30) >> 4;
akashvibhute 0:88de7b3bd889 377 calib_stat[1] = (calib_stat[4] & 0x0c) >> 2;
akashvibhute 0:88de7b3bd889 378 calib_stat[0] = (calib_stat[4] & 0x03);
akashvibhute 0:88de7b3bd889 379
akashvibhute 0:88de7b3bd889 380 calibration_status = (imu_BNO055.calib & 0xc0) >> 6; //upper 2 MSB's denoting calibration status
akashvibhute 0:88de7b3bd889 381
akashvibhute 0:88de7b3bd889 382 //if((calib_stat[4] >= 193) && (imu_stabilized[1] == 0)) {
akashvibhute 0:88de7b3bd889 383 if(imu_stabilized[1] == 0) { //for imu only mode
akashvibhute 0:88de7b3bd889 384 if(imu_stabilized[0] == 0)
akashvibhute 0:88de7b3bd889 385 imu_stabilized[0] = 1;
akashvibhute 9:47b6a8530868 386
akashvibhute 0:88de7b3bd889 387 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 388
akashvibhute 0:88de7b3bd889 389 unstable_readings--;
akashvibhute 0:88de7b3bd889 390 }
akashvibhute 0:88de7b3bd889 391
akashvibhute 0:88de7b3bd889 392 initialAcc[0] = generalFunctions::abs_f(imu_BNO055.lia.x);
akashvibhute 0:88de7b3bd889 393 initialAcc[1] = generalFunctions::abs_f(imu_BNO055.lia.y);
akashvibhute 0:88de7b3bd889 394 initialAcc[2] = generalFunctions::abs_f(imu_BNO055.lia.z);
akashvibhute 0:88de7b3bd889 395
akashvibhute 0:88de7b3bd889 396 if(unstable_readings <= 1) {
akashvibhute 0:88de7b3bd889 397 //imu_initialAngles[0] = imu_BNO055.euler.pitch;
akashvibhute 0:88de7b3bd889 398 //imu_initialAngles[1] = imu_BNO055.euler.roll;
akashvibhute 0:88de7b3bd889 399 imu_initialAngles[2] = 2*RAD_TO_DEG(M_PI) - imu_BNO055.euler.yaw;
akashvibhute 0:88de7b3bd889 400
akashvibhute 0:88de7b3bd889 401 imu_initialAngles[0] = 0;
akashvibhute 0:88de7b3bd889 402 imu_initialAngles[1] = 0;
akashvibhute 0:88de7b3bd889 403
akashvibhute 0:88de7b3bd889 404 imu_stabilized[1] =1;
akashvibhute 0:88de7b3bd889 405
akashvibhute 0:88de7b3bd889 406 bno_timer.reset();
akashvibhute 0:88de7b3bd889 407 }
akashvibhute 0:88de7b3bd889 408 }
akashvibhute 0:88de7b3bd889 409
akashvibhute 0:88de7b3bd889 410 posePRY[0] = imu_BNO055.euler.pitch - imu_initialAngles[0] - PoseCorrection[0];
akashvibhute 0:88de7b3bd889 411 posePRY[1] = imu_BNO055.euler.roll - imu_initialAngles[1] - PoseCorrection[1];
akashvibhute 0:88de7b3bd889 412 //posePRY[2] = -1.0*(imu_BNO055.euler.yaw - imu_initialAngles[0]- DRIFT_CORRECTION(imuTime_s[1])) - PoseCorrection[2];
akashvibhute 0:88de7b3bd889 413 //posePRY[2] = -1.0*(imu_BNO055.euler.yaw - imu_initialAngles[0]) - PoseCorrection[2];
akashvibhute 5:42b4f2fb593f 414 posePRY[2] = (2*RAD_TO_DEG(M_PI) - imu_BNO055.euler.yaw) - imu_initialAngles[2] - PoseCorrection[2];
akashvibhute 0:88de7b3bd889 415
akashvibhute 0:88de7b3bd889 416 //convert angles to 0 to 2pi range
akashvibhute 9:47b6a8530868 417
akashvibhute 0:88de7b3bd889 418 for(int i=0; i<3; i++) {
akashvibhute 0:88de7b3bd889 419 if(posePRY[i] > 2*RAD_TO_DEG(M_PI))
akashvibhute 0:88de7b3bd889 420 posePRY[i] -= 2*RAD_TO_DEG(M_PI);
akashvibhute 0:88de7b3bd889 421 if(posePRY[i] < 0.0)
akashvibhute 0:88de7b3bd889 422 posePRY[i] += 2*RAD_TO_DEG(M_PI);
akashvibhute 0:88de7b3bd889 423 }
akashvibhute 0:88de7b3bd889 424
akashvibhute 0:88de7b3bd889 425
akashvibhute 0:88de7b3bd889 426 //moving window average of pose angle
akashvibhute 0:88de7b3bd889 427 for(int i=0; i<3; i++) {
akashvibhute 9:47b6a8530868 428 movWindow_Pose[i][movWindow_index_Pose] = posePRY[i];
akashvibhute 9:47b6a8530868 429 Pose[i] = generalFunctions::moving_window(movWindow_Pose[i], movWindow_len_Pose);
akashvibhute 0:88de7b3bd889 430 }
akashvibhute 0:88de7b3bd889 431
akashvibhute 0:88de7b3bd889 432 /** Todo: offset->filter quaternion, use this quaternion to generate gravity vector, and consequently aaReal and aaWorld **/
akashvibhute 0:88de7b3bd889 433
akashvibhute 0:88de7b3bd889 434 Quat[0]= imu_BNO055.quat.w;
akashvibhute 0:88de7b3bd889 435 Quat[1]= imu_BNO055.quat.x;
akashvibhute 0:88de7b3bd889 436 Quat[2]= imu_BNO055.quat.y;
akashvibhute 0:88de7b3bd889 437 Quat[3]= imu_BNO055.quat.z;
akashvibhute 0:88de7b3bd889 438
akashvibhute 0:88de7b3bd889 439 //moving window average of gyro velocities
akashvibhute 0:88de7b3bd889 440 movWindow_AngVel[0][movWindow_index_GyroAcc] = imu_BNO055.gyro.x;
akashvibhute 0:88de7b3bd889 441 movWindow_AngVel[1][movWindow_index_GyroAcc] = imu_BNO055.gyro.y;
akashvibhute 0:88de7b3bd889 442 movWindow_AngVel[2][movWindow_index_GyroAcc] = imu_BNO055.gyro.z;
akashvibhute 0:88de7b3bd889 443
akashvibhute 3:d7a72ce26117 444 movWindow_LinAcc[0][movWindow_index_GyroAcc] = imu_BNO055.lia.x * 9.81 * 1000; //convert to mm/s2
akashvibhute 3:d7a72ce26117 445 movWindow_LinAcc[1][movWindow_index_GyroAcc] = imu_BNO055.lia.y * 9.81 * 1000; //convert to mm/s2
akashvibhute 3:d7a72ce26117 446 movWindow_LinAcc[2][movWindow_index_GyroAcc] = imu_BNO055.lia.z * 9.81 * 1000; //convert to mm/s2
akashvibhute 0:88de7b3bd889 447
akashvibhute 0:88de7b3bd889 448 for(int i=0; i<3; i++) {
akashvibhute 0:88de7b3bd889 449 AngVel[i] = generalFunctions::moving_window(movWindow_AngVel[i]/**/, movWindow_index_GyroAcc); /****** PROBABLY WILL BREAK HERE!!! ******/
akashvibhute 0:88de7b3bd889 450 LinAcc[i] = generalFunctions::moving_window(movWindow_LinAcc[i]/**/, movWindow_index_GyroAcc); /****** PROBABLY WILL BREAK HERE!!! ******/
akashvibhute 0:88de7b3bd889 451 }
akashvibhute 0:88de7b3bd889 452
akashvibhute 0:88de7b3bd889 453 movWindow_index_Pose++;
akashvibhute 0:88de7b3bd889 454 if(movWindow_index_Pose >= movWindow_len_Pose) movWindow_index_Pose=0;
akashvibhute 0:88de7b3bd889 455
akashvibhute 0:88de7b3bd889 456 movWindow_index_GyroAcc++;
akashvibhute 0:88de7b3bd889 457 if(movWindow_index_GyroAcc >= movWindow_len_GyroAcc) movWindow_index_GyroAcc=0;
akashvibhute 0:88de7b3bd889 458
akashvibhute 0:88de7b3bd889 459 //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 460 }
akashvibhute 0:88de7b3bd889 461
akashvibhute 0:88de7b3bd889 462 void IMU_BNO055::getPose(float *pose[3])
akashvibhute 0:88de7b3bd889 463 {
akashvibhute 9:47b6a8530868 464 //pitch about X, roll about Y, yaw about Z
akashvibhute 9:47b6a8530868 465 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 466 *pose[i]=Pose[i];
akashvibhute 0:88de7b3bd889 467 }
akashvibhute 0:88de7b3bd889 468
akashvibhute 0:88de7b3bd889 469 void IMU_BNO055::getQuat(float *quaternion[4])
akashvibhute 0:88de7b3bd889 470 {
akashvibhute 0:88de7b3bd889 471 for(int i=0; i<4; i++)
akashvibhute 0:88de7b3bd889 472 *quaternion[i]=Quat[i];
akashvibhute 0:88de7b3bd889 473 }
akashvibhute 0:88de7b3bd889 474
akashvibhute 0:88de7b3bd889 475 void IMU_BNO055::getAngVel(float *angVelocity[3])
akashvibhute 0:88de7b3bd889 476 {
akashvibhute 0:88de7b3bd889 477 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 478 *angVelocity[i]=AngVel[i];
akashvibhute 0:88de7b3bd889 479 }
akashvibhute 0:88de7b3bd889 480
akashvibhute 0:88de7b3bd889 481 void IMU_BNO055::getLinAcc(float *linAcc[3])
akashvibhute 0:88de7b3bd889 482 {
akashvibhute 0:88de7b3bd889 483 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 484 *linAcc[i]=LinAcc[i];
akashvibhute 0:88de7b3bd889 485 }
akashvibhute 0:88de7b3bd889 486
akashvibhute 0:88de7b3bd889 487 float IMU_BNO055::getTime()
akashvibhute 0:88de7b3bd889 488 {
akashvibhute 0:88de7b3bd889 489 return(imuTime_s[1]);
akashvibhute 0:88de7b3bd889 490 }
akashvibhute 0:88de7b3bd889 491
akashvibhute 0:88de7b3bd889 492 void IMU_BNO055::setPose(float pose_in[3])
akashvibhute 0:88de7b3bd889 493 {
akashvibhute 0:88de7b3bd889 494 IMU_BNO055::imuUpdate(); //refresh imu before updating Pose
akashvibhute 9:47b6a8530868 495
akashvibhute 0:88de7b3bd889 496 //pitch about X, roll about Y, yaw about Z
akashvibhute 9:47b6a8530868 497 for(int i=0; i<3; i++)
akashvibhute 0:88de7b3bd889 498 PoseCorrection[i]=(Pose[i] - pose_in[i]);
akashvibhute 0:88de7b3bd889 499 }
akashvibhute 0:88de7b3bd889 500
akashvibhute 0:88de7b3bd889 501 void IMU_BNO055::readCalibrationData()
akashvibhute 0:88de7b3bd889 502 {
akashvibhute 0:88de7b3bd889 503 //imu_BNO055.get_calib();
akashvibhute 0:88de7b3bd889 504 imu_BNO055.read_calibration_data();
akashvibhute 9:47b6a8530868 505
akashvibhute 0:88de7b3bd889 506 for(int i=0; i<22; i++)
akashvibhute 0:88de7b3bd889 507 calibration_regs[i]=imu_BNO055.calibration[i];
akashvibhute 9:47b6a8530868 508
akashvibhute 0:88de7b3bd889 509 offset_acc[0] = (calibration_regs[1] + calibration_regs[0]);
akashvibhute 0:88de7b3bd889 510 offset_acc[1] = (calibration_regs[3] + calibration_regs[2]);
akashvibhute 0:88de7b3bd889 511 offset_acc[2] = (calibration_regs[5] + calibration_regs[4]);
akashvibhute 9:47b6a8530868 512
akashvibhute 0:88de7b3bd889 513 offset_mag[0] = (calibration_regs[7] + calibration_regs[6]);
akashvibhute 0:88de7b3bd889 514 offset_mag[1] = (calibration_regs[9] + calibration_regs[8]);
akashvibhute 0:88de7b3bd889 515 offset_mag[2] = (calibration_regs[11] + calibration_regs[10]);
akashvibhute 9:47b6a8530868 516
akashvibhute 0:88de7b3bd889 517 offset_gyr[0] = (calibration_regs[13] + calibration_regs[12]);
akashvibhute 0:88de7b3bd889 518 offset_gyr[1] = (calibration_regs[15] + calibration_regs[14]);
akashvibhute 0:88de7b3bd889 519 offset_gyr[2] = (calibration_regs[17] + calibration_regs[16]);
akashvibhute 9:47b6a8530868 520
akashvibhute 0:88de7b3bd889 521 radius_acc = (calibration_regs[19] + calibration_regs[18]);
akashvibhute 9:47b6a8530868 522 radius_mag = (calibration_regs[21] + calibration_regs[20]);
akashvibhute 0:88de7b3bd889 523 }
akashvibhute 0:88de7b3bd889 524
akashvibhute 0:88de7b3bd889 525 void IMU_BNO055::writeCalibrationData()
akashvibhute 0:88de7b3bd889 526 {
akashvibhute 0:88de7b3bd889 527 //acc
akashvibhute 0:88de7b3bd889 528 calibration_regs[0] = offset_acc[0] & 0b00001111;
akashvibhute 0:88de7b3bd889 529 calibration_regs[1] = offset_acc[0] & 0b11110000;
akashvibhute 9:47b6a8530868 530
akashvibhute 0:88de7b3bd889 531 calibration_regs[2] = offset_acc[1] & 0b00001111;
akashvibhute 0:88de7b3bd889 532 calibration_regs[3] = offset_acc[1] & 0b11110000;
akashvibhute 9:47b6a8530868 533
akashvibhute 0:88de7b3bd889 534 calibration_regs[4] = offset_acc[2] & 0b00001111;
akashvibhute 0:88de7b3bd889 535 calibration_regs[5] = offset_acc[2] & 0b11110000;
akashvibhute 9:47b6a8530868 536
akashvibhute 0:88de7b3bd889 537 //mag
akashvibhute 0:88de7b3bd889 538 calibration_regs[6] = offset_mag[0] & 0b00001111;
akashvibhute 0:88de7b3bd889 539 calibration_regs[7] = offset_mag[0] & 0b11110000;
akashvibhute 9:47b6a8530868 540
akashvibhute 0:88de7b3bd889 541 calibration_regs[8] = offset_mag[1] & 0b00001111;
akashvibhute 0:88de7b3bd889 542 calibration_regs[9] = offset_mag[1] & 0b11110000;
akashvibhute 9:47b6a8530868 543
akashvibhute 0:88de7b3bd889 544 calibration_regs[10] = offset_mag[2] & 0b00001111;
akashvibhute 0:88de7b3bd889 545 calibration_regs[11] = offset_mag[2] & 0b11110000;
akashvibhute 9:47b6a8530868 546
akashvibhute 0:88de7b3bd889 547 //gyro
akashvibhute 0:88de7b3bd889 548 calibration_regs[12] = offset_gyr[0] & 0b00001111;
akashvibhute 0:88de7b3bd889 549 calibration_regs[13] = offset_gyr[0] & 0b11110000;
akashvibhute 9:47b6a8530868 550
akashvibhute 0:88de7b3bd889 551 calibration_regs[14] = offset_gyr[1] & 0b00001111;
akashvibhute 0:88de7b3bd889 552 calibration_regs[15] = offset_gyr[1] & 0b11110000;
akashvibhute 9:47b6a8530868 553
akashvibhute 0:88de7b3bd889 554 calibration_regs[16] = offset_gyr[2] & 0b00001111;
akashvibhute 0:88de7b3bd889 555 calibration_regs[17] = offset_gyr[2] & 0b11110000;
akashvibhute 9:47b6a8530868 556
akashvibhute 0:88de7b3bd889 557 //acc_radius
akashvibhute 0:88de7b3bd889 558 calibration_regs[18] = radius_acc & 0b00001111;
akashvibhute 0:88de7b3bd889 559 calibration_regs[19] = radius_acc & 0b11110000;
akashvibhute 9:47b6a8530868 560
akashvibhute 0:88de7b3bd889 561 //mag_radius
akashvibhute 0:88de7b3bd889 562 calibration_regs[20] = radius_mag & 0b00001111;
akashvibhute 0:88de7b3bd889 563 calibration_regs[21] = radius_mag & 0b11110000;
akashvibhute 9:47b6a8530868 564
akashvibhute 0:88de7b3bd889 565 for(int i=0; i<22; i++)
akashvibhute 0:88de7b3bd889 566 imu_BNO055.calibration[i] = calibration_regs[i];
akashvibhute 9:47b6a8530868 567
akashvibhute 0:88de7b3bd889 568 imu_BNO055.write_calibration_data();
akashvibhute 0:88de7b3bd889 569 }
akashvibhute 0:88de7b3bd889 570
akashvibhute 0:88de7b3bd889 571 /*** ***/