B+IMU+SD
Dependencies: BMI160 RTC SDFileSystem USBDevice max32630fthr
Fork of MPSMAXbutton by
Diff: IMU.cpp
- Revision:
- 3:bd223559f79b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMU.cpp Wed May 09 11:01:18 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"); + } +} +