Faizan Ahmad / Mbed OS MPSMAXbutton

button

Dependencies:   BMI160 SDFileSystem USBDevice max32630fthr

Fork of MPSMAX by Faizan Ahmad

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?

UserRevisionLine numberNew 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