File content as of revision 3:bd223559f79b:

#include "IMU.h"


I2C i2cBus(P5_7, P6_0);
BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO);

BMI160::AccConfig accConfig;
BMI160::GyroConfig gyroConfig;

int initIMU(){
   i2cBus.frequency(400000);

    // printf("\033[H");  //home
    // printf("\033[0J");  //erase from cursor to end of screen
    
    uint32_t failures = 0;
    
    if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR){
        printf("Failed to set gyroscope power mode\n");
        failures++;
    }
    wait_ms(10);
    
    if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR){
        printf("Failed to set accelerometer power mode\n");
        failures++;
    }
    wait_ms(10);
    
    
    //example of using getSensorConfig
    if(imu.getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR){
        printf("ACC Range = %d\n", accConfig.range);
        printf("ACC UnderSampling = %d\n", accConfig.us);
        printf("ACC BandWidthParam = %d\n", accConfig.bwp);
        printf("ACC OutputDataRate = %d\n\n", accConfig.odr);
    }else{
        printf("Failed to get accelerometer configuration\n");
        failures++;
    }
    
    //example of setting user defined configuration
    accConfig.range = BMI160::SENS_2G ;
    accConfig.us = BMI160::ACC_US_OFF;
    accConfig.bwp = BMI160::ACC_BWP_2;
    accConfig.odr = BMI160::ACC_ODR_8;
    if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR){
        printf("ACC Range = %d\n", accConfig.range);
        printf("ACC UnderSampling = %d\n", accConfig.us);
        printf("ACC BandWidthParam = %d\n", accConfig.bwp);
        printf("ACC OutputDataRate = %d\n\n", accConfig.odr);
    }else{
        printf("Failed to set accelerometer configuration\n");
        failures++;
    }
    
    if(imu.getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR){
        printf("GYRO Range = %d\n", gyroConfig.range);
        printf("GYRO BandWidthParam = %d\n", gyroConfig.bwp);
        printf("GYRO OutputDataRate = %d\n\n", gyroConfig.odr);
    }else{
        printf("Failed to get gyroscope configuration\n");
        failures++;
    }
    
    // printf("\033[H");  //home
    // printf("\033[0J");  //erase from cursor to end of screen
    
    if(failures == 0){
        return 0;
    }else{
        printf("IMU init failed.. returning 1\n");
        return 1;
    }
}

void printIMUData(){
    float imuTemperature; 
    BMI160::SensorData accData;
    BMI160::SensorData gyroData;
    BMI160::SensorTime sensorTime;

    imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
    imu.getTemperature(&imuTemperature);

    printf("ACC xAxis = %d\n", accData.xAxis.raw);
    printf("ACC yAxis = %d\n", accData.yAxis.raw);
    printf("ACC zAxis = %d\n\n", accData.zAxis.raw);

    printf("GYRO xAxis = %d\n", gyroData.xAxis.raw);
    printf("GYRO yAxis = %d\n", gyroData.yAxis.raw);
    printf("GYRO zAxis = %d\n\n", gyroData.zAxis.raw);

    printf("Sensor Time = %f\n", sensorTime.seconds);
    printf("Sensor Temperature = %5.3f\n", imuTemperature);
}


float getIMUAngle(){
    BMI160::SensorData accData;
    BMI160::SensorData gyroData;
    BMI160::SensorTime sensorTime;
    double tempxAngle = 0;
    double xAngle =0;
    double tempyAngle =0;
    double yAngle =0;
    double res =0;

    imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
//    printf("X = %d\tY = %d\tZ = %d\t\n", accData.xAxis.raw, accData.yAxis.raw,  accData.zAxis.raw);
    
    // uint32_t xVal = accData.xAxis.raw;
    // uint32_t yVal = accData.yAxis.raw;
    // uint32_t zVal = accData.zAxis.raw;

    float xVal = accData.xAxis.scaled;
    float yVal = accData.yAxis.scaled;
    float zVal = accData.zAxis.scaled;
    
    /* Compute angles in radian */
    tempxAngle = (double)((yVal * yVal) + (zVal * zVal));
    tempyAngle = (double)((xVal * xVal) + (zVal * zVal));
    xAngle = (double)atan (xVal / sqrt (tempxAngle));
    yAngle = (double)atan (yVal / sqrt(tempyAngle)); 
    /* Convert radian in degree */
    xAngle *= 180.00; 
    yAngle *= 180.00; //zAngle *=180.00;
    xAngle /= M_PI; 
    yAngle /= M_PI; //zAngle /= 3.141592;
        // for x angle         
    res = xAngle;   
    return res;
}



// DEFAULT MBED FUNCTIONs.. not required for MPS
void dumpImuRegisters(BMI160 &imu)
{
    printRegister(imu, BMI160::CHIP_ID);
    printBlock(imu, BMI160::ERR_REG,BMI160::FIFO_DATA);
    printBlock(imu, BMI160::ACC_CONF, BMI160::FIFO_CONFIG_1);
    printBlock(imu, BMI160::MAG_IF_0, BMI160::SELF_TEST);
    printBlock(imu, BMI160::NV_CONF, BMI160::STEP_CONF_1);
    printRegister(imu, BMI160::CMD);
    printf("\n");
}
 
void printRegister(BMI160 &imu, BMI160::Registers reg)
{
    uint8_t data;
    if(imu.readRegister(reg, &data) == BMI160::RTN_NO_ERROR)
    {
        printf("IMU Register 0x%02x = 0x%02x\n", reg, data);
    }
    else
    {
        printf("Failed to read register\n");
    }
}
 
void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg)
{
    uint8_t numBytes = ((stopReg - startReg) + 1);
    uint8_t buff[numBytes];
    uint8_t offset = static_cast<uint8_t>(startReg);
    
    if(imu.readBlock(startReg, stopReg, buff) == BMI160::RTN_NO_ERROR)
    {
        for(uint8_t idx = offset; idx < (numBytes + offset); idx++)
        {
            printf("IMU Register 0x%02x = 0x%02x\n", idx, buff[idx - offset]);
        }
    }
    else
    {
        printf("Failed to read block\n");
    }
}