TSDA Robotics / Mbed 2 deprecated Maxim_Squeeks

Final Tree

Dependencies:   mbed BMI160 max32630fthr_pitch USBDevice Math

Sensors/Orientation.cpp@7:b33be863fbb5, 2019-01-28 (annotated)

Committer:
bjbance
Date:
Mon Jan 28 09:00:47 2019 +0000
Revision:
7:b33be863fbb5
Separate library for sensors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bjbance 7:b33be863fbb5 1 #include "mbed.h"
bjbance 7:b33be863fbb5 2 #include "bmi160.h"
bjbance 7:b33be863fbb5 3 #include "max32630fthr.h"
bjbance 7:b33be863fbb5 4 #include "USBSerial.h"
bjbance 7:b33be863fbb5 5 #include "stdlib.h"
bjbance 7:b33be863fbb5 6 #include "math.h"
bjbance 7:b33be863fbb5 7 #include "Orientation.h"
bjbance 7:b33be863fbb5 8
bjbance 7:b33be863fbb5 9 I2C i2cBus(I2C2_SDA, I2C2_SCL);
bjbance 7:b33be863fbb5 10 BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO);
bjbance 7:b33be863fbb5 11
bjbance 7:b33be863fbb5 12 BMI160::SensorData accData;
bjbance 7:b33be863fbb5 13 BMI160::SensorData gyroData;
bjbance 7:b33be863fbb5 14 BMI160::SensorTime sensorTime;
bjbance 7:b33be863fbb5 15 BMI160::AccConfig accConfig;
bjbance 7:b33be863fbb5 16 BMI160::GyroConfig gyroConfig;
bjbance 7:b33be863fbb5 17 float imuTemperature;
bjbance 7:b33be863fbb5 18
bjbance 7:b33be863fbb5 19
bjbance 7:b33be863fbb5 20 Serial pc2(P2_1, P2_0);
bjbance 7:b33be863fbb5 21
bjbance 7:b33be863fbb5 22 void Orientation::init(){
bjbance 7:b33be863fbb5 23 pitch = 0;
bjbance 7:b33be863fbb5 24 k = 0.65;
bjbance 7:b33be863fbb5 25 time1 = sensorTime.seconds;
bjbance 7:b33be863fbb5 26 time2 = sensorTime.seconds;
bjbance 7:b33be863fbb5 27
bjbance 7:b33be863fbb5 28
bjbance 7:b33be863fbb5 29 i2cBus.frequency(400000);
bjbance 7:b33be863fbb5 30 writeReg(imu, BMI160::GYR_RANGE, BMI160::DPS_500);
bjbance 7:b33be863fbb5 31 writeReg(imu, BMI160::GYR_CONF, BMI160::GYRO_ODR_13);
bjbance 7:b33be863fbb5 32 writeReg(imu, BMI160::FOC_CONF, BMI160::FOC_VALUE );
bjbance 7:b33be863fbb5 33 wait(0.5);
bjbance 7:b33be863fbb5 34 writeReg(imu, BMI160::OFFSET_6, BMI160::FOC_ENABLE_GYR_ACC);
bjbance 7:b33be863fbb5 35
bjbance 7:b33be863fbb5 36 writeReg(imu, BMI160::CMD, BMI160::FOC_START);
bjbance 7:b33be863fbb5 37
bjbance 7:b33be863fbb5 38
bjbance 7:b33be863fbb5 39 pc2.printf("\033[H"); //home
bjbance 7:b33be863fbb5 40 pc2.printf("\033[0J"); //erase from cursor to end of screen
bjbance 7:b33be863fbb5 41
bjbance 7:b33be863fbb5 42 uint32_t failures = 0;
bjbance 7:b33be863fbb5 43
bjbance 7:b33be863fbb5 44 if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
bjbance 7:b33be863fbb5 45 {
bjbance 7:b33be863fbb5 46 pc2.printf("Failed to set gyroscope power mode\r\n");
bjbance 7:b33be863fbb5 47 failures++;
bjbance 7:b33be863fbb5 48 }
bjbance 7:b33be863fbb5 49 wait_ms(100);
bjbance 7:b33be863fbb5 50
bjbance 7:b33be863fbb5 51 if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
bjbance 7:b33be863fbb5 52 {
bjbance 7:b33be863fbb5 53 pc2.printf("Failed to set accelerometer power mode\r\n");
bjbance 7:b33be863fbb5 54 failures++;
bjbance 7:b33be863fbb5 55 }
bjbance 7:b33be863fbb5 56 wait_ms(100);
bjbance 7:b33be863fbb5 57
bjbance 7:b33be863fbb5 58
bjbance 7:b33be863fbb5 59
bjbance 7:b33be863fbb5 60 //example of using getSensorConfig
bjbance 7:b33be863fbb5 61 if(imu.getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
bjbance 7:b33be863fbb5 62 {
bjbance 7:b33be863fbb5 63 pc2.printf("ACC Range = %d\r\n", accConfig.range);
bjbance 7:b33be863fbb5 64 pc2.printf("ACC UnderSampling = %d\r\n", accConfig.us);
bjbance 7:b33be863fbb5 65 pc2.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp);
bjbance 7:b33be863fbb5 66 pc2.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr);
bjbance 7:b33be863fbb5 67 }
bjbance 7:b33be863fbb5 68 else
bjbance 7:b33be863fbb5 69 {
bjbance 7:b33be863fbb5 70 pc2.printf("Failed to get accelerometer configuration\r\n");
bjbance 7:b33be863fbb5 71 failures++;
bjbance 7:b33be863fbb5 72 }
bjbance 7:b33be863fbb5 73
bjbance 7:b33be863fbb5 74 //example of setting user defined configuration
bjbance 7:b33be863fbb5 75 accConfig.range = BMI160::SENS_2G; //rage is 2g
bjbance 7:b33be863fbb5 76 accConfig.us = BMI160::ACC_US_OFF; //undersampling is off
bjbance 7:b33be863fbb5 77 accConfig.bwp = BMI160::ACC_BWP_0; //average 4 cycles
bjbance 7:b33be863fbb5 78 accConfig.odr = BMI160::ACC_ODR_9; //output data rate
bjbance 7:b33be863fbb5 79 if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
bjbance 7:b33be863fbb5 80 {
bjbance 7:b33be863fbb5 81 pc2.printf("ACC Range = %d\r\n", accConfig.range);
bjbance 7:b33be863fbb5 82 pc2.printf("ACC UnderSampling = %d\r\n", accConfig.us);
bjbance 7:b33be863fbb5 83 pc2.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp);
bjbance 7:b33be863fbb5 84 pc2.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr);
bjbance 7:b33be863fbb5 85 }
bjbance 7:b33be863fbb5 86 else
bjbance 7:b33be863fbb5 87 {
bjbance 7:b33be863fbb5 88 pc2.printf("Failed to set accelerometer configuration\r\n");
bjbance 7:b33be863fbb5 89 failures++;
bjbance 7:b33be863fbb5 90 }
bjbance 7:b33be863fbb5 91
bjbance 7:b33be863fbb5 92 if(imu.getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR)
bjbance 7:b33be863fbb5 93 {
bjbance 7:b33be863fbb5 94 pc2.printf("GYRO Range = %d\r\n", gyroConfig.range);
bjbance 7:b33be863fbb5 95 pc2.printf("GYRO BandWidthParam = %d\r\n", gyroConfig.bwp);
bjbance 7:b33be863fbb5 96 pc2.printf("GYRO OutputDataRate = %d\r\n\r\n", gyroConfig.odr);
bjbance 7:b33be863fbb5 97
bjbance 7:b33be863fbb5 98
bjbance 7:b33be863fbb5 99 }
bjbance 7:b33be863fbb5 100 else
bjbance 7:b33be863fbb5 101 {
bjbance 7:b33be863fbb5 102 pc2.printf("Failed to get gyroscope configuration\r\n");
bjbance 7:b33be863fbb5 103 failures++;
bjbance 7:b33be863fbb5 104 }
bjbance 7:b33be863fbb5 105
bjbance 7:b33be863fbb5 106 wait(1.0);
bjbance 7:b33be863fbb5 107
bjbance 7:b33be863fbb5 108 }
bjbance 7:b33be863fbb5 109
bjbance 7:b33be863fbb5 110
bjbance 7:b33be863fbb5 111 float Orientation::getPitch()
bjbance 7:b33be863fbb5 112 {
bjbance 7:b33be863fbb5 113 return pitch;
bjbance 7:b33be863fbb5 114 }
bjbance 7:b33be863fbb5 115
bjbance 7:b33be863fbb5 116 void Orientation::updatePitch()
bjbance 7:b33be863fbb5 117 {
bjbance 7:b33be863fbb5 118 imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
bjbance 7:b33be863fbb5 119 imu.getTemperature(&imuTemperature);
bjbance 7:b33be863fbb5 120
bjbance 7:b33be863fbb5 121 time2 = sensorTime.seconds;
bjbance 7:b33be863fbb5 122 pitch = compFilter(k, pitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, time2 - time1);
bjbance 7:b33be863fbb5 123 //daplink.printf("Forward: %s%4.3f\r\n", "\033[K", apitch);
bjbance 7:b33be863fbb5 124 time1 = time2;
bjbance 7:b33be863fbb5 125 // return pitch;
bjbance 7:b33be863fbb5 126 printRegister(imu, BMI160::GYR_CONF);
bjbance 7:b33be863fbb5 127 }
bjbance 7:b33be863fbb5 128
bjbance 7:b33be863fbb5 129 //*****************************************************************************
bjbance 7:b33be863fbb5 130 void dumpImuRegisters(BMI160 &imu)
bjbance 7:b33be863fbb5 131 {
bjbance 7:b33be863fbb5 132 printRegister(imu, BMI160::CHIP_ID);
bjbance 7:b33be863fbb5 133 printBlock(imu, BMI160::ERR_REG,BMI160::FIFO_DATA);
bjbance 7:b33be863fbb5 134 printBlock(imu, BMI160::ACC_CONF, BMI160::FIFO_CONFIG_1);
bjbance 7:b33be863fbb5 135 printBlock(imu, BMI160::MAG_IF_0, BMI160::SELF_TEST);
bjbance 7:b33be863fbb5 136 printBlock(imu, BMI160::NV_CONF, BMI160::STEP_CONF_1);
bjbance 7:b33be863fbb5 137 printRegister(imu, BMI160::CMD);
bjbance 7:b33be863fbb5 138 // pc.printf("\r\n");
bjbance 7:b33be863fbb5 139 }
bjbance 7:b33be863fbb5 140
bjbance 7:b33be863fbb5 141
bjbance 7:b33be863fbb5 142 //*****************************************************************************
bjbance 7:b33be863fbb5 143 void printRegister(BMI160 &imu, BMI160::Registers reg)
bjbance 7:b33be863fbb5 144 {
bjbance 7:b33be863fbb5 145 uint8_t data;
bjbance 7:b33be863fbb5 146 if(imu.readRegister(reg, &data) == BMI160::RTN_NO_ERROR)
bjbance 7:b33be863fbb5 147 {
bjbance 7:b33be863fbb5 148 // pc.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data);
bjbance 7:b33be863fbb5 149 // daplink.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data);
bjbance 7:b33be863fbb5 150 }
bjbance 7:b33be863fbb5 151 else
bjbance 7:b33be863fbb5 152 {
bjbance 7:b33be863fbb5 153 // pc.printf("Failed to read register\r\n");
bjbance 7:b33be863fbb5 154 }
bjbance 7:b33be863fbb5 155 }
bjbance 7:b33be863fbb5 156
bjbance 7:b33be863fbb5 157 //*****************************************************************************
bjbance 7:b33be863fbb5 158 void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data)
bjbance 7:b33be863fbb5 159 {
bjbance 7:b33be863fbb5 160 imu.writeRegister(reg, data);
bjbance 7:b33be863fbb5 161
bjbance 7:b33be863fbb5 162
bjbance 7:b33be863fbb5 163 }
bjbance 7:b33be863fbb5 164
bjbance 7:b33be863fbb5 165
bjbance 7:b33be863fbb5 166 //*****************************************************************************
bjbance 7:b33be863fbb5 167 void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg)
bjbance 7:b33be863fbb5 168 {
bjbance 7:b33be863fbb5 169 uint8_t numBytes = ((stopReg - startReg) + 1);
bjbance 7:b33be863fbb5 170 uint8_t buff[32];
bjbance 7:b33be863fbb5 171 uint8_t offset = static_cast<uint8_t>(startReg);
bjbance 7:b33be863fbb5 172
bjbance 7:b33be863fbb5 173 if(imu.readBlock(startReg, stopReg, buff) == BMI160::RTN_NO_ERROR)
bjbance 7:b33be863fbb5 174 {
bjbance 7:b33be863fbb5 175 for(uint8_t idx = offset; idx < (numBytes + offset); idx++)
bjbance 7:b33be863fbb5 176 {
bjbance 7:b33be863fbb5 177 // pc.printf("IMU Register 0x%02x = 0x%02x\r\n", idx, buff[idx - offset]);
bjbance 7:b33be863fbb5 178 }
bjbance 7:b33be863fbb5 179 }
bjbance 7:b33be863fbb5 180 else
bjbance 7:b33be863fbb5 181 {
bjbance 7:b33be863fbb5 182 // pc.printf("Failed to read block\r\n");
bjbance 7:b33be863fbb5 183 }
bjbance 7:b33be863fbb5 184 }
bjbance 7:b33be863fbb5 185
bjbance 7:b33be863fbb5 186
bjbance 7:b33be863fbb5 187
bjbance 7:b33be863fbb5 188 float compFilter(float K, float pitch, float gyroX, float accY, float accZ,float DT)
bjbance 7:b33be863fbb5 189 {
bjbance 7:b33be863fbb5 190 return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * ((180.0F / 3.1459F) * atan(accY/accZ))));
bjbance 7:b33be863fbb5 191 }