Diff: IMU.cpp

Revision:

1:6b969a803e1b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.cpp	Tue May 08 13:45:29 2018 +0000
@@ -0,0 +1,180 @@
+#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");
+    }
+}
+