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@16:52a455fb107a, 2018-05-22 (annotated)

Committer:
ahmed_lv
Date:
Tue May 22 02:22:03 2018 +0000
Revision:
16:52a455fb107a
Parent:
9:47b6a8530868 
commit

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