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@3:d7a72ce26117, 2016-05-05 (annotated)

Committer:
akashvibhute
Date:
Thu May 05 02:40:09 2016 +0000
Revision:
3:d7a72ce26117
Parent:
1:927f1b3e4bf4
Child:
4:599d5ac25b73
added acceleration conversion factor to convert from g to mm/s2

Who changed what in which revision?

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