Faizan Ahmad / Mbed OS MPSMAX_copy

B+IMU+SD

Dependencies:   BMI160 RTC SDFileSystem USBDevice max32630fthr sd-driver

Fork of MPSMAXbutton by Faizan Ahmad

IMU.cpp@3:bd223559f79b, 2018-05-09 (annotated)

Committer:
FaizanAhmad
Date:
Wed May 09 11:01:18 2018 +0000
Revision:
3:bd223559f79b
bUTTON+IMU+SD

Who changed what in which revision?

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