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@6:70818c388698, 2016-05-16 (annotated)

Committer:
akashvibhute
Date:
Mon May 16 08:00:42 2016 +0000
Revision:
6:70818c388698
Parent:
5:42b4f2fb593f
Child:
7:132bd7a919f5
corrected moving window averaging method for pose angles to account for incoorect average when pose jumps between 1st and 4th quadrant

Who changed what in which revision?

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