Allan Brignoli / Mbed 2 deprecated Rome2_P6

gugus

Dependencies:   mbed

IMU.cpp@0:1a0321f1ffbc, 2018-05-18 (annotated)

Committer:
Brignall
Date:
Fri May 18 12:18:21 2018 +0000
Revision:
0:1a0321f1ffbc
lala;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Brignall 0:1a0321f1ffbc 1 /*
Brignall 0:1a0321f1ffbc 2 * IMU.cpp
Brignall 0:1a0321f1ffbc 3 * Copyright (c) 2018, ZHAW
Brignall 0:1a0321f1ffbc 4 * All rights reserved.
Brignall 0:1a0321f1ffbc 5 */
Brignall 0:1a0321f1ffbc 6
Brignall 0:1a0321f1ffbc 7 #include <cmath>
Brignall 0:1a0321f1ffbc 8 #include "IMU.h"
Brignall 0:1a0321f1ffbc 9
Brignall 0:1a0321f1ffbc 10 using namespace std;
Brignall 0:1a0321f1ffbc 11
Brignall 0:1a0321f1ffbc 12 const float IMU::PERIOD = 0.001f; // period of filter task, given in [s]
Brignall 0:1a0321f1ffbc 13 const float IMU::PI = 3.14159265f; // the constant PI
Brignall 0:1a0321f1ffbc 14 const float IMU::LOWPASS_FILTER_FREQUENCY = 6.3f; // frequency of lowpass filter, given in [rad/s]
Brignall 0:1a0321f1ffbc 15
Brignall 0:1a0321f1ffbc 16 /**
Brignall 0:1a0321f1ffbc 17 * Creates an IMU object.
Brignall 0:1a0321f1ffbc 18 * @param spi a reference to an spi controller to use.
Brignall 0:1a0321f1ffbc 19 * @param csG the chip select output for the gyro sensor.
Brignall 0:1a0321f1ffbc 20 * @param csXM the chip select output for the accelerometer and the magnetometer.
Brignall 0:1a0321f1ffbc 21 */
Brignall 0:1a0321f1ffbc 22 IMU::IMU(SPI& spi, DigitalOut& csG, DigitalOut& csXM) : spi(spi), csG(csG), csXM(csXM) {
Brignall 0:1a0321f1ffbc 23
Brignall 0:1a0321f1ffbc 24 // initialize SPI interface
Brignall 0:1a0321f1ffbc 25
Brignall 0:1a0321f1ffbc 26 spi.format(8, 3);
Brignall 0:1a0321f1ffbc 27 spi.frequency(1000000);
Brignall 0:1a0321f1ffbc 28
Brignall 0:1a0321f1ffbc 29 // reset chip select lines to logical high
Brignall 0:1a0321f1ffbc 30
Brignall 0:1a0321f1ffbc 31 csG = 1;
Brignall 0:1a0321f1ffbc 32 csXM = 1;
Brignall 0:1a0321f1ffbc 33
Brignall 0:1a0321f1ffbc 34 // initialize gyro
Brignall 0:1a0321f1ffbc 35
Brignall 0:1a0321f1ffbc 36 writeRegister(csG, CTRL_REG1_G, 0x0F); // enable gyro in all 3 axis
Brignall 0:1a0321f1ffbc 37
Brignall 0:1a0321f1ffbc 38 // initialize accelerometer
Brignall 0:1a0321f1ffbc 39
Brignall 0:1a0321f1ffbc 40 writeRegister(csXM, CTRL_REG0_XM, 0x00);
Brignall 0:1a0321f1ffbc 41 writeRegister(csXM, CTRL_REG1_XM, 0x5F);
Brignall 0:1a0321f1ffbc 42 writeRegister(csXM, CTRL_REG2_XM, 0x00);
Brignall 0:1a0321f1ffbc 43 writeRegister(csXM, CTRL_REG3_XM, 0x04);
Brignall 0:1a0321f1ffbc 44
Brignall 0:1a0321f1ffbc 45 // initialize magnetometer
Brignall 0:1a0321f1ffbc 46
Brignall 0:1a0321f1ffbc 47 writeRegister(csXM, CTRL_REG5_XM, 0x94);
Brignall 0:1a0321f1ffbc 48 writeRegister(csXM, CTRL_REG6_XM, 0x00);
Brignall 0:1a0321f1ffbc 49 writeRegister(csXM, CTRL_REG7_XM, 0x00);
Brignall 0:1a0321f1ffbc 50 writeRegister(csXM, CTRL_REG4_XM, 0x04);
Brignall 0:1a0321f1ffbc 51 writeRegister(csXM, INT_CTRL_REG_M, 0x09);
Brignall 0:1a0321f1ffbc 52
Brignall 0:1a0321f1ffbc 53 // initialize local variables
Brignall 0:1a0321f1ffbc 54
Brignall 0:1a0321f1ffbc 55 magnetometerXMin = 1000.0f;
Brignall 0:1a0321f1ffbc 56 magnetometerXMax = -1000.0f;
Brignall 0:1a0321f1ffbc 57 magnetometerYMin = 1000.0f;
Brignall 0:1a0321f1ffbc 58 magnetometerYMax = -1000.0f;
Brignall 0:1a0321f1ffbc 59
Brignall 0:1a0321f1ffbc 60 magnetometerXFilter.setPeriod(PERIOD);
Brignall 0:1a0321f1ffbc 61 magnetometerXFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
Brignall 0:1a0321f1ffbc 62 magnetometerXFilter.reset(readMagnetometerX());
Brignall 0:1a0321f1ffbc 63
Brignall 0:1a0321f1ffbc 64 magnetometerYFilter.setPeriod(PERIOD);
Brignall 0:1a0321f1ffbc 65 magnetometerYFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
Brignall 0:1a0321f1ffbc 66 magnetometerYFilter.reset(readMagnetometerY());
Brignall 0:1a0321f1ffbc 67
Brignall 0:1a0321f1ffbc 68 heading = 0.0f;
Brignall 0:1a0321f1ffbc 69
Brignall 0:1a0321f1ffbc 70 // start periodic task
Brignall 0:1a0321f1ffbc 71
Brignall 0:1a0321f1ffbc 72 ticker.attach(callback(this, &IMU::run), PERIOD);
Brignall 0:1a0321f1ffbc 73 }
Brignall 0:1a0321f1ffbc 74
Brignall 0:1a0321f1ffbc 75 /**
Brignall 0:1a0321f1ffbc 76 * Deletes the IMU object.
Brignall 0:1a0321f1ffbc 77 */
Brignall 0:1a0321f1ffbc 78 IMU::~IMU() {}
Brignall 0:1a0321f1ffbc 79
Brignall 0:1a0321f1ffbc 80 /**
Brignall 0:1a0321f1ffbc 81 * This private method allows to write a register value.
Brignall 0:1a0321f1ffbc 82 * @param cs the chip select output to use, either csG or csXM.
Brignall 0:1a0321f1ffbc 83 * @param address the 7 bit address of the register.
Brignall 0:1a0321f1ffbc 84 * @param value the value to write into the register.
Brignall 0:1a0321f1ffbc 85 */
Brignall 0:1a0321f1ffbc 86 void IMU::writeRegister(DigitalOut& cs, char address, char value) {
Brignall 0:1a0321f1ffbc 87
Brignall 0:1a0321f1ffbc 88 cs = 0;
Brignall 0:1a0321f1ffbc 89
Brignall 0:1a0321f1ffbc 90 spi.write(0x7F & address);
Brignall 0:1a0321f1ffbc 91 spi.write(value & 0xFF);
Brignall 0:1a0321f1ffbc 92
Brignall 0:1a0321f1ffbc 93 cs = 1;
Brignall 0:1a0321f1ffbc 94 }
Brignall 0:1a0321f1ffbc 95
Brignall 0:1a0321f1ffbc 96 /**
Brignall 0:1a0321f1ffbc 97 * This private method allows to read a register value.
Brignall 0:1a0321f1ffbc 98 * @param cs the chip select output to use, either csG or csXM.
Brignall 0:1a0321f1ffbc 99 * @param address the 7 bit address of the register.
Brignall 0:1a0321f1ffbc 100 * @return the value read from the register.
Brignall 0:1a0321f1ffbc 101 */
Brignall 0:1a0321f1ffbc 102 char IMU::readRegister(DigitalOut& cs, char address) {
Brignall 0:1a0321f1ffbc 103
Brignall 0:1a0321f1ffbc 104 cs = 0;
Brignall 0:1a0321f1ffbc 105
Brignall 0:1a0321f1ffbc 106 spi.write(0x80 | address);
Brignall 0:1a0321f1ffbc 107 int value = spi.write(0xFF);
Brignall 0:1a0321f1ffbc 108
Brignall 0:1a0321f1ffbc 109 cs = 1;
Brignall 0:1a0321f1ffbc 110
Brignall 0:1a0321f1ffbc 111 return (char)(value & 0xFF);
Brignall 0:1a0321f1ffbc 112 }
Brignall 0:1a0321f1ffbc 113
Brignall 0:1a0321f1ffbc 114 /**
Brignall 0:1a0321f1ffbc 115 * Reads the gyroscope about the x-axis.
Brignall 0:1a0321f1ffbc 116 * @return the rotational speed about the x-axis given in [rad/s].
Brignall 0:1a0321f1ffbc 117 */
Brignall 0:1a0321f1ffbc 118 float IMU::readGyroX() {
Brignall 0:1a0321f1ffbc 119
Brignall 0:1a0321f1ffbc 120 char low = readRegister(csG, OUT_X_L_G);
Brignall 0:1a0321f1ffbc 121 char high = readRegister(csG, OUT_X_H_G);
Brignall 0:1a0321f1ffbc 122
Brignall 0:1a0321f1ffbc 123 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
Brignall 0:1a0321f1ffbc 124
Brignall 0:1a0321f1ffbc 125 return (float)value/32768.0f*245.0f*PI/180.0f;
Brignall 0:1a0321f1ffbc 126 }
Brignall 0:1a0321f1ffbc 127
Brignall 0:1a0321f1ffbc 128 /**
Brignall 0:1a0321f1ffbc 129 * Reads the gyroscope about the y-axis.
Brignall 0:1a0321f1ffbc 130 * @return the rotational speed about the y-axis given in [rad/s].
Brignall 0:1a0321f1ffbc 131 */
Brignall 0:1a0321f1ffbc 132 float IMU::readGyroY() {
Brignall 0:1a0321f1ffbc 133
Brignall 0:1a0321f1ffbc 134 char low = readRegister(csG, OUT_Y_L_G);
Brignall 0:1a0321f1ffbc 135 char high = readRegister(csG, OUT_Y_H_G);
Brignall 0:1a0321f1ffbc 136
Brignall 0:1a0321f1ffbc 137 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
Brignall 0:1a0321f1ffbc 138
Brignall 0:1a0321f1ffbc 139 return (float)value/32768.0f*245.0f*PI/180.0f;
Brignall 0:1a0321f1ffbc 140 }
Brignall 0:1a0321f1ffbc 141
Brignall 0:1a0321f1ffbc 142 /**
Brignall 0:1a0321f1ffbc 143 * Reads the gyroscope about the z-axis.
Brignall 0:1a0321f1ffbc 144 * @return the rotational speed about the z-axis given in [rad/s].
Brignall 0:1a0321f1ffbc 145 */
Brignall 0:1a0321f1ffbc 146 float IMU::readGyroZ() {
Brignall 0:1a0321f1ffbc 147
Brignall 0:1a0321f1ffbc 148 char low = readRegister(csG, OUT_Z_L_G);
Brignall 0:1a0321f1ffbc 149 char high = readRegister(csG, OUT_Z_H_G);
Brignall 0:1a0321f1ffbc 150
Brignall 0:1a0321f1ffbc 151 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
Brignall 0:1a0321f1ffbc 152
Brignall 0:1a0321f1ffbc 153 return (float)value/32768.0f*245.0f*PI/180.0f;
Brignall 0:1a0321f1ffbc 154 }
Brignall 0:1a0321f1ffbc 155
Brignall 0:1a0321f1ffbc 156 /**
Brignall 0:1a0321f1ffbc 157 * Reads the acceleration in x-direction.
Brignall 0:1a0321f1ffbc 158 * @return the acceleration in x-direction, given in [m/s2].
Brignall 0:1a0321f1ffbc 159 */
Brignall 0:1a0321f1ffbc 160 float IMU::readAccelerationX() {
Brignall 0:1a0321f1ffbc 161
Brignall 0:1a0321f1ffbc 162 char low = readRegister(csXM, OUT_X_L_A);
Brignall 0:1a0321f1ffbc 163 char high = readRegister(csXM, OUT_X_H_A);
Brignall 0:1a0321f1ffbc 164
Brignall 0:1a0321f1ffbc 165 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
Brignall 0:1a0321f1ffbc 166
Brignall 0:1a0321f1ffbc 167 return (float)value/32768.0f*2.0f*9.81f;
Brignall 0:1a0321f1ffbc 168 }
Brignall 0:1a0321f1ffbc 169
Brignall 0:1a0321f1ffbc 170 /**
Brignall 0:1a0321f1ffbc 171 * Reads the acceleration in y-direction.
Brignall 0:1a0321f1ffbc 172 * @return the acceleration in y-direction, given in [m/s2].
Brignall 0:1a0321f1ffbc 173 */
Brignall 0:1a0321f1ffbc 174 float IMU::readAccelerationY() {
Brignall 0:1a0321f1ffbc 175
Brignall 0:1a0321f1ffbc 176 char low = readRegister(csXM, OUT_Y_L_A);
Brignall 0:1a0321f1ffbc 177 char high = readRegister(csXM, OUT_Y_H_A);
Brignall 0:1a0321f1ffbc 178
Brignall 0:1a0321f1ffbc 179 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
Brignall 0:1a0321f1ffbc 180
Brignall 0:1a0321f1ffbc 181 return (float)value/32768.0f*2.0f*9.81f;
Brignall 0:1a0321f1ffbc 182 }
Brignall 0:1a0321f1ffbc 183
Brignall 0:1a0321f1ffbc 184 /**
Brignall 0:1a0321f1ffbc 185 * Reads the acceleration in z-direction.
Brignall 0:1a0321f1ffbc 186 * @return the acceleration in z-direction, given in [m/s2].
Brignall 0:1a0321f1ffbc 187 */
Brignall 0:1a0321f1ffbc 188 float IMU::readAccelerationZ() {
Brignall 0:1a0321f1ffbc 189
Brignall 0:1a0321f1ffbc 190 char low = readRegister(csXM, OUT_Z_L_A);
Brignall 0:1a0321f1ffbc 191 char high = readRegister(csXM, OUT_Z_H_A);
Brignall 0:1a0321f1ffbc 192
Brignall 0:1a0321f1ffbc 193 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
Brignall 0:1a0321f1ffbc 194
Brignall 0:1a0321f1ffbc 195 return (float)value/32768.0f*2.0f*9.81f;
Brignall 0:1a0321f1ffbc 196 }
Brignall 0:1a0321f1ffbc 197
Brignall 0:1a0321f1ffbc 198 /**
Brignall 0:1a0321f1ffbc 199 * Reads the magnetic field in x-direction.
Brignall 0:1a0321f1ffbc 200 * @return the magnetic field in x-direction, given in [Gauss].
Brignall 0:1a0321f1ffbc 201 */
Brignall 0:1a0321f1ffbc 202 float IMU::readMagnetometerX() {
Brignall 0:1a0321f1ffbc 203
Brignall 0:1a0321f1ffbc 204 char low = readRegister(csXM, OUT_X_L_M);
Brignall 0:1a0321f1ffbc 205 char high = readRegister(csXM, OUT_X_H_M);
Brignall 0:1a0321f1ffbc 206
Brignall 0:1a0321f1ffbc 207 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
Brignall 0:1a0321f1ffbc 208
Brignall 0:1a0321f1ffbc 209 return (float)value/32768.0f*2.0f;
Brignall 0:1a0321f1ffbc 210 }
Brignall 0:1a0321f1ffbc 211
Brignall 0:1a0321f1ffbc 212 /**
Brignall 0:1a0321f1ffbc 213 * Reads the magnetic field in x-direction.
Brignall 0:1a0321f1ffbc 214 * @return the magnetic field in x-direction, given in [Gauss].
Brignall 0:1a0321f1ffbc 215 */
Brignall 0:1a0321f1ffbc 216 float IMU::readMagnetometerY() {
Brignall 0:1a0321f1ffbc 217
Brignall 0:1a0321f1ffbc 218 char low = readRegister(csXM, OUT_Y_L_M);
Brignall 0:1a0321f1ffbc 219 char high = readRegister(csXM, OUT_Y_H_M);
Brignall 0:1a0321f1ffbc 220
Brignall 0:1a0321f1ffbc 221 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
Brignall 0:1a0321f1ffbc 222
Brignall 0:1a0321f1ffbc 223 return (float)value/32768.0f*2.0f;
Brignall 0:1a0321f1ffbc 224 }
Brignall 0:1a0321f1ffbc 225
Brignall 0:1a0321f1ffbc 226 /**
Brignall 0:1a0321f1ffbc 227 * Reads the magnetic field in x-direction.
Brignall 0:1a0321f1ffbc 228 * @return the magnetic field in x-direction, given in [Gauss].
Brignall 0:1a0321f1ffbc 229 */
Brignall 0:1a0321f1ffbc 230 float IMU::readMagnetometerZ() {
Brignall 0:1a0321f1ffbc 231
Brignall 0:1a0321f1ffbc 232 char low = readRegister(csXM, OUT_Z_L_M);
Brignall 0:1a0321f1ffbc 233 char high = readRegister(csXM, OUT_Z_H_M);
Brignall 0:1a0321f1ffbc 234
Brignall 0:1a0321f1ffbc 235 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
Brignall 0:1a0321f1ffbc 236
Brignall 0:1a0321f1ffbc 237 return (float)value/32768.0f*2.0f;
Brignall 0:1a0321f1ffbc 238 }
Brignall 0:1a0321f1ffbc 239
Brignall 0:1a0321f1ffbc 240 /**
Brignall 0:1a0321f1ffbc 241 * Reads the compass heading about the z-axis.
Brignall 0:1a0321f1ffbc 242 * @return the compass heading in the range -PI to +PI, given in [rad].
Brignall 0:1a0321f1ffbc 243 */
Brignall 0:1a0321f1ffbc 244 float IMU::readHeading() {
Brignall 0:1a0321f1ffbc 245
Brignall 0:1a0321f1ffbc 246 return heading;
Brignall 0:1a0321f1ffbc 247 }
Brignall 0:1a0321f1ffbc 248
Brignall 0:1a0321f1ffbc 249 /**
Brignall 0:1a0321f1ffbc 250 * This method is called periodically by the ticker object and contains the
Brignall 0:1a0321f1ffbc 251 * calculation and filtering of the heading information.
Brignall 0:1a0321f1ffbc 252 */
Brignall 0:1a0321f1ffbc 253 void IMU::run() {
Brignall 0:1a0321f1ffbc 254
Brignall 0:1a0321f1ffbc 255 // read actual measurements from magnetometer registers
Brignall 0:1a0321f1ffbc 256
Brignall 0:1a0321f1ffbc 257 float magnetometerX = magnetometerXFilter.filter(readMagnetometerX());
Brignall 0:1a0321f1ffbc 258 float magnetometerY = magnetometerYFilter.filter(readMagnetometerY());
Brignall 0:1a0321f1ffbc 259
Brignall 0:1a0321f1ffbc 260 // adjust the minimum and maximum limits, if needed
Brignall 0:1a0321f1ffbc 261
Brignall 0:1a0321f1ffbc 262 if (magnetometerXMin > magnetometerX) magnetometerXMin = magnetometerX;
Brignall 0:1a0321f1ffbc 263 if (magnetometerXMax < magnetometerX) magnetometerXMax = magnetometerX;
Brignall 0:1a0321f1ffbc 264 if (magnetometerYMin > magnetometerY) magnetometerYMin = magnetometerY;
Brignall 0:1a0321f1ffbc 265 if (magnetometerYMax < magnetometerY) magnetometerYMax = magnetometerY;
Brignall 0:1a0321f1ffbc 266
Brignall 0:1a0321f1ffbc 267 // calculate adjusted magnetometer values (gain and offset compensation)
Brignall 0:1a0321f1ffbc 268
Brignall 0:1a0321f1ffbc 269 if (magnetometerXMin < magnetometerXMax) magnetometerX = (magnetometerX-magnetometerXMin)/(magnetometerXMax-magnetometerXMin)-0.5f;
Brignall 0:1a0321f1ffbc 270 if (magnetometerYMin < magnetometerYMax) magnetometerY = (magnetometerY-magnetometerYMin)/(magnetometerYMax-magnetometerYMin)-0.5f;
Brignall 0:1a0321f1ffbc 271
Brignall 0:1a0321f1ffbc 272 // calculate heading with atan2 from x and y magnetometer measurements
Brignall 0:1a0321f1ffbc 273
Brignall 0:1a0321f1ffbc 274 heading = atan2(magnetometerX, magnetometerY);
Brignall 0:1a0321f1ffbc 275 }
Brignall 0:1a0321f1ffbc 276