![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Final Tree
Dependencies: mbed BMI160 max32630fthr_pitch USBDevice Math
Sensors/Orientation.cpp
- Committer:
- bjbance
- Date:
- 2019-01-28
- Revision:
- 7:b33be863fbb5
File content as of revision 7:b33be863fbb5:
#include "mbed.h" #include "bmi160.h" #include "max32630fthr.h" #include "USBSerial.h" #include "stdlib.h" #include "math.h" #include "Orientation.h" I2C i2cBus(I2C2_SDA, I2C2_SCL); BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO); BMI160::SensorData accData; BMI160::SensorData gyroData; BMI160::SensorTime sensorTime; BMI160::AccConfig accConfig; BMI160::GyroConfig gyroConfig; float imuTemperature; Serial pc2(P2_1, P2_0); void Orientation::init(){ pitch = 0; k = 0.65; time1 = sensorTime.seconds; time2 = sensorTime.seconds; i2cBus.frequency(400000); writeReg(imu, BMI160::GYR_RANGE, BMI160::DPS_500); writeReg(imu, BMI160::GYR_CONF, BMI160::GYRO_ODR_13); writeReg(imu, BMI160::FOC_CONF, BMI160::FOC_VALUE ); wait(0.5); writeReg(imu, BMI160::OFFSET_6, BMI160::FOC_ENABLE_GYR_ACC); writeReg(imu, BMI160::CMD, BMI160::FOC_START); pc2.printf("\033[H"); //home pc2.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) { pc2.printf("Failed to set gyroscope power mode\r\n"); failures++; } wait_ms(100); if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) { pc2.printf("Failed to set accelerometer power mode\r\n"); failures++; } wait_ms(100); //example of using getSensorConfig if(imu.getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) { pc2.printf("ACC Range = %d\r\n", accConfig.range); pc2.printf("ACC UnderSampling = %d\r\n", accConfig.us); pc2.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp); pc2.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr); } else { pc2.printf("Failed to get accelerometer configuration\r\n"); failures++; } //example of setting user defined configuration accConfig.range = BMI160::SENS_2G; //rage is 2g accConfig.us = BMI160::ACC_US_OFF; //undersampling is off accConfig.bwp = BMI160::ACC_BWP_0; //average 4 cycles accConfig.odr = BMI160::ACC_ODR_9; //output data rate if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) { pc2.printf("ACC Range = %d\r\n", accConfig.range); pc2.printf("ACC UnderSampling = %d\r\n", accConfig.us); pc2.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp); pc2.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr); } else { pc2.printf("Failed to set accelerometer configuration\r\n"); failures++; } if(imu.getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR) { pc2.printf("GYRO Range = %d\r\n", gyroConfig.range); pc2.printf("GYRO BandWidthParam = %d\r\n", gyroConfig.bwp); pc2.printf("GYRO OutputDataRate = %d\r\n\r\n", gyroConfig.odr); } else { pc2.printf("Failed to get gyroscope configuration\r\n"); failures++; } wait(1.0); } float Orientation::getPitch() { return pitch; } void Orientation::updatePitch() { imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range); imu.getTemperature(&imuTemperature); time2 = sensorTime.seconds; pitch = compFilter(k, pitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, time2 - time1); //daplink.printf("Forward: %s%4.3f\r\n", "\033[K", apitch); time1 = time2; // return pitch; printRegister(imu, BMI160::GYR_CONF); } //***************************************************************************** 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); // pc.printf("\r\n"); } //***************************************************************************** void printRegister(BMI160 &imu, BMI160::Registers reg) { uint8_t data; if(imu.readRegister(reg, &data) == BMI160::RTN_NO_ERROR) { // pc.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data); // daplink.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data); } else { // pc.printf("Failed to read register\r\n"); } } //***************************************************************************** void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data) { imu.writeRegister(reg, data); } //***************************************************************************** void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg) { uint8_t numBytes = ((stopReg - startReg) + 1); uint8_t buff[32]; 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++) { // pc.printf("IMU Register 0x%02x = 0x%02x\r\n", idx, buff[idx - offset]); } } else { // pc.printf("Failed to read block\r\n"); } } float compFilter(float K, float pitch, float gyroX, float accY, float accZ,float DT) { return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * ((180.0F / 3.1459F) * atan(accY/accZ)))); }