Diff: imuHandler.cpp

Revision:

0:88de7b3bd889

Child:

1:927f1b3e4bf4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/imuHandler.cpp	Tue Apr 26 10:38:27 2016 +0000
@@ -0,0 +1,514 @@
+#include "imuHandler.h"
+
+/*** MPU6050 ***/
+
+IMU_MPU6050::IMU_MPU6050(): imu_mpu6050(i2c_SDA, i2c_SCL)
+{
+    unsigned int movWindowSize_Pose = Pose_MWindowSize;
+    unsigned int movWindowSize_GyroAcc = GyroAcc_MWindowSize;
+
+    unstable_readings = MPU6050_StabilizationReadings;
+        
+    imu_stabilized=false;
+
+    if(movWindowSize_Pose <= movWindow_lenMax) movWindow_len_Pose = movWindowSize_Pose;
+    else movWindow_len_Pose = movWindow_lenMax;
+
+    if(movWindow_len_GyroAcc <= movWindow_lenMax) movWindow_len_GyroAcc = movWindowSize_GyroAcc;
+    else movWindow_len_GyroAcc = movWindow_lenMax;
+
+    movWindow_index_Pose=0;
+    movWindow_index_GyroAcc=0;
+
+    for(int i=0; i<3; i++)
+        PoseCorrection[i]=0;
+}
+
+bool IMU_MPU6050::imuInit()
+{
+    //debug_printf("MPU6050 initializing.\n");
+
+    imu_mpu6050.initialize();
+    if (imu_mpu6050.testConnection()) {
+        //debug_printf("MPU6050 test connection passed.\n");
+    } else {
+        //debug_printf("MPU6050 test connection failed.\n");
+        return false;
+    }
+    if (imu_mpu6050.dmpInitialize() == 0) {
+        //debug_printf("succeed in MPU6050 DMP Initializing.\n");
+    } else {
+        //debug_printf("failed in MPU6050 DMP Initializing.\n");
+        return false;
+    }
+    imu_mpu6050.setXAccelOffset(MPU6050_Offset_Ax);
+    imu_mpu6050.setYAccelOffset(MPU6050_Offset_Ay);
+    imu_mpu6050.setZAccelOffset(MPU6050_Offset_Az);
+    imu_mpu6050.setFullScaleGyroRange(MPU6050_GYRO_FS);
+    imu_mpu6050.setFullScaleAccelRange(MPU6050_ACCEL_FS);
+    imu_mpu6050.setXGyroOffsetUser(MPU6050_Offset_Gx);
+    imu_mpu6050.setYGyroOffsetUser(MPU6050_Offset_Gy);
+    imu_mpu6050.setZGyroOffsetUser(MPU6050_Offset_Gz);
+    imu_mpu6050.setDMPEnabled(true);    // Enable DMP
+    packetSize = imu_mpu6050.dmpGetFIFOPacketSize();
+
+    //debug_printf("Init finish!\n");
+
+    return true;
+}
+
+void IMU_MPU6050::imuUpdate()
+{
+    mpuIntStatus = imu_mpu6050.getIntStatus();
+    fifoCount = imu_mpu6050.getFIFOCount();
+
+    // Check that this interrupt is a FIFO buffer overflow interrupt.
+    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
+
+        /*imu_mpu6050.resetFIFO();
+        debug_printf("FIFO overflow!\n");*/
+        return;
+
+        // Check that this interrupt is a Data Ready interrupt.
+    }
+
+    else if (mpuIntStatus & 0x02) {
+        while (fifoCount < packetSize) fifoCount = imu_mpu6050.getFIFOCount();
+
+        if(unstable_readings >= 1) {
+
+            imu_mpu6050.getFIFOBytes(fifoBuffer, packetSize);
+
+            imu_mpu6050.dmpGetQuaternion(&imu_Data.q, fifoBuffer);
+            imu_mpu6050.dmpGetGravity(&imu_Data.gravity, &imu_Data.q);
+
+            imu_mpu6050.dmpGetAccel(&imu_Data.aa, fifoBuffer);
+            imu_mpu6050.dmpGetLinearAccel(&imu_Data.aaReal, &imu_Data.aa, &imu_Data.gravity);
+
+            imu_mpu6050.dmpGetLinearAccelInWorld(&imu_Data.aaWorld, &imu_Data.aaReal, &imu_Data.q);
+
+            if((generalFunctions::abs_f(imu_Data.aaWorld.x) <= 8) && (generalFunctions::abs_f(imu_Data.aaWorld.y) <= 8)) {
+                if(unstable_readings > 1)
+                    unstable_readings --;
+                else {
+                    imu_mpu6050.dmpGetYawPitchRoll(imu_initialAngles, &imu_Data.q, &imu_Data.gravity);
+                    mpu_timer.start();
+                    unstable_readings --;
+
+                    imu_stabilized = true;
+                    //debug_printf("\nIMU stabilized\n");
+                }
+            }
+        }
+
+        else {
+            float yawPitchRoll[3];
+            int16_t raw_gyro[3];
+            imu_mpu6050.getFIFOBytes(fifoBuffer, packetSize);
+            imu_mpu6050.dmpGetQuaternion(&imu_Data.q, fifoBuffer);
+            imu_mpu6050.dmpGetGravity(&imu_Data.gravity, &imu_Data.q);
+            imu_mpu6050.dmpGetYawPitchRoll(yawPitchRoll, &imu_Data.q, &imu_Data.gravity);
+
+            //debug_printf("DEBUG>> got YPR data\n");
+
+            if(mpu_timer.read() > 2100) { //reset as timer is close to overflow
+                mpu_timer.reset();
+                imuTime_s[0] = imuTime_s[1];
+            } else
+                imuTime_s[1] = imuTime_s[0] + mpu_timer.read();
+
+            time_s = imuTime_s[1]; //imu timestamp
+
+            imu_Data.posePRY[2] = -1*(yawPitchRoll[0] - imu_initialAngles[0]- DRIFT_CORRECTION(imuTime_s[1])) - PoseCorrection[0];
+            imu_Data.posePRY[1] = yawPitchRoll[1] - imu_initialAngles[1] - PoseCorrection[1];
+            imu_Data.posePRY[0] = yawPitchRoll[2] - imu_initialAngles[2] - PoseCorrection[2];
+
+            //convert angles to 0 to 2pi range
+            for(int i=0; i<3; i++) {
+                if(imu_Data.posePRY[i] > 2*M_PI)  imu_Data.posePRY[i] -= 2*M_PI;
+                if(imu_Data.posePRY[i] < 0)       imu_Data.posePRY[i] += 2*M_PI;
+            }
+
+            //moving window average of pose angle
+            for(int i=0; i<3; i++) {
+                movWindow_Pose[i][movWindow_index_Pose] = RAD_TO_DEG(imu_Data.posePRY[i]);
+                Pose[i] = generalFunctions::moving_window(movWindow_Pose[i]/**/, movWindow_len_Pose);    /****** PROBABLY WILL BREAK HERE!!! ******/
+            }
+
+            /** Todo: offset->filter quaternion, use this quaternion to generate gravity vector, and consequently aaReal and aaWorld  **/
+
+            //convert filtered pose to quaternion here
+
+            Quat[0]= imu_Data.q.w;
+            Quat[1]= imu_Data.q.x;
+            Quat[2]= imu_Data.q.y;
+            Quat[3]= imu_Data.q.z;
+
+            //imu_mpu6050.dmpGetGravity(&imu_Data.gravity, &imu_Data.q /* filtered quaternion */);
+
+            /***************************************/
+
+            imu_mpu6050.dmpGetAccel(&imu_Data.aa, fifoBuffer);
+            imu_mpu6050.dmpGetGyro(raw_gyro, fifoBuffer);
+            imu_mpu6050.dmpGetLinearAccel(&imu_Data.aaReal, &imu_Data.aa, &imu_Data.gravity);
+            imu_mpu6050.dmpGetLinearAccelInWorld(&imu_Data.aaWorld, &imu_Data.aaReal, &imu_Data.q);
+
+            imu_Data.accW.x = (float) imu_Data.aaWorld.x * (9806.0/8192);
+            imu_Data.accW.y = (float) imu_Data.aaWorld.y * (9806.0/8192);
+            imu_Data.accW.z = (float) imu_Data.aaWorld.z * (9806.0/8192);
+
+            imu_Data.gyro.x = (float) raw_gyro[0];
+            imu_Data.gyro.y = (float) raw_gyro[1];
+            imu_Data.gyro.z = (float) raw_gyro[2];
+
+
+            //moving window average of gyro velocities
+            movWindow_AngVel[0][movWindow_index_GyroAcc] = imu_Data.gyro.x;
+            movWindow_AngVel[1][movWindow_index_GyroAcc] = imu_Data.gyro.y;
+            movWindow_AngVel[2][movWindow_index_GyroAcc] = imu_Data.gyro.z;
+
+            movWindow_LinAcc[0][movWindow_index_GyroAcc] = imu_Data.accW.x;
+            movWindow_LinAcc[1][movWindow_index_GyroAcc] = imu_Data.accW.y;
+            movWindow_LinAcc[2][movWindow_index_GyroAcc] = imu_Data.accW.z;
+
+            for(int i=0; i<3; i++) {
+                AngVel[i] = generalFunctions::moving_window(movWindow_AngVel[i]/**/, movWindow_index_GyroAcc);    /****** PROBABLY WILL BREAK HERE!!! ******/
+                LinAcc[i] = generalFunctions::moving_window(movWindow_LinAcc[i]/**/, movWindow_index_GyroAcc);    /****** PROBABLY WILL BREAK HERE!!! ******/
+            }
+
+            movWindow_index_Pose++;
+            if(movWindow_index_Pose >= movWindow_len_Pose) movWindow_index_Pose=0;
+
+            movWindow_index_GyroAcc++;
+            if(movWindow_index_GyroAcc >= movWindow_len_GyroAcc) movWindow_index_GyroAcc=0;
+
+            //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));
+        }
+    }
+}
+
+void IMU_MPU6050::getPose(float *pose[3])
+{
+    //pitch about X, roll about Y, yaw about Z 
+    for(int i=0; i<3; i++)      
+        *pose[i]=Pose[i];
+}
+
+void IMU_MPU6050::getQuat(float *quaternion[4])
+{
+    for(int i=0; i<4; i++)
+        *quaternion[i]=Quat[i];
+}
+
+void IMU_MPU6050::getAngVel(float *angVelocity[3])
+{
+    for(int i=0; i<3; i++)
+        *angVelocity[i]=AngVel[i];
+}
+
+void IMU_MPU6050::getLinAcc(float *linAcc[3])
+{
+    for(int i=0; i<3; i++)
+        *linAcc[i]=LinAcc[i];
+}
+
+float IMU_MPU6050::getTime()
+{
+    return(imuTime_s[1]);
+}
+
+void IMU_MPU6050::setPose(float pose_in[3])
+{
+    IMU_MPU6050::imuUpdate(); //refresh imu before updating Pose
+
+    //pitch about X, roll about Y, yaw about Z   
+    for(int i=0; i<3; i++)   
+        PoseCorrection[i]=(Pose[i] - pose_in[i]);
+}
+
+/***  ***/
+
+/*** BNO055 ***/
+
+IMU_BNO055::IMU_BNO055(): imu_BNO055(i2c_SDA, i2c_SCL)
+{
+    unsigned int movWindowSize_Pose = Pose_MWindowSize;
+    unsigned int movWindowSize_GyroAcc = GyroAcc_MWindowSize;
+
+    unstable_readings = BNO055_StabilizationReadings;
+    //imu_stabilized[0] =0;
+    imu_stabilized[0] =1; //for imu mode no need to calibrate
+    imu_stabilized[1] =0;
+
+    if(movWindowSize_Pose <= movWindow_lenMax) movWindow_len_Pose = movWindowSize_Pose;
+    else movWindow_len_Pose = movWindow_lenMax;
+
+    if(movWindow_len_GyroAcc <= movWindow_lenMax) movWindow_len_GyroAcc = movWindowSize_GyroAcc;
+    else movWindow_len_GyroAcc = movWindow_lenMax;
+
+    movWindow_index_Pose=0;
+    movWindow_index_GyroAcc=0;
+
+    for(int i=0; i<3; i++)
+        PoseCorrection[i]=0;
+    
+    bno_timer.start();    
+}
+
+bool IMU_BNO055::imuInit()
+{
+    imu_BNO055.reset();
+
+    if (imu_BNO055.check()) {
+        
+        //load default calib parameters
+        offset_acc[0] =10;
+        offset_acc[1] =509;
+        offset_acc[2] =500;
+
+        offset_mag[0] =409;
+        offset_mag[1] =487;
+        offset_mag[2] =290;
+
+        offset_gyr[0] =4;
+        offset_gyr[0] =0;
+        offset_gyr[2] =1;
+
+        radius_acc = 235;
+        radius_mag = 165;
+
+        writeCalibrationData();
+
+        imu_BNO055.set_mapping(2);
+        imu_BNO055.setmode(OPERATION_MODE_IMUPLUS);
+
+        imu_BNO055.get_calib();
+        calibration_status = (imu_BNO055.calib & 0xc0) >> 6; //upper 2 MSB's denoting calibration status
+
+        return(true);
+    } else return(false);
+
+    //debug_printf("Init finish!\n");
+}
+
+void IMU_BNO055::imuUpdate()
+{
+    imu_BNO055.get_calib();
+    imu_BNO055.get_angles();
+
+    imu_BNO055.get_lia(); //imu_BNO055.get_accel();
+    imu_BNO055.get_gyro();
+    //imu_BNO055.get_mag();
+    imu_BNO055.get_quat();
+    //imu_BNO055.get_grv();
+
+    float posePRY[3];
+
+    //debug_printf("DEBUG>> got YPR data\n");
+
+    if(bno_timer.read() > 2100) { //reset as timer is close to overflow
+        bno_timer.reset();
+        imuTime_s[0] = imuTime_s[1];
+    } else
+        imuTime_s[1] = imuTime_s[0] + bno_timer.read();
+
+    time_s = imuTime_s[1]; //imu timestamp
+
+    calib_stat[4] = imu_BNO055.calib;
+    calib_stat[3] = (calib_stat[4] & 0xc0) >> 6;
+    calib_stat[2] = (calib_stat[4] & 0x30) >> 4;
+    calib_stat[1] = (calib_stat[4] & 0x0c) >> 2;
+    calib_stat[0] = (calib_stat[4] & 0x03);
+
+    calibration_status = (imu_BNO055.calib & 0xc0) >> 6; //upper 2 MSB's denoting calibration status
+
+    //if((calib_stat[4] >= 193) && (imu_stabilized[1] == 0)) {
+    if(imu_stabilized[1] == 0) { //for imu only mode
+        if(imu_stabilized[0] == 0)
+            imu_stabilized[0] = 1;
+            
+        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)) {
+
+            unstable_readings--;
+        }
+
+        initialAcc[0] = generalFunctions::abs_f(imu_BNO055.lia.x);
+        initialAcc[1] = generalFunctions::abs_f(imu_BNO055.lia.y);
+        initialAcc[2] = generalFunctions::abs_f(imu_BNO055.lia.z);
+
+        if(unstable_readings <= 1) {
+            //imu_initialAngles[0] = imu_BNO055.euler.pitch;
+            //imu_initialAngles[1] = imu_BNO055.euler.roll;
+            imu_initialAngles[2] = 2*RAD_TO_DEG(M_PI) - imu_BNO055.euler.yaw;
+
+            imu_initialAngles[0] = 0;
+            imu_initialAngles[1] = 0;
+
+            imu_stabilized[1] =1;
+
+            bno_timer.reset();
+        }
+    }
+
+    posePRY[0] = imu_BNO055.euler.pitch - imu_initialAngles[0] - PoseCorrection[0];
+    posePRY[1] = imu_BNO055.euler.roll - imu_initialAngles[1] - PoseCorrection[1];
+    //posePRY[2] = -1.0*(imu_BNO055.euler.yaw - imu_initialAngles[0]- DRIFT_CORRECTION(imuTime_s[1])) - PoseCorrection[2];
+    //posePRY[2] = -1.0*(imu_BNO055.euler.yaw - imu_initialAngles[0]) - PoseCorrection[2];
+    posePRY[2] = (2*RAD_TO_DEG(M_PI) - imu_BNO055.euler.yaw) - imu_initialAngles[2];
+
+    //convert angles to 0 to 2pi range
+
+    for(int i=0; i<3; i++) {
+        if(posePRY[i] > 2*RAD_TO_DEG(M_PI))
+            posePRY[i] -= 2*RAD_TO_DEG(M_PI);
+        if(posePRY[i] < 0.0)
+            posePRY[i] += 2*RAD_TO_DEG(M_PI);
+    }
+
+
+    //moving window average of pose angle
+    for(int i=0; i<3; i++) {
+        movWindow_Pose[i][movWindow_index_Pose] = posePRY[i];
+        Pose[i] = generalFunctions::moving_window(movWindow_Pose[i], movWindow_len_Pose);
+    }
+
+    /** Todo: offset->filter quaternion, use this quaternion to generate gravity vector, and consequently aaReal and aaWorld  **/
+
+    Quat[0]= imu_BNO055.quat.w;
+    Quat[1]= imu_BNO055.quat.x;
+    Quat[2]= imu_BNO055.quat.y;
+    Quat[3]= imu_BNO055.quat.z;
+
+    //moving window average of gyro velocities
+    movWindow_AngVel[0][movWindow_index_GyroAcc] = imu_BNO055.gyro.x;
+    movWindow_AngVel[1][movWindow_index_GyroAcc] = imu_BNO055.gyro.y;
+    movWindow_AngVel[2][movWindow_index_GyroAcc] = imu_BNO055.gyro.z;
+
+    movWindow_LinAcc[0][movWindow_index_GyroAcc] = imu_BNO055.lia.x;
+    movWindow_LinAcc[1][movWindow_index_GyroAcc] = imu_BNO055.lia.y;
+    movWindow_LinAcc[2][movWindow_index_GyroAcc] = imu_BNO055.lia.z;
+
+    for(int i=0; i<3; i++) {
+        AngVel[i] = generalFunctions::moving_window(movWindow_AngVel[i]/**/, movWindow_index_GyroAcc);    /****** PROBABLY WILL BREAK HERE!!! ******/
+        LinAcc[i] = generalFunctions::moving_window(movWindow_LinAcc[i]/**/, movWindow_index_GyroAcc);    /****** PROBABLY WILL BREAK HERE!!! ******/
+    }
+
+    movWindow_index_Pose++;
+    if(movWindow_index_Pose >= movWindow_len_Pose) movWindow_index_Pose=0;
+
+    movWindow_index_GyroAcc++;
+    if(movWindow_index_GyroAcc >= movWindow_len_GyroAcc) movWindow_index_GyroAcc=0;
+
+    //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));
+}
+
+void IMU_BNO055::getPose(float *pose[3])
+{
+    //pitch about X, roll about Y, yaw about Z 
+    for(int i=0; i<3; i++)      
+        *pose[i]=Pose[i];
+}
+
+void IMU_BNO055::getQuat(float *quaternion[4])
+{
+    for(int i=0; i<4; i++)
+        *quaternion[i]=Quat[i];
+}
+
+void IMU_BNO055::getAngVel(float *angVelocity[3])
+{
+    for(int i=0; i<3; i++)
+        *angVelocity[i]=AngVel[i];
+}
+
+void IMU_BNO055::getLinAcc(float *linAcc[3])
+{
+    for(int i=0; i<3; i++)
+        *linAcc[i]=LinAcc[i];
+}
+
+float IMU_BNO055::getTime()
+{
+    return(imuTime_s[1]);
+}
+
+void IMU_BNO055::setPose(float pose_in[3])
+{
+    IMU_BNO055::imuUpdate(); //refresh imu before updating Pose
+    
+    //pitch about X, roll about Y, yaw about Z
+    for(int i=0; i<3; i++)   
+        PoseCorrection[i]=(Pose[i] - pose_in[i]);
+}
+
+void IMU_BNO055::readCalibrationData()
+{
+    //imu_BNO055.get_calib();
+    imu_BNO055.read_calibration_data();
+    
+    for(int i=0; i<22; i++)
+        calibration_regs[i]=imu_BNO055.calibration[i];
+        
+    offset_acc[0] = (calibration_regs[1] + calibration_regs[0]);
+    offset_acc[1] = (calibration_regs[3] + calibration_regs[2]);
+    offset_acc[2] = (calibration_regs[5] + calibration_regs[4]);
+    
+    offset_mag[0] = (calibration_regs[7] + calibration_regs[6]);
+    offset_mag[1] = (calibration_regs[9] + calibration_regs[8]);
+    offset_mag[2] = (calibration_regs[11] + calibration_regs[10]);
+    
+    offset_gyr[0] = (calibration_regs[13] + calibration_regs[12]);
+    offset_gyr[1] = (calibration_regs[15] + calibration_regs[14]);
+    offset_gyr[2] = (calibration_regs[17] + calibration_regs[16]);
+    
+    radius_acc = (calibration_regs[19] + calibration_regs[18]);
+    radius_mag = (calibration_regs[21] + calibration_regs[20]);    
+}
+
+void IMU_BNO055::writeCalibrationData()
+{
+    //acc
+    calibration_regs[0] = offset_acc[0] & 0b00001111;
+    calibration_regs[1] = offset_acc[0] & 0b11110000;
+    
+    calibration_regs[2] = offset_acc[1] & 0b00001111;
+    calibration_regs[3] = offset_acc[1] & 0b11110000;
+    
+    calibration_regs[4] = offset_acc[2] & 0b00001111;
+    calibration_regs[5] = offset_acc[2] & 0b11110000;
+    
+    //mag
+    calibration_regs[6] = offset_mag[0] & 0b00001111;
+    calibration_regs[7] = offset_mag[0] & 0b11110000;
+    
+    calibration_regs[8] = offset_mag[1] & 0b00001111;
+    calibration_regs[9] = offset_mag[1] & 0b11110000;
+    
+    calibration_regs[10] = offset_mag[2] & 0b00001111;
+    calibration_regs[11] = offset_mag[2] & 0b11110000;
+    
+    //gyro
+    calibration_regs[12] = offset_gyr[0] & 0b00001111;
+    calibration_regs[13] = offset_gyr[0] & 0b11110000;
+    
+    calibration_regs[14] = offset_gyr[1] & 0b00001111;
+    calibration_regs[15] = offset_gyr[1] & 0b11110000;
+    
+    calibration_regs[16] = offset_gyr[2] & 0b00001111;
+    calibration_regs[17] = offset_gyr[2] & 0b11110000;
+    
+    //acc_radius
+    calibration_regs[18] = radius_acc & 0b00001111;
+    calibration_regs[19] = radius_acc & 0b11110000;
+    
+    //mag_radius
+    calibration_regs[20] = radius_mag & 0b00001111;
+    calibration_regs[21] = radius_mag & 0b11110000;
+    
+    for(int i=0; i<22; i++)
+        imu_BNO055.calibration[i] = calibration_regs[i];
+    
+    imu_BNO055.write_calibration_data();
+}
+
+/***  ***/
\ No newline at end of file