Zürcher Eliteeinheit / Mbed 2 deprecated ROME2_P4

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ROME2_P2 mbed

Fork of ROME2_P3 by Zürcher Eliteeinheit

IMU.cpp@6:67263dc2c2cf, 2018-04-26 (annotated)

Committer:
matajarb
Date:
Thu Apr 26 12:22:58 2018 +0000
Revision:
6:67263dc2c2cf
s?mme du schluch

Who changed what in which revision?

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