BA / Mbed OS BaBoRo1

Erste version der Software für der Prototyp

IMU.cpp@4:75df35ef4fb6, 2018-03-30 (annotated)

Committer:
borlanic
Date:
Fri Mar 30 14:07:05 2018 +0000
Revision:
4:75df35ef4fb6
Parent:
0:380207fcb5c1 
commentar

Who changed what in which revision?

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