ROME2 / Mbed 2 deprecated ROME2_P5

ROME_P5

Dependencies:   mbed

IMU.cpp@0:29be10cb0afc, 2018-04-27 (annotated)

Committer:
Inaueadr
Date:
Fri Apr 27 08:47:34 2018 +0000
Revision:
0:29be10cb0afc
Hallo

Who changed what in which revision?

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