button
Dependencies: BMI160 SDFileSystem USBDevice max32630fthr
Fork of MPSMAX by
IMU.cpp@2:5767f3f6909f, 2018-05-08 (annotated)
- Committer:
- FaizanAhmad
- Date:
- Tue May 08 13:45:42 2018 +0000
- Revision:
- 2:5767f3f6909f
- Parent:
- 1:6b969a803e1b
\button
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
FaizanAhmad | 1:6b969a803e1b | 1 | #include "IMU.h" |
FaizanAhmad | 1:6b969a803e1b | 2 | |
FaizanAhmad | 1:6b969a803e1b | 3 | |
FaizanAhmad | 1:6b969a803e1b | 4 | I2C i2cBus(P5_7, P6_0); |
FaizanAhmad | 1:6b969a803e1b | 5 | BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO); |
FaizanAhmad | 1:6b969a803e1b | 6 | |
FaizanAhmad | 1:6b969a803e1b | 7 | BMI160::AccConfig accConfig; |
FaizanAhmad | 1:6b969a803e1b | 8 | BMI160::GyroConfig gyroConfig; |
FaizanAhmad | 1:6b969a803e1b | 9 | |
FaizanAhmad | 1:6b969a803e1b | 10 | int initIMU(){ |
FaizanAhmad | 1:6b969a803e1b | 11 | i2cBus.frequency(400000); |
FaizanAhmad | 1:6b969a803e1b | 12 | |
FaizanAhmad | 1:6b969a803e1b | 13 | // printf("\033[H"); //home |
FaizanAhmad | 1:6b969a803e1b | 14 | // printf("\033[0J"); //erase from cursor to end of screen |
FaizanAhmad | 1:6b969a803e1b | 15 | |
FaizanAhmad | 1:6b969a803e1b | 16 | uint32_t failures = 0; |
FaizanAhmad | 1:6b969a803e1b | 17 | |
FaizanAhmad | 1:6b969a803e1b | 18 | if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR){ |
FaizanAhmad | 1:6b969a803e1b | 19 | printf("Failed to set gyroscope power mode\n"); |
FaizanAhmad | 1:6b969a803e1b | 20 | failures++; |
FaizanAhmad | 1:6b969a803e1b | 21 | } |
FaizanAhmad | 1:6b969a803e1b | 22 | wait_ms(10); |
FaizanAhmad | 1:6b969a803e1b | 23 | |
FaizanAhmad | 1:6b969a803e1b | 24 | if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR){ |
FaizanAhmad | 1:6b969a803e1b | 25 | printf("Failed to set accelerometer power mode\n"); |
FaizanAhmad | 1:6b969a803e1b | 26 | failures++; |
FaizanAhmad | 1:6b969a803e1b | 27 | } |
FaizanAhmad | 1:6b969a803e1b | 28 | wait_ms(10); |
FaizanAhmad | 1:6b969a803e1b | 29 | |
FaizanAhmad | 1:6b969a803e1b | 30 | |
FaizanAhmad | 1:6b969a803e1b | 31 | //example of using getSensorConfig |
FaizanAhmad | 1:6b969a803e1b | 32 | if(imu.getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR){ |
FaizanAhmad | 1:6b969a803e1b | 33 | printf("ACC Range = %d\n", accConfig.range); |
FaizanAhmad | 1:6b969a803e1b | 34 | printf("ACC UnderSampling = %d\n", accConfig.us); |
FaizanAhmad | 1:6b969a803e1b | 35 | printf("ACC BandWidthParam = %d\n", accConfig.bwp); |
FaizanAhmad | 1:6b969a803e1b | 36 | printf("ACC OutputDataRate = %d\n\n", accConfig.odr); |
FaizanAhmad | 1:6b969a803e1b | 37 | }else{ |
FaizanAhmad | 1:6b969a803e1b | 38 | printf("Failed to get accelerometer configuration\n"); |
FaizanAhmad | 1:6b969a803e1b | 39 | failures++; |
FaizanAhmad | 1:6b969a803e1b | 40 | } |
FaizanAhmad | 1:6b969a803e1b | 41 | |
FaizanAhmad | 1:6b969a803e1b | 42 | //example of setting user defined configuration |
FaizanAhmad | 1:6b969a803e1b | 43 | accConfig.range = BMI160::SENS_2G ; |
FaizanAhmad | 1:6b969a803e1b | 44 | accConfig.us = BMI160::ACC_US_OFF; |
FaizanAhmad | 1:6b969a803e1b | 45 | accConfig.bwp = BMI160::ACC_BWP_2; |
FaizanAhmad | 1:6b969a803e1b | 46 | accConfig.odr = BMI160::ACC_ODR_8; |
FaizanAhmad | 1:6b969a803e1b | 47 | if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR){ |
FaizanAhmad | 1:6b969a803e1b | 48 | printf("ACC Range = %d\n", accConfig.range); |
FaizanAhmad | 1:6b969a803e1b | 49 | printf("ACC UnderSampling = %d\n", accConfig.us); |
FaizanAhmad | 1:6b969a803e1b | 50 | printf("ACC BandWidthParam = %d\n", accConfig.bwp); |
FaizanAhmad | 1:6b969a803e1b | 51 | printf("ACC OutputDataRate = %d\n\n", accConfig.odr); |
FaizanAhmad | 1:6b969a803e1b | 52 | }else{ |
FaizanAhmad | 1:6b969a803e1b | 53 | printf("Failed to set accelerometer configuration\n"); |
FaizanAhmad | 1:6b969a803e1b | 54 | failures++; |
FaizanAhmad | 1:6b969a803e1b | 55 | } |
FaizanAhmad | 1:6b969a803e1b | 56 | |
FaizanAhmad | 1:6b969a803e1b | 57 | if(imu.getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR){ |
FaizanAhmad | 1:6b969a803e1b | 58 | printf("GYRO Range = %d\n", gyroConfig.range); |
FaizanAhmad | 1:6b969a803e1b | 59 | printf("GYRO BandWidthParam = %d\n", gyroConfig.bwp); |
FaizanAhmad | 1:6b969a803e1b | 60 | printf("GYRO OutputDataRate = %d\n\n", gyroConfig.odr); |
FaizanAhmad | 1:6b969a803e1b | 61 | }else{ |
FaizanAhmad | 1:6b969a803e1b | 62 | printf("Failed to get gyroscope configuration\n"); |
FaizanAhmad | 1:6b969a803e1b | 63 | failures++; |
FaizanAhmad | 1:6b969a803e1b | 64 | } |
FaizanAhmad | 1:6b969a803e1b | 65 | |
FaizanAhmad | 1:6b969a803e1b | 66 | // printf("\033[H"); //home |
FaizanAhmad | 1:6b969a803e1b | 67 | // printf("\033[0J"); //erase from cursor to end of screen |
FaizanAhmad | 1:6b969a803e1b | 68 | |
FaizanAhmad | 1:6b969a803e1b | 69 | if(failures == 0){ |
FaizanAhmad | 1:6b969a803e1b | 70 | return 0; |
FaizanAhmad | 1:6b969a803e1b | 71 | }else{ |
FaizanAhmad | 1:6b969a803e1b | 72 | printf("IMU init failed.. returning 1\n"); |
FaizanAhmad | 1:6b969a803e1b | 73 | return 1; |
FaizanAhmad | 1:6b969a803e1b | 74 | } |
FaizanAhmad | 1:6b969a803e1b | 75 | } |
FaizanAhmad | 1:6b969a803e1b | 76 | |
FaizanAhmad | 1:6b969a803e1b | 77 | void printIMUData(){ |
FaizanAhmad | 1:6b969a803e1b | 78 | float imuTemperature; |
FaizanAhmad | 1:6b969a803e1b | 79 | BMI160::SensorData accData; |
FaizanAhmad | 1:6b969a803e1b | 80 | BMI160::SensorData gyroData; |
FaizanAhmad | 1:6b969a803e1b | 81 | BMI160::SensorTime sensorTime; |
FaizanAhmad | 1:6b969a803e1b | 82 | |
FaizanAhmad | 1:6b969a803e1b | 83 | imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range); |
FaizanAhmad | 1:6b969a803e1b | 84 | imu.getTemperature(&imuTemperature); |
FaizanAhmad | 1:6b969a803e1b | 85 | |
FaizanAhmad | 1:6b969a803e1b | 86 | printf("ACC xAxis = %d\n", accData.xAxis.raw); |
FaizanAhmad | 1:6b969a803e1b | 87 | printf("ACC yAxis = %d\n", accData.yAxis.raw); |
FaizanAhmad | 1:6b969a803e1b | 88 | printf("ACC zAxis = %d\n\n", accData.zAxis.raw); |
FaizanAhmad | 1:6b969a803e1b | 89 | |
FaizanAhmad | 1:6b969a803e1b | 90 | printf("GYRO xAxis = %d\n", gyroData.xAxis.raw); |
FaizanAhmad | 1:6b969a803e1b | 91 | printf("GYRO yAxis = %d\n", gyroData.yAxis.raw); |
FaizanAhmad | 1:6b969a803e1b | 92 | printf("GYRO zAxis = %d\n\n", gyroData.zAxis.raw); |
FaizanAhmad | 1:6b969a803e1b | 93 | |
FaizanAhmad | 1:6b969a803e1b | 94 | printf("Sensor Time = %f\n", sensorTime.seconds); |
FaizanAhmad | 1:6b969a803e1b | 95 | printf("Sensor Temperature = %5.3f\n", imuTemperature); |
FaizanAhmad | 1:6b969a803e1b | 96 | } |
FaizanAhmad | 1:6b969a803e1b | 97 | |
FaizanAhmad | 1:6b969a803e1b | 98 | |
FaizanAhmad | 1:6b969a803e1b | 99 | float getIMUAngle(){ |
FaizanAhmad | 1:6b969a803e1b | 100 | BMI160::SensorData accData; |
FaizanAhmad | 1:6b969a803e1b | 101 | BMI160::SensorData gyroData; |
FaizanAhmad | 1:6b969a803e1b | 102 | BMI160::SensorTime sensorTime; |
FaizanAhmad | 1:6b969a803e1b | 103 | double tempxAngle = 0; |
FaizanAhmad | 1:6b969a803e1b | 104 | double xAngle =0; |
FaizanAhmad | 1:6b969a803e1b | 105 | double tempyAngle =0; |
FaizanAhmad | 1:6b969a803e1b | 106 | double yAngle =0; |
FaizanAhmad | 1:6b969a803e1b | 107 | double res =0; |
FaizanAhmad | 1:6b969a803e1b | 108 | |
FaizanAhmad | 1:6b969a803e1b | 109 | imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range); |
FaizanAhmad | 1:6b969a803e1b | 110 | // printf("X = %d\tY = %d\tZ = %d\t\n", accData.xAxis.raw, accData.yAxis.raw, accData.zAxis.raw); |
FaizanAhmad | 1:6b969a803e1b | 111 | |
FaizanAhmad | 1:6b969a803e1b | 112 | // uint32_t xVal = accData.xAxis.raw; |
FaizanAhmad | 1:6b969a803e1b | 113 | // uint32_t yVal = accData.yAxis.raw; |
FaizanAhmad | 1:6b969a803e1b | 114 | // uint32_t zVal = accData.zAxis.raw; |
FaizanAhmad | 1:6b969a803e1b | 115 | |
FaizanAhmad | 1:6b969a803e1b | 116 | float xVal = accData.xAxis.scaled; |
FaizanAhmad | 1:6b969a803e1b | 117 | float yVal = accData.yAxis.scaled; |
FaizanAhmad | 1:6b969a803e1b | 118 | float zVal = accData.zAxis.scaled; |
FaizanAhmad | 1:6b969a803e1b | 119 | |
FaizanAhmad | 1:6b969a803e1b | 120 | /* Compute angles in radian */ |
FaizanAhmad | 1:6b969a803e1b | 121 | tempxAngle = (double)((yVal * yVal) + (zVal * zVal)); |
FaizanAhmad | 1:6b969a803e1b | 122 | tempyAngle = (double)((xVal * xVal) + (zVal * zVal)); |
FaizanAhmad | 1:6b969a803e1b | 123 | xAngle = (double)atan (xVal / sqrt (tempxAngle)); |
FaizanAhmad | 1:6b969a803e1b | 124 | yAngle = (double)atan (yVal / sqrt(tempyAngle)); |
FaizanAhmad | 1:6b969a803e1b | 125 | /* Convert radian in degree */ |
FaizanAhmad | 1:6b969a803e1b | 126 | xAngle *= 180.00; |
FaizanAhmad | 1:6b969a803e1b | 127 | yAngle *= 180.00; //zAngle *=180.00; |
FaizanAhmad | 1:6b969a803e1b | 128 | xAngle /= M_PI; |
FaizanAhmad | 1:6b969a803e1b | 129 | yAngle /= M_PI; //zAngle /= 3.141592; |
FaizanAhmad | 1:6b969a803e1b | 130 | // for x angle |
FaizanAhmad | 1:6b969a803e1b | 131 | res = xAngle; |
FaizanAhmad | 1:6b969a803e1b | 132 | return res; |
FaizanAhmad | 1:6b969a803e1b | 133 | } |
FaizanAhmad | 1:6b969a803e1b | 134 | |
FaizanAhmad | 1:6b969a803e1b | 135 | |
FaizanAhmad | 1:6b969a803e1b | 136 | |
FaizanAhmad | 1:6b969a803e1b | 137 | // DEFAULT MBED FUNCTIONs.. not required for MPS |
FaizanAhmad | 1:6b969a803e1b | 138 | void dumpImuRegisters(BMI160 &imu) |
FaizanAhmad | 1:6b969a803e1b | 139 | { |
FaizanAhmad | 1:6b969a803e1b | 140 | printRegister(imu, BMI160::CHIP_ID); |
FaizanAhmad | 1:6b969a803e1b | 141 | printBlock(imu, BMI160::ERR_REG,BMI160::FIFO_DATA); |
FaizanAhmad | 1:6b969a803e1b | 142 | printBlock(imu, BMI160::ACC_CONF, BMI160::FIFO_CONFIG_1); |
FaizanAhmad | 1:6b969a803e1b | 143 | printBlock(imu, BMI160::MAG_IF_0, BMI160::SELF_TEST); |
FaizanAhmad | 1:6b969a803e1b | 144 | printBlock(imu, BMI160::NV_CONF, BMI160::STEP_CONF_1); |
FaizanAhmad | 1:6b969a803e1b | 145 | printRegister(imu, BMI160::CMD); |
FaizanAhmad | 1:6b969a803e1b | 146 | printf("\n"); |
FaizanAhmad | 1:6b969a803e1b | 147 | } |
FaizanAhmad | 1:6b969a803e1b | 148 | |
FaizanAhmad | 1:6b969a803e1b | 149 | void printRegister(BMI160 &imu, BMI160::Registers reg) |
FaizanAhmad | 1:6b969a803e1b | 150 | { |
FaizanAhmad | 1:6b969a803e1b | 151 | uint8_t data; |
FaizanAhmad | 1:6b969a803e1b | 152 | if(imu.readRegister(reg, &data) == BMI160::RTN_NO_ERROR) |
FaizanAhmad | 1:6b969a803e1b | 153 | { |
FaizanAhmad | 1:6b969a803e1b | 154 | printf("IMU Register 0x%02x = 0x%02x\n", reg, data); |
FaizanAhmad | 1:6b969a803e1b | 155 | } |
FaizanAhmad | 1:6b969a803e1b | 156 | else |
FaizanAhmad | 1:6b969a803e1b | 157 | { |
FaizanAhmad | 1:6b969a803e1b | 158 | printf("Failed to read register\n"); |
FaizanAhmad | 1:6b969a803e1b | 159 | } |
FaizanAhmad | 1:6b969a803e1b | 160 | } |
FaizanAhmad | 1:6b969a803e1b | 161 | |
FaizanAhmad | 1:6b969a803e1b | 162 | void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg) |
FaizanAhmad | 1:6b969a803e1b | 163 | { |
FaizanAhmad | 1:6b969a803e1b | 164 | uint8_t numBytes = ((stopReg - startReg) + 1); |
FaizanAhmad | 1:6b969a803e1b | 165 | uint8_t buff[numBytes]; |
FaizanAhmad | 1:6b969a803e1b | 166 | uint8_t offset = static_cast<uint8_t>(startReg); |
FaizanAhmad | 1:6b969a803e1b | 167 | |
FaizanAhmad | 1:6b969a803e1b | 168 | if(imu.readBlock(startReg, stopReg, buff) == BMI160::RTN_NO_ERROR) |
FaizanAhmad | 1:6b969a803e1b | 169 | { |
FaizanAhmad | 1:6b969a803e1b | 170 | for(uint8_t idx = offset; idx < (numBytes + offset); idx++) |
FaizanAhmad | 1:6b969a803e1b | 171 | { |
FaizanAhmad | 1:6b969a803e1b | 172 | printf("IMU Register 0x%02x = 0x%02x\n", idx, buff[idx - offset]); |
FaizanAhmad | 1:6b969a803e1b | 173 | } |
FaizanAhmad | 1:6b969a803e1b | 174 | } |
FaizanAhmad | 1:6b969a803e1b | 175 | else |
FaizanAhmad | 1:6b969a803e1b | 176 | { |
FaizanAhmad | 1:6b969a803e1b | 177 | printf("Failed to read block\n"); |
FaizanAhmad | 1:6b969a803e1b | 178 | } |
FaizanAhmad | 1:6b969a803e1b | 179 | } |
FaizanAhmad | 1:6b969a803e1b | 180 |