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