Zürcher Eliteeinheit / Mbed 2 deprecated ROME2_P6

rome2_p6 imported

Dependencies:   mbed

IMU.cpp@5:957580f33e52, 2018-05-18 (annotated)

Committer:
Appalco
Date:
Fri May 18 13:54:25 2018 +0000
Revision:
5:957580f33e52
Parent:
0:351a2fb21235 
fixed tolerance and wayponts

Who changed what in which revision?

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