B+IMU+SD
Dependencies: BMI160 RTC SDFileSystem USBDevice max32630fthr
Fork of MPSMAXbutton by
IMU.cpp
00001 #include "IMU.h" 00002 00003 00004 I2C i2cBus(P5_7, P6_0); 00005 BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO); 00006 00007 BMI160::AccConfig accConfig; 00008 BMI160::GyroConfig gyroConfig; 00009 00010 int initIMU(){ 00011 i2cBus.frequency(400000); 00012 00013 // printf("\033[H"); //home 00014 // printf("\033[0J"); //erase from cursor to end of screen 00015 00016 uint32_t failures = 0; 00017 00018 if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR){ 00019 printf("Failed to set gyroscope power mode\n"); 00020 failures++; 00021 } 00022 wait_ms(10); 00023 00024 if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR){ 00025 printf("Failed to set accelerometer power mode\n"); 00026 failures++; 00027 } 00028 wait_ms(10); 00029 00030 00031 //example of using getSensorConfig 00032 if(imu.getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR){ 00033 printf("ACC Range = %d\n", accConfig.range); 00034 printf("ACC UnderSampling = %d\n", accConfig.us); 00035 printf("ACC BandWidthParam = %d\n", accConfig.bwp); 00036 printf("ACC OutputDataRate = %d\n\n", accConfig.odr); 00037 }else{ 00038 printf("Failed to get accelerometer configuration\n"); 00039 failures++; 00040 } 00041 00042 //example of setting user defined configuration 00043 accConfig.range = BMI160::SENS_2G ; 00044 accConfig.us = BMI160::ACC_US_OFF; 00045 accConfig.bwp = BMI160::ACC_BWP_2; 00046 accConfig.odr = BMI160::ACC_ODR_8; 00047 if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR){ 00048 printf("ACC Range = %d\n", accConfig.range); 00049 printf("ACC UnderSampling = %d\n", accConfig.us); 00050 printf("ACC BandWidthParam = %d\n", accConfig.bwp); 00051 printf("ACC OutputDataRate = %d\n\n", accConfig.odr); 00052 }else{ 00053 printf("Failed to set accelerometer configuration\n"); 00054 failures++; 00055 } 00056 00057 if(imu.getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR){ 00058 printf("GYRO Range = %d\n", gyroConfig.range); 00059 printf("GYRO BandWidthParam = %d\n", gyroConfig.bwp); 00060 printf("GYRO OutputDataRate = %d\n\n", gyroConfig.odr); 00061 }else{ 00062 printf("Failed to get gyroscope configuration\n"); 00063 failures++; 00064 } 00065 00066 // printf("\033[H"); //home 00067 // printf("\033[0J"); //erase from cursor to end of screen 00068 00069 if(failures == 0){ 00070 return 0; 00071 }else{ 00072 printf("IMU init failed.. returning 1\n"); 00073 return 1; 00074 } 00075 } 00076 00077 void printIMUData(){ 00078 float imuTemperature; 00079 BMI160::SensorData accData; 00080 BMI160::SensorData gyroData; 00081 BMI160::SensorTime sensorTime; 00082 00083 imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range); 00084 imu.getTemperature(&imuTemperature); 00085 00086 printf("ACC xAxis = %d\n", accData.xAxis.raw); 00087 printf("ACC yAxis = %d\n", accData.yAxis.raw); 00088 printf("ACC zAxis = %d\n\n", accData.zAxis.raw); 00089 00090 printf("GYRO xAxis = %d\n", gyroData.xAxis.raw); 00091 printf("GYRO yAxis = %d\n", gyroData.yAxis.raw); 00092 printf("GYRO zAxis = %d\n\n", gyroData.zAxis.raw); 00093 00094 printf("Sensor Time = %f\n", sensorTime.seconds); 00095 printf("Sensor Temperature = %5.3f\n", imuTemperature); 00096 } 00097 00098 00099 float getIMUAngle(){ 00100 BMI160::SensorData accData; 00101 BMI160::SensorData gyroData; 00102 BMI160::SensorTime sensorTime; 00103 double tempxAngle = 0; 00104 double xAngle =0; 00105 double tempyAngle =0; 00106 double yAngle =0; 00107 double res =0; 00108 00109 imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range); 00110 // printf("X = %d\tY = %d\tZ = %d\t\n", accData.xAxis.raw, accData.yAxis.raw, accData.zAxis.raw); 00111 00112 // uint32_t xVal = accData.xAxis.raw; 00113 // uint32_t yVal = accData.yAxis.raw; 00114 // uint32_t zVal = accData.zAxis.raw; 00115 00116 float xVal = accData.xAxis.scaled; 00117 float yVal = accData.yAxis.scaled; 00118 float zVal = accData.zAxis.scaled; 00119 00120 /* Compute angles in radian */ 00121 tempxAngle = (double)((yVal * yVal) + (zVal * zVal)); 00122 tempyAngle = (double)((xVal * xVal) + (zVal * zVal)); 00123 xAngle = (double)atan (xVal / sqrt (tempxAngle)); 00124 yAngle = (double)atan (yVal / sqrt(tempyAngle)); 00125 /* Convert radian in degree */ 00126 xAngle *= 180.00; 00127 yAngle *= 180.00; //zAngle *=180.00; 00128 xAngle /= M_PI; 00129 yAngle /= M_PI; //zAngle /= 3.141592; 00130 // for x angle 00131 res = xAngle; 00132 return res; 00133 } 00134 00135 00136 00137 // DEFAULT MBED FUNCTIONs.. not required for MPS 00138 void dumpImuRegisters(BMI160 &imu) 00139 { 00140 printRegister(imu, BMI160::CHIP_ID); 00141 printBlock(imu, BMI160::ERR_REG,BMI160::FIFO_DATA); 00142 printBlock(imu, BMI160::ACC_CONF, BMI160::FIFO_CONFIG_1); 00143 printBlock(imu, BMI160::MAG_IF_0, BMI160::SELF_TEST); 00144 printBlock(imu, BMI160::NV_CONF, BMI160::STEP_CONF_1); 00145 printRegister(imu, BMI160::CMD); 00146 printf("\n"); 00147 } 00148 00149 void printRegister(BMI160 &imu, BMI160::Registers reg) 00150 { 00151 uint8_t data; 00152 if(imu.readRegister(reg, &data) == BMI160::RTN_NO_ERROR) 00153 { 00154 printf("IMU Register 0x%02x = 0x%02x\n", reg, data); 00155 } 00156 else 00157 { 00158 printf("Failed to read register\n"); 00159 } 00160 } 00161 00162 void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg) 00163 { 00164 uint8_t numBytes = ((stopReg - startReg) + 1); 00165 uint8_t buff[numBytes]; 00166 uint8_t offset = static_cast<uint8_t>(startReg); 00167 00168 if(imu.readBlock(startReg, stopReg, buff) == BMI160::RTN_NO_ERROR) 00169 { 00170 for(uint8_t idx = offset; idx < (numBytes + offset); idx++) 00171 { 00172 printf("IMU Register 0x%02x = 0x%02x\n", idx, buff[idx - offset]); 00173 } 00174 } 00175 else 00176 { 00177 printf("Failed to read block\n"); 00178 } 00179 } 00180
Generated on Tue Jul 12 2022 19:03:03 by 1.7.2