Nicolas Borla / Mbed OS BBR_1Ebene

BBR 1 Ebene

IMU.cpp@0:fbdae7e6d805, 2018-05-14 (annotated)

Committer:
borlanic
Date:
Mon May 14 11:29:06 2018 +0000
Revision:
0:fbdae7e6d805
BBR

Who changed what in which revision?

UserRevisionLine numberNew contents of line
borlanic 0:fbdae7e6d805 1 /*
borlanic 0:fbdae7e6d805 2 * IMU.cpp
borlanic 0:fbdae7e6d805 3 * Copyright (c) 2018, ZHAW
borlanic 0:fbdae7e6d805 4 * All righSAMPLE_TIME reserved.
borlanic 0:fbdae7e6d805 5 */
borlanic 0:fbdae7e6d805 6
borlanic 0:fbdae7e6d805 7 #include "IMU.h"
borlanic 0:fbdae7e6d805 8 #include "mbed.h"
borlanic 0:fbdae7e6d805 9 #include "IIR_filter.h"
borlanic 0:fbdae7e6d805 10
borlanic 0:fbdae7e6d805 11
borlanic 0:fbdae7e6d805 12
borlanic 0:fbdae7e6d805 13 using namespace std;
borlanic 0:fbdae7e6d805 14
borlanic 0:fbdae7e6d805 15 const float IMU::M_PI = 3.14159265358979323846f; // the mathematical constant PI
borlanic 0:fbdae7e6d805 16
borlanic 0:fbdae7e6d805 17 // Nick ====================================
borlanic 0:fbdae7e6d805 18 const float IMU::SAMPLE_TIME = 0.001f;
borlanic 0:fbdae7e6d805 19 const float IMU::STD_ALPHA = 0.02f; // Messrauschen sensor standardabweichung gx - R
borlanic 0:fbdae7e6d805 20 const float IMU::STD_OMEGA = 0.034f; // Messrauschen sensor standardabweichung gx - R
borlanic 0:fbdae7e6d805 21 //==========================================
borlanic 0:fbdae7e6d805 22
borlanic 0:fbdae7e6d805 23 /**
borlanic 0:fbdae7e6d805 24 * Creates an IMU object.
borlanic 0:fbdae7e6d805 25 * @param spi a reference to an spi controller to use.
borlanic 0:fbdae7e6d805 26 * @param csAG the chip select output for the accelerometer and the gyro sensor.
borlanic 0:fbdae7e6d805 27 * @param csM the chip select output for the magnetometer.
borlanic 0:fbdae7e6d805 28 */
borlanic 0:fbdae7e6d805 29 IMU::IMU(SPI& spi, DigitalOut& csAG, DigitalOut& csM) : spi(spi), csAG(csAG), csM(csM), thread(osPriorityHigh, STACK_SIZE)
borlanic 0:fbdae7e6d805 30 {
borlanic 0:fbdae7e6d805 31
borlanic 0:fbdae7e6d805 32 // initialize SPI interface
borlanic 0:fbdae7e6d805 33
borlanic 0:fbdae7e6d805 34 spi.format(8, 3);
borlanic 0:fbdae7e6d805 35 spi.frequency(1000000);
borlanic 0:fbdae7e6d805 36
borlanic 0:fbdae7e6d805 37 // reset chip select lines to logical high
borlanic 0:fbdae7e6d805 38
borlanic 0:fbdae7e6d805 39 csAG = 1;
borlanic 0:fbdae7e6d805 40 csM = 1;
borlanic 0:fbdae7e6d805 41
borlanic 0:fbdae7e6d805 42 // initialize accelerometer and gyro
borlanic 0:fbdae7e6d805 43
borlanic 0:fbdae7e6d805 44 writeRegister(csAG, CTRL_REG1_G, 0xC3); // ODR 952 Hz, full scale 245 deg/s
borlanic 0:fbdae7e6d805 45 writeRegister(csAG, CTRL_REG2_G, 0x00); // disable interrupt generation
borlanic 0:fbdae7e6d805 46 writeRegister(csAG, CTRL_REG3_G, 0x00); // disable low power mode, disable high pass filter, high pass cutoff frequency 57 Hz
borlanic 0:fbdae7e6d805 47 writeRegister(csAG, CTRL_REG4, 0x38); // enable gyro in all 3 axis
borlanic 0:fbdae7e6d805 48 writeRegister(csAG, CTRL_REG5_XL, 0x38); // no decimation, enable accelerometer in all 3 axis
borlanic 0:fbdae7e6d805 49 writeRegister(csAG, CTRL_REG6_XL, 0xC0); // ODR 952 Hz, full scale 2g
borlanic 0:fbdae7e6d805 50 writeRegister(csAG, CTRL_REG7_XL, 0x00); // high res mode disabled, filter bypassed
borlanic 0:fbdae7e6d805 51 writeRegister(csAG, CTRL_REG8, 0x00); // 4-wire SPI interface, LSB at lower address
borlanic 0:fbdae7e6d805 52 writeRegister(csAG, CTRL_REG9, 0x04); // disable gyro sleep mode, disable I2C interface, disable FIFO
borlanic 0:fbdae7e6d805 53 writeRegister(csAG, CTRL_REG10, 0x00); // self test disabled
borlanic 0:fbdae7e6d805 54
borlanic 0:fbdae7e6d805 55 // initialize magnetometer
borlanic 0:fbdae7e6d805 56
borlanic 0:fbdae7e6d805 57 writeRegister(csM, CTRL_REG1_M, 0x10); // temperature not compensated, low power mode for x & y axis, data rate 10 Hz
borlanic 0:fbdae7e6d805 58 writeRegister(csM, CTRL_REG2_M, 0x00); // full scale 4 gauss
borlanic 0:fbdae7e6d805 59 writeRegister(csM, CTRL_REG3_M, 0x80); // disable I2C interface, low power mode, SPI write only, continuous conversion mode
borlanic 0:fbdae7e6d805 60 writeRegister(csM, CTRL_REG4_M, 0x00); // low power mode for z axis, LSB at lower address
borlanic 0:fbdae7e6d805 61 writeRegister(csM, CTRL_REG5_M, 0x00); // fast read disabled
borlanic 0:fbdae7e6d805 62
borlanic 0:fbdae7e6d805 63 gammaXFilter.setPeriod(SAMPLE_TIME);
borlanic 0:fbdae7e6d805 64 gammaXFilter.setFrequency(300.0f);
borlanic 0:fbdae7e6d805 65 gammaYFilter.setPeriod(SAMPLE_TIME);
borlanic 0:fbdae7e6d805 66 gammaYFilter.setFrequency(300.0f);
borlanic 0:fbdae7e6d805 67 d_gammaXFilter.setPeriod(SAMPLE_TIME);
borlanic 0:fbdae7e6d805 68 d_gammaXFilter.setFrequency(300.0f);
borlanic 0:fbdae7e6d805 69 d_gammaYFilter.setPeriod(SAMPLE_TIME);
borlanic 0:fbdae7e6d805 70 d_gammaYFilter.setFrequency(300.0f);
borlanic 0:fbdae7e6d805 71
borlanic 0:fbdae7e6d805 72 thread.start(callback(this, &IMU::kalman));
borlanic 0:fbdae7e6d805 73 ticker.attach(callback(this, &IMU::sendSignal), SAMPLE_TIME);
borlanic 0:fbdae7e6d805 74 }
borlanic 0:fbdae7e6d805 75
borlanic 0:fbdae7e6d805 76 /**
borlanic 0:fbdae7e6d805 77 * Deletes the IMU object.
borlanic 0:fbdae7e6d805 78 */
borlanic 0:fbdae7e6d805 79 IMU::~IMU() {}
borlanic 0:fbdae7e6d805 80
borlanic 0:fbdae7e6d805 81 /**
borlanic 0:fbdae7e6d805 82 * This private method allows to write a register value.
borlanic 0:fbdae7e6d805 83 * @param cs the chip select output to use, either csAG or csM.
borlanic 0:fbdae7e6d805 84 * @param address the 7 bit address of the register.
borlanic 0:fbdae7e6d805 85 * @param value the value to write into the register.
borlanic 0:fbdae7e6d805 86 */
borlanic 0:fbdae7e6d805 87 void IMU::writeRegister(DigitalOut& cs, uint8_t address, uint8_t value)
borlanic 0:fbdae7e6d805 88 {
borlanic 0:fbdae7e6d805 89
borlanic 0:fbdae7e6d805 90 cs = 0;
borlanic 0:fbdae7e6d805 91
borlanic 0:fbdae7e6d805 92 spi.write(0x7F & address);
borlanic 0:fbdae7e6d805 93 spi.write(value & 0xFF);
borlanic 0:fbdae7e6d805 94
borlanic 0:fbdae7e6d805 95 cs = 1;
borlanic 0:fbdae7e6d805 96 }
borlanic 0:fbdae7e6d805 97
borlanic 0:fbdae7e6d805 98 /**
borlanic 0:fbdae7e6d805 99 * This private method allows to read a register value.
borlanic 0:fbdae7e6d805 100 * @param cs the chip select output to use, either csAG or csM.
borlanic 0:fbdae7e6d805 101 * @param address the 7 bit address of the register.
borlanic 0:fbdae7e6d805 102 * @return the value read from the register.
borlanic 0:fbdae7e6d805 103 */
borlanic 0:fbdae7e6d805 104 uint8_t IMU::readRegister(DigitalOut& cs, uint8_t address)
borlanic 0:fbdae7e6d805 105 {
borlanic 0:fbdae7e6d805 106
borlanic 0:fbdae7e6d805 107 cs = 0;
borlanic 0:fbdae7e6d805 108
borlanic 0:fbdae7e6d805 109 spi.write(0x80 | address);
borlanic 0:fbdae7e6d805 110 int32_t value = spi.write(0xFF);
borlanic 0:fbdae7e6d805 111
borlanic 0:fbdae7e6d805 112 cs = 1;
borlanic 0:fbdae7e6d805 113
borlanic 0:fbdae7e6d805 114 return static_cast<uint8_t>(value & 0xFF);
borlanic 0:fbdae7e6d805 115 }
borlanic 0:fbdae7e6d805 116
borlanic 0:fbdae7e6d805 117 /**
borlanic 0:fbdae7e6d805 118 * Reads the gyroscope about the x-axis.
borlanic 0:fbdae7e6d805 119 * @return the rotational speed about the x-axis given in [rad/s].
borlanic 0:fbdae7e6d805 120 */
borlanic 0:fbdae7e6d805 121 float IMU::readGyroX()
borlanic 0:fbdae7e6d805 122 {
borlanic 0:fbdae7e6d805 123
borlanic 0:fbdae7e6d805 124 uint8_t low = readRegister(csAG, OUT_X_L_G);
borlanic 0:fbdae7e6d805 125 uint8_t high = readRegister(csAG, OUT_X_H_G);
borlanic 0:fbdae7e6d805 126
borlanic 0:fbdae7e6d805 127 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
borlanic 0:fbdae7e6d805 128
borlanic 0:fbdae7e6d805 129 return static_cast<float>(value)/32768.0f*245.0f*M_PI/180.0f;
borlanic 0:fbdae7e6d805 130 }
borlanic 0:fbdae7e6d805 131
borlanic 0:fbdae7e6d805 132 /**
borlanic 0:fbdae7e6d805 133 * Reads the gyroscope about the y-axis.
borlanic 0:fbdae7e6d805 134 * @return the rotational speed about the y-axis given in [rad/s].
borlanic 0:fbdae7e6d805 135 */
borlanic 0:fbdae7e6d805 136 float IMU::readGyroY()
borlanic 0:fbdae7e6d805 137 {
borlanic 0:fbdae7e6d805 138
borlanic 0:fbdae7e6d805 139 uint8_t low = readRegister(csAG, OUT_Y_L_G);
borlanic 0:fbdae7e6d805 140 uint8_t high = readRegister(csAG, OUT_Y_H_G);
borlanic 0:fbdae7e6d805 141
borlanic 0:fbdae7e6d805 142 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
borlanic 0:fbdae7e6d805 143
borlanic 0:fbdae7e6d805 144 return static_cast<float>(value)/32768.0f*245.0f*M_PI/180.0f;
borlanic 0:fbdae7e6d805 145 }
borlanic 0:fbdae7e6d805 146
borlanic 0:fbdae7e6d805 147 /**
borlanic 0:fbdae7e6d805 148 * Reads the gyroscope about the z-axis.
borlanic 0:fbdae7e6d805 149 * @return the rotational speed about the z-axis given in [rad/s].
borlanic 0:fbdae7e6d805 150 */
borlanic 0:fbdae7e6d805 151 float IMU::readGyroZ()
borlanic 0:fbdae7e6d805 152 {
borlanic 0:fbdae7e6d805 153
borlanic 0:fbdae7e6d805 154 uint8_t low = readRegister(csAG, OUT_Z_L_G);
borlanic 0:fbdae7e6d805 155 uint8_t high = readRegister(csAG, OUT_Z_H_G);
borlanic 0:fbdae7e6d805 156
borlanic 0:fbdae7e6d805 157 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
borlanic 0:fbdae7e6d805 158
borlanic 0:fbdae7e6d805 159 return static_cast<float>(value)/32768.0f*245.0f*M_PI/180.0f;
borlanic 0:fbdae7e6d805 160 }
borlanic 0:fbdae7e6d805 161
borlanic 0:fbdae7e6d805 162 /**
borlanic 0:fbdae7e6d805 163 * Reads the acceleration in x-direction.
borlanic 0:fbdae7e6d805 164 * @return the acceleration in x-direction, given in [m/s2].
borlanic 0:fbdae7e6d805 165 */
borlanic 0:fbdae7e6d805 166 float IMU::readAccelerationX()
borlanic 0:fbdae7e6d805 167 {
borlanic 0:fbdae7e6d805 168
borlanic 0:fbdae7e6d805 169 uint8_t low = readRegister(csAG, OUT_X_L_XL);
borlanic 0:fbdae7e6d805 170 uint8_t high = readRegister(csAG, OUT_X_H_XL);
borlanic 0:fbdae7e6d805 171
borlanic 0:fbdae7e6d805 172 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
borlanic 0:fbdae7e6d805 173
borlanic 0:fbdae7e6d805 174 return static_cast<float>(value)/32768.0f*2.0f*9.81f;
borlanic 0:fbdae7e6d805 175 }
borlanic 0:fbdae7e6d805 176
borlanic 0:fbdae7e6d805 177 /**
borlanic 0:fbdae7e6d805 178 * Reads the acceleration in y-direction.
borlanic 0:fbdae7e6d805 179 * @return the acceleration in y-direction, given in [m/s2].
borlanic 0:fbdae7e6d805 180 */
borlanic 0:fbdae7e6d805 181 float IMU::readAccelerationY()
borlanic 0:fbdae7e6d805 182 {
borlanic 0:fbdae7e6d805 183
borlanic 0:fbdae7e6d805 184 uint8_t low = readRegister(csAG, OUT_Y_L_XL);
borlanic 0:fbdae7e6d805 185 uint8_t high = readRegister(csAG, OUT_Y_H_XL);
borlanic 0:fbdae7e6d805 186
borlanic 0:fbdae7e6d805 187 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
borlanic 0:fbdae7e6d805 188
borlanic 0:fbdae7e6d805 189 return static_cast<float>(value)/32768.0f*2.0f*9.81f;
borlanic 0:fbdae7e6d805 190 }
borlanic 0:fbdae7e6d805 191
borlanic 0:fbdae7e6d805 192 /**
borlanic 0:fbdae7e6d805 193 * Reads the acceleration in z-direction.
borlanic 0:fbdae7e6d805 194 * @return the acceleration in z-direction, given in [m/s2].
borlanic 0:fbdae7e6d805 195 */
borlanic 0:fbdae7e6d805 196 float IMU::readAccelerationZ()
borlanic 0:fbdae7e6d805 197 {
borlanic 0:fbdae7e6d805 198
borlanic 0:fbdae7e6d805 199 uint8_t low = readRegister(csAG, OUT_Z_L_XL);
borlanic 0:fbdae7e6d805 200 uint8_t high = readRegister(csAG, OUT_Z_H_XL);
borlanic 0:fbdae7e6d805 201
borlanic 0:fbdae7e6d805 202 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
borlanic 0:fbdae7e6d805 203
borlanic 0:fbdae7e6d805 204 return static_cast<float>(value)/32768.0f*2.0f*9.81f;
borlanic 0:fbdae7e6d805 205 }
borlanic 0:fbdae7e6d805 206
borlanic 0:fbdae7e6d805 207 /**
borlanic 0:fbdae7e6d805 208 * Reads the magnetic field in x-direction.
borlanic 0:fbdae7e6d805 209 * @return the magnetic field in x-direction, given in [Gauss].
borlanic 0:fbdae7e6d805 210 */
borlanic 0:fbdae7e6d805 211 float IMU::readMagnetometerX()
borlanic 0:fbdae7e6d805 212 {
borlanic 0:fbdae7e6d805 213
borlanic 0:fbdae7e6d805 214 uint8_t low = readRegister(csM, OUT_X_L_M);
borlanic 0:fbdae7e6d805 215 uint8_t high = readRegister(csM, OUT_X_H_M);
borlanic 0:fbdae7e6d805 216
borlanic 0:fbdae7e6d805 217 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
borlanic 0:fbdae7e6d805 218
borlanic 0:fbdae7e6d805 219 return static_cast<float>(value)/32768.0f*4.0f;
borlanic 0:fbdae7e6d805 220 }
borlanic 0:fbdae7e6d805 221
borlanic 0:fbdae7e6d805 222 /**
borlanic 0:fbdae7e6d805 223 * Reads the magnetic field in x-direction.
borlanic 0:fbdae7e6d805 224 * @return the magnetic field in x-direction, given in [Gauss].
borlanic 0:fbdae7e6d805 225 */
borlanic 0:fbdae7e6d805 226 float IMU::readMagnetometerY()
borlanic 0:fbdae7e6d805 227 {
borlanic 0:fbdae7e6d805 228
borlanic 0:fbdae7e6d805 229 uint8_t low = readRegister(csM, OUT_Y_L_M);
borlanic 0:fbdae7e6d805 230 uint8_t high = readRegister(csM, OUT_Y_H_M);
borlanic 0:fbdae7e6d805 231
borlanic 0:fbdae7e6d805 232 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
borlanic 0:fbdae7e6d805 233
borlanic 0:fbdae7e6d805 234 return static_cast<float>(value)/32768.0f*4.0f;
borlanic 0:fbdae7e6d805 235 }
borlanic 0:fbdae7e6d805 236
borlanic 0:fbdae7e6d805 237 /**
borlanic 0:fbdae7e6d805 238 * Reads the magnetic field in x-direction.
borlanic 0:fbdae7e6d805 239 * @return the magnetic field in x-direction, given in [Gauss].
borlanic 0:fbdae7e6d805 240 */
borlanic 0:fbdae7e6d805 241 float IMU::readMagnetometerZ()
borlanic 0:fbdae7e6d805 242 {
borlanic 0:fbdae7e6d805 243
borlanic 0:fbdae7e6d805 244 uint8_t low = readRegister(csM, OUT_Z_L_M);
borlanic 0:fbdae7e6d805 245 uint8_t high = readRegister(csM, OUT_Z_H_M);
borlanic 0:fbdae7e6d805 246
borlanic 0:fbdae7e6d805 247 int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
borlanic 0:fbdae7e6d805 248
borlanic 0:fbdae7e6d805 249 return static_cast<float>(value)/32768.0f*4.0f;
borlanic 0:fbdae7e6d805 250 }
borlanic 0:fbdae7e6d805 251
borlanic 0:fbdae7e6d805 252 float IMU::getGammaX()
borlanic 0:fbdae7e6d805 253 {
borlanic 0:fbdae7e6d805 254 return gammaX;
borlanic 0:fbdae7e6d805 255 }
borlanic 0:fbdae7e6d805 256
borlanic 0:fbdae7e6d805 257 float IMU::getGammaY()
borlanic 0:fbdae7e6d805 258 {
borlanic 0:fbdae7e6d805 259 return gammaY;
borlanic 0:fbdae7e6d805 260 }
borlanic 0:fbdae7e6d805 261
borlanic 0:fbdae7e6d805 262 float IMU::getGammaZ()
borlanic 0:fbdae7e6d805 263 {
borlanic 0:fbdae7e6d805 264 return gammaZ;
borlanic 0:fbdae7e6d805 265 }
borlanic 0:fbdae7e6d805 266
borlanic 0:fbdae7e6d805 267 float IMU::getDGammaX()
borlanic 0:fbdae7e6d805 268 {
borlanic 0:fbdae7e6d805 269 return d_gammaX;
borlanic 0:fbdae7e6d805 270 }
borlanic 0:fbdae7e6d805 271
borlanic 0:fbdae7e6d805 272 float IMU::getDGammaY()
borlanic 0:fbdae7e6d805 273 {
borlanic 0:fbdae7e6d805 274 return d_gammaY;
borlanic 0:fbdae7e6d805 275 }
borlanic 0:fbdae7e6d805 276
borlanic 0:fbdae7e6d805 277 float IMU::getDGammaZ()
borlanic 0:fbdae7e6d805 278 {
borlanic 0:fbdae7e6d805 279 return d_gammaZ;
borlanic 0:fbdae7e6d805 280 }
borlanic 0:fbdae7e6d805 281
borlanic 0:fbdae7e6d805 282 void IMU::sendSignal() {
borlanic 0:fbdae7e6d805 283
borlanic 0:fbdae7e6d805 284 thread.signal_set(signal);
borlanic 0:fbdae7e6d805 285 }
borlanic 0:fbdae7e6d805 286
borlanic 0:fbdae7e6d805 287 void IMU::kalman()
borlanic 0:fbdae7e6d805 288 {
borlanic 0:fbdae7e6d805 289
borlanic 0:fbdae7e6d805 290 Serial pc1(USBTX, USBRX); // tx, rx
borlanic 0:fbdae7e6d805 291 pc1.baud(100000);
borlanic 0:fbdae7e6d805 292
borlanic 0:fbdae7e6d805 293 // Messrauschen sensor standardabweichung gx - R
borlanic 0:fbdae7e6d805 294 float R11 = STD_ALPHA*STD_ALPHA;
borlanic 0:fbdae7e6d805 295 float R22 = STD_OMEGA*STD_OMEGA;
borlanic 0:fbdae7e6d805 296
borlanic 0:fbdae7e6d805 297 // Messrauschen prozessor - Q
borlanic 0:fbdae7e6d805 298 float Q11 = 0.0000001f;
borlanic 0:fbdae7e6d805 299 float Q22 = 0.0001f;
borlanic 0:fbdae7e6d805 300
borlanic 0:fbdae7e6d805 301 // Matrix A
borlanic 0:fbdae7e6d805 302 float A11 = 1.0f;
borlanic 0:fbdae7e6d805 303 float A12 = SAMPLE_TIME;
borlanic 0:fbdae7e6d805 304 float A21 = 0.0f;
borlanic 0:fbdae7e6d805 305 float A22 = 1.0f;
borlanic 0:fbdae7e6d805 306
borlanic 0:fbdae7e6d805 307 // rot X
borlanic 0:fbdae7e6d805 308 float alpha_p_x = 0.0f;
borlanic 0:fbdae7e6d805 309 float omega_p_x = 0.0f;
borlanic 0:fbdae7e6d805 310 float Pk_x11 = 0.001f;
borlanic 0:fbdae7e6d805 311 float Pk_x12 = 0.0f;
borlanic 0:fbdae7e6d805 312 float Pk_x21 = 0.0f;
borlanic 0:fbdae7e6d805 313 float Pk_x22 = 0.001f;
borlanic 0:fbdae7e6d805 314 float alpha_korr_x = 0.0f;
borlanic 0:fbdae7e6d805 315 float omega_korr_x = 0.0f;
borlanic 0:fbdae7e6d805 316
borlanic 0:fbdae7e6d805 317 // rot Y
borlanic 0:fbdae7e6d805 318 float alpha_p_y = 0.0f;
borlanic 0:fbdae7e6d805 319 float omega_p_y = 0.0f;
borlanic 0:fbdae7e6d805 320 float Pk_y11 = 0.001f;
borlanic 0:fbdae7e6d805 321 float Pk_y12 = 0.0f;
borlanic 0:fbdae7e6d805 322 float Pk_y21 = 0.0f;
borlanic 0:fbdae7e6d805 323 float Pk_y22 = 0.001f;
borlanic 0:fbdae7e6d805 324 float alpha_korr_y = 0.0f;
borlanic 0:fbdae7e6d805 325 float omega_korr_y = 0.0f;
borlanic 0:fbdae7e6d805 326
borlanic 0:fbdae7e6d805 327 // rot Z
borlanic 0:fbdae7e6d805 328 float alpha_p_z = 0.0f;
borlanic 0:fbdae7e6d805 329 float omega_p_z = 0.0f;
borlanic 0:fbdae7e6d805 330 float Pk_z11 = 0.001f;
borlanic 0:fbdae7e6d805 331 float Pk_z12 = 0.0f;
borlanic 0:fbdae7e6d805 332 float Pk_z21 = 0.0f;
borlanic 0:fbdae7e6d805 333 float Pk_z22 = 0.001f;
borlanic 0:fbdae7e6d805 334 float alpha_korr_z = 0.0f;
borlanic 0:fbdae7e6d805 335 float omega_korr_z = 0.0f;
borlanic 0:fbdae7e6d805 336
borlanic 0:fbdae7e6d805 337 double mx_f_vor=this->readMagnetometerX();
borlanic 0:fbdae7e6d805 338 double mx_vor=this->readMagnetometerX();
borlanic 0:fbdae7e6d805 339
borlanic 0:fbdae7e6d805 340 double my_f_vor=this->readMagnetometerY();
borlanic 0:fbdae7e6d805 341 double my_vor=this->readMagnetometerY();
borlanic 0:fbdae7e6d805 342
borlanic 0:fbdae7e6d805 343 int t = 0;
borlanic 0:fbdae7e6d805 344 float gamma_z_int = 0;
borlanic 0:fbdae7e6d805 345
borlanic 0:fbdae7e6d805 346 // messung
borlanic 0:fbdae7e6d805 347
borlanic 0:fbdae7e6d805 348 int * T = new int[2000];
borlanic 0:fbdae7e6d805 349 float * Mes1 = new float[2000];
borlanic 0:fbdae7e6d805 350 float * Mes2 = new float[2000];
borlanic 0:fbdae7e6d805 351 float * Mes3 = new float[2000];
borlanic 0:fbdae7e6d805 352 float * Mes4 = new float[2000];
borlanic 0:fbdae7e6d805 353 float * Mes5 = new float[2000];
borlanic 0:fbdae7e6d805 354 float * Mes6 = new float[2000];
borlanic 0:fbdae7e6d805 355
borlanic 0:fbdae7e6d805 356
borlanic 0:fbdae7e6d805 357 int a = 0;
borlanic 0:fbdae7e6d805 358
borlanic 0:fbdae7e6d805 359 // initialise Lowpass filters for compl.fil.
borlanic 0:fbdae7e6d805 360 float tau = 1.0;
borlanic 0:fbdae7e6d805 361 IIR_filter FilterACCx(tau, SAMPLE_TIME, 1.0f); // 1st order LP for complementary filter acc_x
borlanic 0:fbdae7e6d805 362 IIR_filter FilterACCz(tau, SAMPLE_TIME, 1.0f); // 1st order LP for complementary filter acc_y
borlanic 0:fbdae7e6d805 363 IIR_filter FilterACCy(tau, SAMPLE_TIME, 1.0f); // 1st order LP for complementary filter acc_y
borlanic 0:fbdae7e6d805 364 IIR_filter FilterGYRY(tau, SAMPLE_TIME, tau); // 1st order LP for complementary filter gyro
borlanic 0:fbdae7e6d805 365 IIR_filter FilterGYRX(tau, SAMPLE_TIME, tau); // 1st order LP for complementary filter gyro
borlanic 0:fbdae7e6d805 366
borlanic 0:fbdae7e6d805 367 while (true) {
borlanic 0:fbdae7e6d805 368 /*Kalman Filter--------------------------------------------------*/
borlanic 0:fbdae7e6d805 369
borlanic 0:fbdae7e6d805 370 // wait for the periodic signal
borlanic 0:fbdae7e6d805 371
borlanic 0:fbdae7e6d805 372 thread.signal_wait(signal);
borlanic 0:fbdae7e6d805 373 //pc1.printf("IMU\r\n");
borlanic 0:fbdae7e6d805 374
borlanic 0:fbdae7e6d805 375 /*
borlanic 0:fbdae7e6d805 376 if(t==21000) {
borlanic 0:fbdae7e6d805 377 pc1.printf("invio dati:\r\n\n");
borlanic 0:fbdae7e6d805 378 for(int j=0; j<2000; j++) {
borlanic 0:fbdae7e6d805 379 //pc1.printf("%d %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n",*(T+j),*(PX+j),*(PY+j),*(GX+j),*(GY+j),*(GZ+j),*(dGX+j),*(dGY+j),*(dGZ+j),*(dPX+j),*(dPY+j),*(W1+j),*(W2+j),*(W3+j));
borlanic 0:fbdae7e6d805 380 pc1.printf("%d %.7f %.7f %.7f %.7f %.7f %.7f\r\n",*(T+j),*(Mes1+j),*(Mes2+j),*(Mes3+j),*(Mes4+j),*(Mes5+j),*(Mes6+j));
borlanic 0:fbdae7e6d805 381 }
borlanic 0:fbdae7e6d805 382 pc1.printf("fine dati:\r\n\n");
borlanic 0:fbdae7e6d805 383 delete T;
borlanic 0:fbdae7e6d805 384 delete Mes1;
borlanic 0:fbdae7e6d805 385 delete Mes2;
borlanic 0:fbdae7e6d805 386 delete Mes3;
borlanic 0:fbdae7e6d805 387 delete Mes4;
borlanic 0:fbdae7e6d805 388 delete Mes5;
borlanic 0:fbdae7e6d805 389 delete Mes6;
borlanic 0:fbdae7e6d805 390
borlanic 0:fbdae7e6d805 391 }
borlanic 0:fbdae7e6d805 392 */
borlanic 0:fbdae7e6d805 393
borlanic 0:fbdae7e6d805 394
borlanic 0:fbdae7e6d805 395 //printf("IMU start\r\n");
borlanic 0:fbdae7e6d805 396
borlanic 0:fbdae7e6d805 397 float ax = this->readAccelerationX();
borlanic 0:fbdae7e6d805 398 float ay = this->readAccelerationY();
borlanic 0:fbdae7e6d805 399 float az = this->readAccelerationZ();
borlanic 0:fbdae7e6d805 400
borlanic 0:fbdae7e6d805 401 float gx = this->readGyroX();
borlanic 0:fbdae7e6d805 402 float gy = this->readGyroY();
borlanic 0:fbdae7e6d805 403 float gz = this->readGyroZ();
borlanic 0:fbdae7e6d805 404
borlanic 0:fbdae7e6d805 405 float mx = this->readMagnetometerX();
borlanic 0:fbdae7e6d805 406 float my = this->readMagnetometerY();
borlanic 0:fbdae7e6d805 407 float mz = this->readMagnetometerZ();
borlanic 0:fbdae7e6d805 408
borlanic 0:fbdae7e6d805 409 // LowPass Magnetometer
borlanic 0:fbdae7e6d805 410 float RC = 1.0/(10*2*3.14); // Cutoff 10Hz
borlanic 0:fbdae7e6d805 411 float dt = 1.0/SAMPLE_TIME;
borlanic 0:fbdae7e6d805 412 float alpha = dt/(RC+dt);
borlanic 0:fbdae7e6d805 413
borlanic 0:fbdae7e6d805 414 float mx_f = mx_f_vor + (alpha*(mx-mx_vor));
borlanic 0:fbdae7e6d805 415 float my_f = my_f_vor + (alpha*(my-my_vor));
borlanic 0:fbdae7e6d805 416
borlanic 0:fbdae7e6d805 417 mx_f_vor = mx_f;
borlanic 0:fbdae7e6d805 418 mx_vor = mx;
borlanic 0:fbdae7e6d805 419 my_f_vor = my_f;
borlanic 0:fbdae7e6d805 420 my_vor = my;
borlanic 0:fbdae7e6d805 421
borlanic 0:fbdae7e6d805 422 // rot x
borlanic 0:fbdae7e6d805 423 float alpha_x = atan2(-ay,az);
borlanic 0:fbdae7e6d805 424 float omega_x = gx;
borlanic 0:fbdae7e6d805 425
borlanic 0:fbdae7e6d805 426 // rot y
borlanic 0:fbdae7e6d805 427 float alpha_y = atan2(-ax,az);
borlanic 0:fbdae7e6d805 428 float omega_y = -gy;
borlanic 0:fbdae7e6d805 429
borlanic 0:fbdae7e6d805 430 // rot z
borlanic 0:fbdae7e6d805 431 float mx_fil = (mx_f+0.3614f)*11.5937f;
borlanic 0:fbdae7e6d805 432 float my_fil = (my_f-0.4466f)*15.2002f;
borlanic 0:fbdae7e6d805 433 float alpha_z = atan2(my_fil,mx_fil);// Sostituire con calcolo gamma encoder
borlanic 0:fbdae7e6d805 434 float omega_z = gz;
borlanic 0:fbdae7e6d805 435
borlanic 0:fbdae7e6d805 436 /*
borlanic 0:fbdae7e6d805 437 float alpha_z = 0.024f/(0.095f*3.0f*0.7071f)*(w1+w2+w3)
borlanic 0:fbdae7e6d805 438 */
borlanic 0:fbdae7e6d805 439
borlanic 0:fbdae7e6d805 440 // Prediction
borlanic 0:fbdae7e6d805 441 // x
borlanic 0:fbdae7e6d805 442 alpha_p_x = alpha_p_x + SAMPLE_TIME*omega_x;
borlanic 0:fbdae7e6d805 443 omega_p_x = omega_p_x;
borlanic 0:fbdae7e6d805 444 Pk_x11 = Q11 + A11*(A11*Pk_x11 + A12*Pk_x21) + A12*(A11*Pk_x12 + A12*Pk_x22);
borlanic 0:fbdae7e6d805 445 Pk_x12 = A21*(A11*Pk_x11 + A12*Pk_x21) + A22*(A11*Pk_x12 + A12*Pk_x22);
borlanic 0:fbdae7e6d805 446 Pk_x21 = A11*(A21*Pk_x11 + A22*Pk_x21) + A12*(A21*Pk_x12 + A22*Pk_x22);
borlanic 0:fbdae7e6d805 447 Pk_x22 = Q22 + A21*(A21*Pk_x11 + A22*Pk_x21) + A22*(A21*Pk_x12 + A22*Pk_x22);
borlanic 0:fbdae7e6d805 448 // y
borlanic 0:fbdae7e6d805 449 alpha_p_y = alpha_p_y + SAMPLE_TIME*omega_y;
borlanic 0:fbdae7e6d805 450 omega_p_y = omega_p_y;
borlanic 0:fbdae7e6d805 451 Pk_y11 = Q11 + A11*(A11*Pk_y11 + A12*Pk_y21) + A12*(A11*Pk_y12 + A12*Pk_y22);
borlanic 0:fbdae7e6d805 452 Pk_y12 = A21*(A11*Pk_y11 + A12*Pk_y21) + A22*(A11*Pk_y12 + A12*Pk_y22);
borlanic 0:fbdae7e6d805 453 Pk_y21 = A11*(A21*Pk_y11 + A22*Pk_y21) + A12*(A21*Pk_y12 + A22*Pk_y22);
borlanic 0:fbdae7e6d805 454 Pk_y22 = Q22 + A21*(A21*Pk_y11 + A22*Pk_y21) + A22*(A21*Pk_y12 + A22*Pk_y22);
borlanic 0:fbdae7e6d805 455 // z
borlanic 0:fbdae7e6d805 456 alpha_p_z = alpha_p_z + SAMPLE_TIME*omega_z;
borlanic 0:fbdae7e6d805 457 omega_p_z = omega_p_z;
borlanic 0:fbdae7e6d805 458 Pk_z11 = Q11 + A11*(A11*Pk_z11 + A12*Pk_z21) + A12*(A11*Pk_z12 + A12*Pk_z22);
borlanic 0:fbdae7e6d805 459 Pk_z12 = A21*(A11*Pk_z11 + A12*Pk_z21) + A22*(A11*Pk_z12 + A12*Pk_z22);
borlanic 0:fbdae7e6d805 460 Pk_z21 = A11*(A21*Pk_z11 + A22*Pk_z21) + A12*(A21*Pk_z12 + A22*Pk_z22);
borlanic 0:fbdae7e6d805 461 Pk_z22 = Q22 + A21*(A21*Pk_z11 + A22*Pk_z21) + A22*(A21*Pk_z12 + A22*Pk_z22);
borlanic 0:fbdae7e6d805 462
borlanic 0:fbdae7e6d805 463 // Correction
borlanic 0:fbdae7e6d805 464 // x
borlanic 0:fbdae7e6d805 465 float Kk_x11 = (Pk_x11*(Pk_x22 + R22))/(Pk_x11*R22 + Pk_x22*R11 + R11*R22 + Pk_x11*Pk_x22 - Pk_x12*Pk_x21) - (Pk_x12*Pk_x21)/(Pk_x11*R22 + Pk_x22*R11 + R11*R22 + Pk_x11*Pk_x22 - Pk_x12*Pk_x21);
borlanic 0:fbdae7e6d805 466 float Kk_x12 = (Pk_x12*(Pk_x11 + R11))/(Pk_x11*R22 + Pk_x22*R11 + R11*R22 + Pk_x11*Pk_x22 - Pk_x12*Pk_x21) - (Pk_x11*Pk_x12)/(Pk_x11*R22 + Pk_x22*R11 + R11*R22 + Pk_x11*Pk_x22 - Pk_x12*Pk_x21);
borlanic 0:fbdae7e6d805 467 float Kk_x21 = (Pk_x21*(Pk_x22 + R22))/(Pk_x11*R22 + Pk_x22*R11 + R11*R22 + Pk_x11*Pk_x22 - Pk_x12*Pk_x21) - (Pk_x21*Pk_x22)/(Pk_x11*R22 + Pk_x22*R11 + R11*R22 + Pk_x11*Pk_x22 - Pk_x12*Pk_x21);
borlanic 0:fbdae7e6d805 468 float Kk_x22 = (Pk_x22*(Pk_x11 + R11))/(Pk_x11*R22 + Pk_x22*R11 + R11*R22 + Pk_x11*Pk_x22 - Pk_x12*Pk_x21) - (Pk_x12*Pk_x21)/(Pk_x11*R22 + Pk_x22*R11 + R11*R22 + Pk_x11*Pk_x22 - Pk_x12*Pk_x21);
borlanic 0:fbdae7e6d805 469 alpha_korr_x = alpha_p_x + Kk_x11*(alpha_x-alpha_p_x) + Kk_x12*(omega_x - omega_p_x);
borlanic 0:fbdae7e6d805 470 omega_korr_x = omega_p_x + Kk_x21*(alpha_x-alpha_p_x) + Kk_x22*(omega_x-omega_p_x);
borlanic 0:fbdae7e6d805 471
borlanic 0:fbdae7e6d805 472 // y
borlanic 0:fbdae7e6d805 473 float Kk_y11 = (Pk_y11*(Pk_y22 + R22))/(Pk_y11*R22 + Pk_y22*R11 + R11*R22 + Pk_y11*Pk_y22 - Pk_y12*Pk_y21) - (Pk_y12*Pk_y21)/(Pk_y11*R22 + Pk_y22*R11 + R11*R22 + Pk_y11*Pk_y22 - Pk_y12*Pk_y21);
borlanic 0:fbdae7e6d805 474 float Kk_y12 = (Pk_y12*(Pk_y11 + R11))/(Pk_y11*R22 + Pk_y22*R11 + R11*R22 + Pk_y11*Pk_y22 - Pk_y12*Pk_y21) - (Pk_y11*Pk_y12)/(Pk_y11*R22 + Pk_y22*R11 + R11*R22 + Pk_y11*Pk_y22 - Pk_y12*Pk_y21);
borlanic 0:fbdae7e6d805 475 float Kk_y21 = (Pk_y21*(Pk_y22 + R22))/(Pk_y11*R22 + Pk_y22*R11 + R11*R22 + Pk_y11*Pk_y22 - Pk_y12*Pk_y21) - (Pk_y21*Pk_y22)/(Pk_y11*R22 + Pk_y22*R11 + R11*R22 + Pk_y11*Pk_y22 - Pk_y12*Pk_y21);
borlanic 0:fbdae7e6d805 476 float Kk_y22 = (Pk_y22*(Pk_y11 + R11))/(Pk_y11*R22 + Pk_y22*R11 + R11*R22 + Pk_y11*Pk_y22 - Pk_y12*Pk_y21) - (Pk_y12*Pk_y21)/(Pk_y11*R22 + Pk_y22*R11 + R11*R22 + Pk_y11*Pk_y22 - Pk_y12*Pk_y21);
borlanic 0:fbdae7e6d805 477 alpha_korr_y = alpha_p_y + Kk_y11*(alpha_y-alpha_p_y) + Kk_y12*(omega_y - omega_p_y);
borlanic 0:fbdae7e6d805 478 omega_korr_y = omega_p_y + Kk_y21*(alpha_y-alpha_p_y) + Kk_y22*(omega_y-omega_p_y);
borlanic 0:fbdae7e6d805 479
borlanic 0:fbdae7e6d805 480 // z
borlanic 0:fbdae7e6d805 481 float Kk_z11 = (Pk_z11*(Pk_z22 + R22))/(Pk_z11*R22 + Pk_z22*R11 + R11*R22 + Pk_z11*Pk_z22 - Pk_z12*Pk_z21) - (Pk_z12*Pk_z21)/(Pk_z11*R22 + Pk_z22*R11 + R11*R22 + Pk_z11*Pk_y22 - Pk_z12*Pk_z21);
borlanic 0:fbdae7e6d805 482 float Kk_z12 = (Pk_z12*(Pk_z11 + R11))/(Pk_z11*R22 + Pk_z22*R11 + R11*R22 + Pk_z11*Pk_z22 - Pk_z12*Pk_z21) - (Pk_z11*Pk_z12)/(Pk_z11*R22 + Pk_z22*R11 + R11*R22 + Pk_z11*Pk_y22 - Pk_z12*Pk_z21);
borlanic 0:fbdae7e6d805 483 float Kk_z21 = (Pk_z21*(Pk_z22 + R22))/(Pk_z11*R22 + Pk_z22*R11 + R11*R22 + Pk_z11*Pk_z22 - Pk_z12*Pk_z21) - (Pk_z21*Pk_z22)/(Pk_z11*R22 + Pk_z22*R11 + R11*R22 + Pk_z11*Pk_z22 - Pk_z12*Pk_z21);
borlanic 0:fbdae7e6d805 484 float Kk_z22 = (Pk_z22*(Pk_z11 + R11))/(Pk_z11*R22 + Pk_z22*R11 + R11*R22 + Pk_z11*Pk_z22 - Pk_z12*Pk_z21) - (Pk_z12*Pk_z21)/(Pk_z11*R22 + Pk_z22*R11 + R11*R22 + Pk_z11*Pk_z22 - Pk_z12*Pk_z21);
borlanic 0:fbdae7e6d805 485 alpha_korr_z = alpha_p_z + Kk_z11*(alpha_z-alpha_p_z) + Kk_z12*(omega_z - omega_p_z);
borlanic 0:fbdae7e6d805 486 omega_korr_z = omega_p_z + Kk_z21*(alpha_z-alpha_p_z) + Kk_z22*(omega_z-omega_p_z);
borlanic 0:fbdae7e6d805 487
borlanic 0:fbdae7e6d805 488 // rot in z simple integration
borlanic 0:fbdae7e6d805 489 gamma_z_int = gz*0.05f + gamma_z_int;
borlanic 0:fbdae7e6d805 490 float f = t*t*(0.000000014370490f)+t*(-0.0012f) + 0.03f;
borlanic 0:fbdae7e6d805 491 t++;
borlanic 0:fbdae7e6d805 492
borlanic 0:fbdae7e6d805 493 //printf("%.7f %.7f\r\n",mx,my);
borlanic 0:fbdae7e6d805 494
borlanic 0:fbdae7e6d805 495 //Complementary filter
borlanic 0:fbdae7e6d805 496 //--------------------
borlanic 0:fbdae7e6d805 497 float gammaY_compl = atan2(-FilterACCx(ax), FilterACCz(az)) - FilterGYRY(gy);
borlanic 0:fbdae7e6d805 498 float gammaX_compl = atan2(-FilterACCy(ay), FilterACCz(az)) + FilterGYRX(gx);
borlanic 0:fbdae7e6d805 499
borlanic 0:fbdae7e6d805 500
borlanic 0:fbdae7e6d805 501
borlanic 0:fbdae7e6d805 502 //intf("%.7f %.7f\r\n",gammaY_compl,gammaX_compl);
borlanic 0:fbdae7e6d805 503 this->gammaX = gammaX_compl; //gammaXFilter.filter(alpha_korr_x);
borlanic 0:fbdae7e6d805 504 this->gammaY = gammaY_compl; //gammaYFilter.filter(alpha_korr_y);
borlanic 0:fbdae7e6d805 505 this->gammaZ = gamma_z_int-f;
borlanic 0:fbdae7e6d805 506 this->d_gammaX = gx;
borlanic 0:fbdae7e6d805 507 this->d_gammaY = -gy;
borlanic 0:fbdae7e6d805 508 this->d_gammaZ = gz;
borlanic 0:fbdae7e6d805 509
borlanic 0:fbdae7e6d805 510
borlanic 0:fbdae7e6d805 511 if(t<20001) {
borlanic 0:fbdae7e6d805 512 if (t % 10 == 0) {
borlanic 0:fbdae7e6d805 513 *(T+a) = t;
borlanic 0:fbdae7e6d805 514 *(Mes1+a) = gammaX_compl;//alpha_korr_x; //M1_AOUT1.read()*3.3f*4000.0f/3.0f - 2000.0f;
borlanic 0:fbdae7e6d805 515 *(Mes2+a) = gammaY_compl;//gammaXFilter.filter(alpha_korr_x);
borlanic 0:fbdae7e6d805 516 *(Mes3+a) = gammaY_compl;//medX;
borlanic 0:fbdae7e6d805 517 *(Mes4+a) = ax;//d_gammaX;
borlanic 0:fbdae7e6d805 518 *(Mes5+a) = az;//d_gammaY;
borlanic 0:fbdae7e6d805 519 *(Mes6+a) = gy;//d_gammaZ;
borlanic 0:fbdae7e6d805 520 a++;
borlanic 0:fbdae7e6d805 521 }
borlanic 0:fbdae7e6d805 522 }
borlanic 0:fbdae7e6d805 523
borlanic 0:fbdae7e6d805 524
borlanic 0:fbdae7e6d805 525
borlanic 0:fbdae7e6d805 526 //printf("%.2f %.2f %.2f %.2f %.2f %.2f\r\n",gammaX,gammaY,gammaZ,d_gammaX,d_gammaY,d_gammaZ);
borlanic 0:fbdae7e6d805 527 }
borlanic 0:fbdae7e6d805 528 }
borlanic 0:fbdae7e6d805 529
borlanic 0:fbdae7e6d805 530