Nicolas Borla / Mbed 2 deprecated Example_for_Joel

IMU measurement + Speed controller

Dependencies:   mbed

IMU.cpp@1:17fdd812cb8d, 2019-05-30 (annotated)

Committer:
boro
Date:
Thu May 30 13:21:44 2019 +0000
Revision:
1:17fdd812cb8d
Parent:
0:5a93e4916fb1 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
boro 0:5a93e4916fb1 1 /*
boro 0:5a93e4916fb1 2 * Main.cpp
boro 0:5a93e4916fb1 3 * Copyright (c) 2018, ZHAW
boro 0:5a93e4916fb1 4 * All rights reserved.
boro 0:5a93e4916fb1 5 */
boro 0:5a93e4916fb1 6
boro 0:5a93e4916fb1 7 #include "IMU.h"
boro 0:5a93e4916fb1 8 #include "mbed.h"
boro 0:5a93e4916fb1 9
boro 0:5a93e4916fb1 10
boro 0:5a93e4916fb1 11
boro 0:5a93e4916fb1 12 using namespace std;
boro 0:5a93e4916fb1 13
boro 0:5a93e4916fb1 14 const float IMU::M_PI = 3.14159265358979323846f; // the mathematical constant PI
boro 0:5a93e4916fb1 15 const float IMU::SAMPLE_TIME = 0.001f;
boro 0:5a93e4916fb1 16 const float IMU::STD_ALPHA = 0.02f; // standard deviation Measurement noise sensor gx - R
boro 0:5a93e4916fb1 17 const float IMU::STD_OMEGA = 0.034f; // standard deviation Measurement noise sensor gx - R
boro 0:5a93e4916fb1 18
boro 0:5a93e4916fb1 19
boro 0:5a93e4916fb1 20 /**
boro 0:5a93e4916fb1 21 * Creates an IMU object.
boro 0:5a93e4916fb1 22 * @param spi a reference to an spi controller to use.
boro 0:5a93e4916fb1 23 * @param csAG the chip select output for the accelerometer and the gyro sensor.
boro 0:5a93e4916fb1 24 * @param csM the chip select output for the magnetometer.
boro 0:5a93e4916fb1 25 */
boro 0:5a93e4916fb1 26 IMU::IMU(SPI& spi, DigitalOut& csAG, DigitalOut& csM) : spi(spi), csAG(csAG), csM(csM)
boro 0:5a93e4916fb1 27 {
boro 0:5a93e4916fb1 28
boro 0:5a93e4916fb1 29 // initialize SPI interface
boro 0:5a93e4916fb1 30
boro 0:5a93e4916fb1 31 spi.format(8, 3);
boro 0:5a93e4916fb1 32 spi.frequency(1000000);
boro 0:5a93e4916fb1 33
boro 0:5a93e4916fb1 34 // reset chip select lines to logical high
boro 0:5a93e4916fb1 35
boro 0:5a93e4916fb1 36 csAG = 1;
boro 0:5a93e4916fb1 37 csM = 1;
boro 0:5a93e4916fb1 38
boro 0:5a93e4916fb1 39 // initialize accelerometer and gyro
boro 0:5a93e4916fb1 40
boro 0:5a93e4916fb1 41 writeRegister(csAG, CTRL_REG1_G, 0xC3); // ODR 952 Hz, full scale 245 deg/s
boro 0:5a93e4916fb1 42 writeRegister(csAG, CTRL_REG2_G, 0x00); // disable interrupt generation
boro 0:5a93e4916fb1 43 writeRegister(csAG, CTRL_REG3_G, 0x00); // disable low power mode, disable high pass filter, high pass cutoff frequency 57 Hz
boro 0:5a93e4916fb1 44 writeRegister(csAG, CTRL_REG4, 0x38); // enable gyro in all 3 axis
boro 0:5a93e4916fb1 45 writeRegister(csAG, CTRL_REG5_XL, 0x38); // no decimation, enable accelerometer in all 3 axis
boro 0:5a93e4916fb1 46 writeRegister(csAG, CTRL_REG6_XL, 0xC0); // ODR 952 Hz, full scale 2g
boro 0:5a93e4916fb1 47 writeRegister(csAG, CTRL_REG7_XL, 0x00); // high res mode disabled, filter bypassed
boro 0:5a93e4916fb1 48 writeRegister(csAG, CTRL_REG8, 0x00); // 4-wire SPI interface, LSB at lower address
boro 0:5a93e4916fb1 49 writeRegister(csAG, CTRL_REG9, 0x04); // disable gyro sleep mode, disable I2C interface, disable FIFO
boro 0:5a93e4916fb1 50 writeRegister(csAG, CTRL_REG10, 0x00); // self test disabled
boro 0:5a93e4916fb1 51
boro 0:5a93e4916fb1 52 // initialize magnetometer
boro 0:5a93e4916fb1 53
boro 0:5a93e4916fb1 54 writeRegister(csM, CTRL_REG1_M, 0x10); // temperature not compensated, low power mode for x & y axis, data rate 10 Hz
boro 0:5a93e4916fb1 55 writeRegister(csM, CTRL_REG2_M, 0x00); // full scale 4 gauss
boro 0:5a93e4916fb1 56 writeRegister(csM, CTRL_REG3_M, 0x80); // disable I2C interface, low power mode, SPI write only, continuous conversion mode
boro 0:5a93e4916fb1 57 writeRegister(csM, CTRL_REG4_M, 0x00); // low power mode for z axis, LSB at lower address
boro 0:5a93e4916fb1 58 writeRegister(csM, CTRL_REG5_M, 0x00); // fast read disabled
boro 0:5a93e4916fb1 59
boro 0:5a93e4916fb1 60 }
boro 0:5a93e4916fb1 61
boro 0:5a93e4916fb1 62 /**
boro 0:5a93e4916fb1 63 * Deletes the IMU object.
boro 0:5a93e4916fb1 64 */
boro 0:5a93e4916fb1 65 IMU::~IMU() {}
boro 0:5a93e4916fb1 66
boro 0:5a93e4916fb1 67 /**
boro 0:5a93e4916fb1 68 * This private method allows to write a register value.
boro 0:5a93e4916fb1 69 * @param cs the chip select output to use, either csAG or csM.
boro 0:5a93e4916fb1 70 * @param address the 7 bit address of the register.
boro 0:5a93e4916fb1 71 * @param value the value to write into the register.
boro 0:5a93e4916fb1 72 */
boro 0:5a93e4916fb1 73 void IMU::writeRegister(DigitalOut& cs, uint8_t address, uint8_t value)
boro 0:5a93e4916fb1 74 {
boro 0:5a93e4916fb1 75
boro 0:5a93e4916fb1 76 cs = 0;
boro 0:5a93e4916fb1 77
boro 0:5a93e4916fb1 78 spi.write(0x7F & address);
boro 0:5a93e4916fb1 79 spi.write(value & 0xFF);
boro 0:5a93e4916fb1 80
boro 0:5a93e4916fb1 81 cs = 1;
boro 0:5a93e4916fb1 82 }
boro 0:5a93e4916fb1 83
boro 0:5a93e4916fb1 84 /**
boro 0:5a93e4916fb1 85 * This private method allows to read a register value.
boro 0:5a93e4916fb1 86 * @param cs the chip select output to use, either csAG or csM.
boro 0:5a93e4916fb1 87 * @param address the 7 bit address of the register.
boro 0:5a93e4916fb1 88 * @return the value read from the register.
boro 0:5a93e4916fb1 89 */
boro 0:5a93e4916fb1 90 uint8_t IMU::readRegister(DigitalOut& cs, uint8_t address)
boro 0:5a93e4916fb1 91 {
boro 0:5a93e4916fb1 92
boro 0:5a93e4916fb1 93 cs = 0;
boro 0:5a93e4916fb1 94
boro 0:5a93e4916fb1 95 spi.write(0x80 | address);
boro 0:5a93e4916fb1 96 int32_t value = spi.write(0xFF);
boro 0:5a93e4916fb1 97
boro 0:5a93e4916fb1 98 cs = 1;
boro 0:5a93e4916fb1 99
boro 0:5a93e4916fb1 100 return static_cast<uint8_t>(value & 0xFF);
boro 0:5a93e4916fb1 101 }
boro 0:5a93e4916fb1 102
boro 0:5a93e4916fb1 103 /**
boro 0:5a93e4916fb1 104 * Reads the gyroscope about the x-axis.
boro 0:5a93e4916fb1 105 * @return the rotational speed about the x-axis given in [rad/s].
boro 0:5a93e4916fb1 106 */
boro 0:5a93e4916fb1 107 float IMU::readGyroX()
boro 0:5a93e4916fb1 108 {
boro 0:5a93e4916fb1 109
boro 0:5a93e4916fb1 110 uint8_t low = readRegister(csAG, OUT_X_L_G);
boro 0:5a93e4916fb1 111 uint8_t high = readRegister(csAG, OUT_X_H_G);
boro 0:5a93e4916fb1 112
boro 0:5a93e4916fb1 113 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
boro 0:5a93e4916fb1 114
boro 0:5a93e4916fb1 115 return static_cast<float>(value)/32768.0f*245.0f*M_PI/180.0f;
boro 0:5a93e4916fb1 116 }
boro 0:5a93e4916fb1 117
boro 0:5a93e4916fb1 118 /**
boro 0:5a93e4916fb1 119 * Reads the gyroscope about the y-axis.
boro 0:5a93e4916fb1 120 * @return the rotational speed about the y-axis given in [rad/s].
boro 0:5a93e4916fb1 121 */
boro 0:5a93e4916fb1 122 float IMU::readGyroY()
boro 0:5a93e4916fb1 123 {
boro 0:5a93e4916fb1 124
boro 0:5a93e4916fb1 125 uint8_t low = readRegister(csAG, OUT_Y_L_G);
boro 0:5a93e4916fb1 126 uint8_t high = readRegister(csAG, OUT_Y_H_G);
boro 0:5a93e4916fb1 127
boro 0:5a93e4916fb1 128 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
boro 0:5a93e4916fb1 129
boro 0:5a93e4916fb1 130 return static_cast<float>(value)/32768.0f*245.0f*M_PI/180.0f;
boro 0:5a93e4916fb1 131 }
boro 0:5a93e4916fb1 132
boro 0:5a93e4916fb1 133 /**
boro 0:5a93e4916fb1 134 * Reads the gyroscope about the z-axis.
boro 0:5a93e4916fb1 135 * @return the rotational speed about the z-axis given in [rad/s].
boro 0:5a93e4916fb1 136 */
boro 0:5a93e4916fb1 137 float IMU::readGyroZ()
boro 0:5a93e4916fb1 138 {
boro 0:5a93e4916fb1 139
boro 0:5a93e4916fb1 140 uint8_t low = readRegister(csAG, OUT_Z_L_G);
boro 0:5a93e4916fb1 141 uint8_t high = readRegister(csAG, OUT_Z_H_G);
boro 0:5a93e4916fb1 142
boro 0:5a93e4916fb1 143 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
boro 0:5a93e4916fb1 144
boro 0:5a93e4916fb1 145 return static_cast<float>(value)/32768.0f*245.0f*M_PI/180.0f;
boro 0:5a93e4916fb1 146 }
boro 0:5a93e4916fb1 147
boro 0:5a93e4916fb1 148 /**
boro 0:5a93e4916fb1 149 * Reads the acceleration in x-direction.
boro 0:5a93e4916fb1 150 * @return the acceleration in x-direction, given in [m/s2].
boro 0:5a93e4916fb1 151 */
boro 0:5a93e4916fb1 152 float IMU::readAccelerationX()
boro 0:5a93e4916fb1 153 {
boro 0:5a93e4916fb1 154
boro 0:5a93e4916fb1 155 uint8_t low = readRegister(csAG, OUT_X_L_XL);
boro 0:5a93e4916fb1 156 uint8_t high = readRegister(csAG, OUT_X_H_XL);
boro 0:5a93e4916fb1 157
boro 0:5a93e4916fb1 158 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
boro 0:5a93e4916fb1 159
boro 0:5a93e4916fb1 160 return static_cast<float>(value)/32768.0f*2.0f*9.81f;
boro 0:5a93e4916fb1 161 }
boro 0:5a93e4916fb1 162
boro 0:5a93e4916fb1 163 /**
boro 0:5a93e4916fb1 164 * Reads the acceleration in y-direction.
boro 0:5a93e4916fb1 165 * @return the acceleration in y-direction, given in [m/s2].
boro 0:5a93e4916fb1 166 */
boro 0:5a93e4916fb1 167 float IMU::readAccelerationY()
boro 0:5a93e4916fb1 168 {
boro 0:5a93e4916fb1 169
boro 0:5a93e4916fb1 170 uint8_t low = readRegister(csAG, OUT_Y_L_XL);
boro 0:5a93e4916fb1 171 uint8_t high = readRegister(csAG, OUT_Y_H_XL);
boro 0:5a93e4916fb1 172
boro 0:5a93e4916fb1 173 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
boro 0:5a93e4916fb1 174
boro 0:5a93e4916fb1 175 return static_cast<float>(value)/32768.0f*2.0f*9.81f;
boro 0:5a93e4916fb1 176 }
boro 0:5a93e4916fb1 177
boro 0:5a93e4916fb1 178 /**
boro 0:5a93e4916fb1 179 * Reads the acceleration in z-direction.
boro 0:5a93e4916fb1 180 * @return the acceleration in z-direction, given in [m/s2].
boro 0:5a93e4916fb1 181 */
boro 0:5a93e4916fb1 182 float IMU::readAccelerationZ()
boro 0:5a93e4916fb1 183 {
boro 0:5a93e4916fb1 184
boro 0:5a93e4916fb1 185 uint8_t low = readRegister(csAG, OUT_Z_L_XL);
boro 0:5a93e4916fb1 186 uint8_t high = readRegister(csAG, OUT_Z_H_XL);
boro 0:5a93e4916fb1 187
boro 0:5a93e4916fb1 188 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
boro 0:5a93e4916fb1 189
boro 0:5a93e4916fb1 190 return static_cast<float>(value)/32768.0f*2.0f*9.81f;
boro 0:5a93e4916fb1 191 }
boro 0:5a93e4916fb1 192
boro 0:5a93e4916fb1 193 /**
boro 0:5a93e4916fb1 194 * Reads the magnetic field in x-direction.
boro 0:5a93e4916fb1 195 * @return the magnetic field in x-direction, given in [Gauss].
boro 0:5a93e4916fb1 196 */
boro 0:5a93e4916fb1 197 float IMU::readMagnetometerX()
boro 0:5a93e4916fb1 198 {
boro 0:5a93e4916fb1 199
boro 0:5a93e4916fb1 200 uint8_t low = readRegister(csM, OUT_X_L_M);
boro 0:5a93e4916fb1 201 uint8_t high = readRegister(csM, OUT_X_H_M);
boro 0:5a93e4916fb1 202
boro 0:5a93e4916fb1 203 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
boro 0:5a93e4916fb1 204
boro 0:5a93e4916fb1 205 return static_cast<float>(value)/32768.0f*4.0f;
boro 0:5a93e4916fb1 206 }
boro 0:5a93e4916fb1 207
boro 0:5a93e4916fb1 208 /**
boro 0:5a93e4916fb1 209 * Reads the magnetic field in x-direction.
boro 0:5a93e4916fb1 210 * @return the magnetic field in x-direction, given in [Gauss].
boro 0:5a93e4916fb1 211 */
boro 0:5a93e4916fb1 212 float IMU::readMagnetometerY()
boro 0:5a93e4916fb1 213 {
boro 0:5a93e4916fb1 214
boro 0:5a93e4916fb1 215 uint8_t low = readRegister(csM, OUT_Y_L_M);
boro 0:5a93e4916fb1 216 uint8_t high = readRegister(csM, OUT_Y_H_M);
boro 0:5a93e4916fb1 217
boro 0:5a93e4916fb1 218 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
boro 0:5a93e4916fb1 219
boro 0:5a93e4916fb1 220 return static_cast<float>(value)/32768.0f*4.0f;
boro 0:5a93e4916fb1 221 }
boro 0:5a93e4916fb1 222
boro 0:5a93e4916fb1 223 /**
boro 0:5a93e4916fb1 224 * Reads the magnetic field in x-direction.
boro 0:5a93e4916fb1 225 * @return the magnetic field in x-direction, given in [Gauss].
boro 0:5a93e4916fb1 226 */
boro 0:5a93e4916fb1 227 float IMU::readMagnetometerZ()
boro 0:5a93e4916fb1 228 {
boro 0:5a93e4916fb1 229
boro 0:5a93e4916fb1 230 uint8_t low = readRegister(csM, OUT_Z_L_M);
boro 0:5a93e4916fb1 231 uint8_t high = readRegister(csM, OUT_Z_H_M);
boro 0:5a93e4916fb1 232
boro 0:5a93e4916fb1 233 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
boro 0:5a93e4916fb1 234
boro 0:5a93e4916fb1 235 return static_cast<float>(value)/32768.0f*4.0f;
boro 0:5a93e4916fb1 236 }
boro 0:5a93e4916fb1 237
boro 0:5a93e4916fb1 238