BA / Mbed OS BaBoRo_test2

Backup 1

IMU.cpp@0:02dd72d1d465, 2018-04-24 (annotated)

Committer:
borlanic
Date:
Tue Apr 24 11:45:18 2018 +0000
Revision:
0:02dd72d1d465
BaBoRo_test2 - backup 1

Who changed what in which revision?

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