TeamSurface / Libs

Nim leo niiiim

IMU.cpp@0:da791f233257, 2018-05-11 (annotated)

Committer:
Kiwicjam
Date:
Fri May 11 12:21:19 2018 +0000
Revision:
0:da791f233257
start of rome2 p5;

Who changed what in which revision?

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