File content as of revision 7:132bd7a919f5:

/*
* Copyright (C) 2016 Akash Vibhute <akash.roboticist@gmail.com>
*
* Wrapper to handle all quirks of MPU9250 and BNO055 IMU on Virgo 3 robot.
* Based off various user libraries
*
*
* Initial Release: Apr/26/2016
*
*/

#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*RAD_TO_DEG(M_PI))
                    imu_Data.posePRY[i] -= 2*RAD_TO_DEG(M_PI);
                if(imu_Data.posePRY[i] < 0.0)
                    imu_Data.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] = sin(imu_Data.posePRY[i]);
                Pose[i] = asin(generalFunctions::moving_window(movWindow_Pose[i], movWindow_len_Pose));
            }

            //convert angles to 0 to 2pi range
            for(int i=0; i<3; i++) {
                if(imu_Data.posePRY[i] > 2*RAD_TO_DEG(M_PI))
                    imu_Data.posePRY[i] -= 2*RAD_TO_DEG(M_PI);
                if(imu_Data.posePRY[i] < 0.0)
                    imu_Data.posePRY[i] += 2*RAD_TO_DEG(M_PI);
            }

            /** 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] - PoseCorrection[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] = sin(posePRY[i]);
        Pose[i] = asin(generalFunctions::moving_window(movWindow_Pose[i], movWindow_len_Pose));
    }

    //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);
    }

    /** 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 * 9.81 * 1000; //convert to mm/s2
    movWindow_LinAcc[1][movWindow_index_GyroAcc] = imu_BNO055.lia.y * 9.81 * 1000; //convert to mm/s2
    movWindow_LinAcc[2][movWindow_index_GyroAcc] = imu_BNO055.lia.z * 9.81 * 1000; //convert to mm/s2

    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();
}

/***  ***/