button
Dependencies: BMI160 SDFileSystem USBDevice max32630fthr
Fork of MPSMAX by
IMU.cpp
- Committer:
- FaizanAhmad
- Date:
- 2018-05-08
- Revision:
- 2:5767f3f6909f
- Parent:
- 1:6b969a803e1b
File content as of revision 2:5767f3f6909f:
#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"); } }