TeamSurface / ROME2_P4

Es loht sich compile und lieferet au Wert. Das wärs aber au scho gsi.

IMU.cpp@0:1a79273bc3e6, 2018-04-09 (annotated)

Committer:
wannesim
Date:
Mon Apr 09 07:00:45 2018 +0000
Revision:
0:1a79273bc3e6
D? Wille isch do gsi.; Es macht ?pis aber nemert wass wa...

Who changed what in which revision?

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