Nicolas Borla
BBR 1 Ebene
Diff: IMU.cpp
- Revision:
- 0:fbdae7e6d805
diff -r 000000000000 -r fbdae7e6d805 IMU.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.cpp Mon May 14 11:29:06 2018 +0000
@@ -0,0 +1,530 @@
+/*
+ * IMU.cpp
+ * Copyright (c) 2018, ZHAW
+ * All righSAMPLE_TIME reserved.
+ */
+
+#include "IMU.h"
+#include "mbed.h"
+#include "IIR_filter.h"
+
+
+
+using namespace std;
+
+const float IMU::M_PI = 3.14159265358979323846f; // the mathematical constant PI
+
+// Nick ====================================
+const float IMU::SAMPLE_TIME = 0.001f;
+const float IMU::STD_ALPHA = 0.02f; // Messrauschen sensor standardabweichung gx - R
+const float IMU::STD_OMEGA = 0.034f; // Messrauschen sensor standardabweichung gx - R
+//==========================================
+
+/**
+ * Creates an IMU object.
+ * @param spi a reference to an spi controller to use.
+ * @param csAG the chip select output for the accelerometer and the gyro sensor.
+ * @param csM the chip select output for the magnetometer.
+ */
+IMU::IMU(SPI& spi, DigitalOut& csAG, DigitalOut& csM) : spi(spi), csAG(csAG), csM(csM), thread(osPriorityHigh, STACK_SIZE)
+{
+
+ // initialize SPI interface
+
+ spi.format(8, 3);
+ spi.frequency(1000000);
+
+ // reset chip select lines to logical high
+
+ csAG = 1;
+ csM = 1;
+
+ // initialize accelerometer and gyro
+
+ writeRegister(csAG, CTRL_REG1_G, 0xC3); // ODR 952 Hz, full scale 245 deg/s
+ writeRegister(csAG, CTRL_REG2_G, 0x00); // disable interrupt generation
+ writeRegister(csAG, CTRL_REG3_G, 0x00); // disable low power mode, disable high pass filter, high pass cutoff frequency 57 Hz
+ writeRegister(csAG, CTRL_REG4, 0x38); // enable gyro in all 3 axis
+ writeRegister(csAG, CTRL_REG5_XL, 0x38); // no decimation, enable accelerometer in all 3 axis
+ writeRegister(csAG, CTRL_REG6_XL, 0xC0); // ODR 952 Hz, full scale 2g
+ writeRegister(csAG, CTRL_REG7_XL, 0x00); // high res mode disabled, filter bypassed
+ writeRegister(csAG, CTRL_REG8, 0x00); // 4-wire SPI interface, LSB at lower address
+ writeRegister(csAG, CTRL_REG9, 0x04); // disable gyro sleep mode, disable I2C interface, disable FIFO
+ writeRegister(csAG, CTRL_REG10, 0x00); // self test disabled
+
+ // initialize magnetometer
+
+ writeRegister(csM, CTRL_REG1_M, 0x10); // temperature not compensated, low power mode for x & y axis, data rate 10 Hz
+ writeRegister(csM, CTRL_REG2_M, 0x00); // full scale 4 gauss
+ writeRegister(csM, CTRL_REG3_M, 0x80); // disable I2C interface, low power mode, SPI write only, continuous conversion mode
+ writeRegister(csM, CTRL_REG4_M, 0x00); // low power mode for z axis, LSB at lower address
+ writeRegister(csM, CTRL_REG5_M, 0x00); // fast read disabled
+
+ gammaXFilter.setPeriod(SAMPLE_TIME);
+ gammaXFilter.setFrequency(300.0f);
+ gammaYFilter.setPeriod(SAMPLE_TIME);
+ gammaYFilter.setFrequency(300.0f);
+ d_gammaXFilter.setPeriod(SAMPLE_TIME);
+ d_gammaXFilter.setFrequency(300.0f);
+ d_gammaYFilter.setPeriod(SAMPLE_TIME);
+ d_gammaYFilter.setFrequency(300.0f);
+
+ thread.start(callback(this, &IMU::kalman));
+ ticker.attach(callback(this, &IMU::sendSignal), SAMPLE_TIME);
+}
+
+/**
+ * Deletes the IMU object.
+ */
+IMU::~IMU() {}
+
+/**
+ * This private method allows to write a register value.
+ * @param cs the chip select output to use, either csAG or csM.
+ * @param address the 7 bit address of the register.
+ * @param value the value to write into the register.
+ */
+void IMU::writeRegister(DigitalOut& cs, uint8_t address, uint8_t value)
+{
+
+ cs = 0;
+
+ spi.write(0x7F & address);
+ spi.write(value & 0xFF);
+
+ cs = 1;
+}
+
+/**
+ * This private method allows to read a register value.
+ * @param cs the chip select output to use, either csAG or csM.
+ * @param address the 7 bit address of the register.
+ * @return the value read from the register.
+ */
+uint8_t IMU::readRegister(DigitalOut& cs, uint8_t address)
+{
+
+ cs = 0;
+
+ spi.write(0x80 | address);
+ int32_t value = spi.write(0xFF);
+
+ cs = 1;
+
+ return static_cast<uint8_t>(value & 0xFF);
+}
+
+/**
+ * Reads the gyroscope about the x-axis.
+ * @return the rotational speed about the x-axis given in [rad/s].
+ */
+float IMU::readGyroX()
+{
+
+ uint8_t low = readRegister(csAG, OUT_X_L_G);
+ uint8_t high = readRegister(csAG, OUT_X_H_G);
+
+ int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
+
+ return static_cast<float>(value)/32768.0f*245.0f*M_PI/180.0f;
+}
+
+/**
+ * Reads the gyroscope about the y-axis.
+ * @return the rotational speed about the y-axis given in [rad/s].
+ */
+float IMU::readGyroY()
+{
+
+ uint8_t low = readRegister(csAG, OUT_Y_L_G);
+ uint8_t high = readRegister(csAG, OUT_Y_H_G);
+
+ int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
+
+ return static_cast<float>(value)/32768.0f*245.0f*M_PI/180.0f;
+}
+
+/**
+ * Reads the gyroscope about the z-axis.
+ * @return the rotational speed about the z-axis given in [rad/s].
+ */
+float IMU::readGyroZ()
+{
+
+ uint8_t low = readRegister(csAG, OUT_Z_L_G);
+ uint8_t high = readRegister(csAG, OUT_Z_H_G);
+
+ int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
+
+ return static_cast<float>(value)/32768.0f*245.0f*M_PI/180.0f;
+}
+
+/**
+ * Reads the acceleration in x-direction.
+ * @return the acceleration in x-direction, given in [m/s2].
+ */
+float IMU::readAccelerationX()
+{
+
+ uint8_t low = readRegister(csAG, OUT_X_L_XL);
+ uint8_t high = readRegister(csAG, OUT_X_H_XL);
+
+ int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
+
+ return static_cast<float>(value)/32768.0f*2.0f*9.81f;
+}
+
+/**
+ * Reads the acceleration in y-direction.
+ * @return the acceleration in y-direction, given in [m/s2].
+ */
+float IMU::readAccelerationY()
+{
+
+ uint8_t low = readRegister(csAG, OUT_Y_L_XL);
+ uint8_t high = readRegister(csAG, OUT_Y_H_XL);
+
+ int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
+
+ return static_cast<float>(value)/32768.0f*2.0f*9.81f;
+}
+
+/**
+ * Reads the acceleration in z-direction.
+ * @return the acceleration in z-direction, given in [m/s2].
+ */
+float IMU::readAccelerationZ()
+{
+
+ uint8_t low = readRegister(csAG, OUT_Z_L_XL);
+ uint8_t high = readRegister(csAG, OUT_Z_H_XL);
+
+ int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
+
+ return static_cast<float>(value)/32768.0f*2.0f*9.81f;
+}
+
+/**
+ * Reads the magnetic field in x-direction.
+ * @return the magnetic field in x-direction, given in [Gauss].
+ */
+float IMU::readMagnetometerX()
+{
+
+ uint8_t low = readRegister(csM, OUT_X_L_M);
+ uint8_t high = readRegister(csM, OUT_X_H_M);
+
+ int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
+
+ return static_cast<float>(value)/32768.0f*4.0f;
+}
+
+/**
+ * Reads the magnetic field in x-direction.
+ * @return the magnetic field in x-direction, given in [Gauss].
+ */
+float IMU::readMagnetometerY()
+{
+
+ uint8_t low = readRegister(csM, OUT_Y_L_M);
+ uint8_t high = readRegister(csM, OUT_Y_H_M);
+
+ int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
+
+ return static_cast<float>(value)/32768.0f*4.0f;
+}
+
+/**
+ * Reads the magnetic field in x-direction.
+ * @return the magnetic field in x-direction, given in [Gauss].
+ */
+float IMU::readMagnetometerZ()
+{
+
+ uint8_t low = readRegister(csM, OUT_Z_L_M);
+ uint8_t high = readRegister(csM, OUT_Z_H_M);
+
+ int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
+
+ return static_cast<float>(value)/32768.0f*4.0f;
+}
+
+float IMU::getGammaX()
+{
+ return gammaX;
+}
+
+float IMU::getGammaY()
+{
+ return gammaY;
+}
+
+float IMU::getGammaZ()
+{
+ return gammaZ;
+}
+
+float IMU::getDGammaX()
+{
+ return d_gammaX;
+}
+
+float IMU::getDGammaY()
+{
+ return d_gammaY;
+}
+
+float IMU::getDGammaZ()
+{
+ return d_gammaZ;
+}
+
+void IMU::sendSignal() {
+
+ thread.signal_set(signal);
+}
+
+void IMU::kalman()
+{
+
+ Serial pc1(USBTX, USBRX); // tx, rx
+ pc1.baud(100000);
+
+ // Messrauschen sensor standardabweichung gx - R
+ float R11 = STD_ALPHA*STD_ALPHA;
+ float R22 = STD_OMEGA*STD_OMEGA;
+
+ // Messrauschen prozessor - Q
+ float Q11 = 0.0000001f;
+ float Q22 = 0.0001f;
+
+ // Matrix A
+ float A11 = 1.0f;
+ float A12 = SAMPLE_TIME;
+ float A21 = 0.0f;
+ float A22 = 1.0f;
+
+ // rot X
+ float alpha_p_x = 0.0f;
+ float omega_p_x = 0.0f;
+ float Pk_x11 = 0.001f;
+ float Pk_x12 = 0.0f;
+ float Pk_x21 = 0.0f;
+ float Pk_x22 = 0.001f;
+ float alpha_korr_x = 0.0f;
+ float omega_korr_x = 0.0f;
+
+ // rot Y
+ float alpha_p_y = 0.0f;
+ float omega_p_y = 0.0f;
+ float Pk_y11 = 0.001f;
+ float Pk_y12 = 0.0f;
+ float Pk_y21 = 0.0f;
+ float Pk_y22 = 0.001f;
+ float alpha_korr_y = 0.0f;
+ float omega_korr_y = 0.0f;
+
+ // rot Z
+ float alpha_p_z = 0.0f;
+ float omega_p_z = 0.0f;
+ float Pk_z11 = 0.001f;
+ float Pk_z12 = 0.0f;
+ float Pk_z21 = 0.0f;
+ float Pk_z22 = 0.001f;
+ float alpha_korr_z = 0.0f;
+ float omega_korr_z = 0.0f;
+
+ double mx_f_vor=this->readMagnetometerX();
+ double mx_vor=this->readMagnetometerX();
+
+ double my_f_vor=this->readMagnetometerY();
+ double my_vor=this->readMagnetometerY();
+
+ int t = 0;
+ float gamma_z_int = 0;
+
+ // messung
+
+ int * T = new int[2000];
+ float * Mes1 = new float[2000];
+ float * Mes2 = new float[2000];
+ float * Mes3 = new float[2000];
+ float * Mes4 = new float[2000];
+ float * Mes5 = new float[2000];
+ float * Mes6 = new float[2000];
+
+
+ int a = 0;
+
+ // initialise Lowpass filters for compl.fil.
+ float tau = 1.0;
+ IIR_filter FilterACCx(tau, SAMPLE_TIME, 1.0f); // 1st order LP for complementary filter acc_x
+ IIR_filter FilterACCz(tau, SAMPLE_TIME, 1.0f); // 1st order LP for complementary filter acc_y
+ IIR_filter FilterACCy(tau, SAMPLE_TIME, 1.0f); // 1st order LP for complementary filter acc_y
+ IIR_filter FilterGYRY(tau, SAMPLE_TIME, tau); // 1st order LP for complementary filter gyro
+ IIR_filter FilterGYRX(tau, SAMPLE_TIME, tau); // 1st order LP for complementary filter gyro
+
+ while (true) {
+ /*Kalman Filter--------------------------------------------------*/
+
+ // wait for the periodic signal
+
+ thread.signal_wait(signal);
+ //pc1.printf("IMU\r\n");
+
+ /*
+ if(t==21000) {
+ pc1.printf("invio dati:\r\n\n");
+ for(int j=0; j<2000; j++) {
+ //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));
+ 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));
+ }
+ pc1.printf("fine dati:\r\n\n");
+ delete T;
+ delete Mes1;
+ delete Mes2;
+ delete Mes3;
+ delete Mes4;
+ delete Mes5;
+ delete Mes6;
+
+ }
+ */
+
+
+ //printf("IMU start\r\n");
+
+ float ax = this->readAccelerationX();
+ float ay = this->readAccelerationY();
+ float az = this->readAccelerationZ();
+
+ float gx = this->readGyroX();
+ float gy = this->readGyroY();
+ float gz = this->readGyroZ();
+
+ float mx = this->readMagnetometerX();
+ float my = this->readMagnetometerY();
+ float mz = this->readMagnetometerZ();
+
+ // LowPass Magnetometer
+ float RC = 1.0/(10*2*3.14); // Cutoff 10Hz
+ float dt = 1.0/SAMPLE_TIME;
+ float alpha = dt/(RC+dt);
+
+ float mx_f = mx_f_vor + (alpha*(mx-mx_vor));
+ float my_f = my_f_vor + (alpha*(my-my_vor));
+
+ mx_f_vor = mx_f;
+ mx_vor = mx;
+ my_f_vor = my_f;
+ my_vor = my;
+
+ // rot x
+ float alpha_x = atan2(-ay,az);
+ float omega_x = gx;
+
+ // rot y
+ float alpha_y = atan2(-ax,az);
+ float omega_y = -gy;
+
+ // rot z
+ float mx_fil = (mx_f+0.3614f)*11.5937f;
+ float my_fil = (my_f-0.4466f)*15.2002f;
+ float alpha_z = atan2(my_fil,mx_fil);// Sostituire con calcolo gamma encoder
+ float omega_z = gz;
+
+ /*
+ float alpha_z = 0.024f/(0.095f*3.0f*0.7071f)*(w1+w2+w3)
+ */
+
+ // Prediction
+ // x
+ alpha_p_x = alpha_p_x + SAMPLE_TIME*omega_x;
+ omega_p_x = omega_p_x;
+ Pk_x11 = Q11 + A11*(A11*Pk_x11 + A12*Pk_x21) + A12*(A11*Pk_x12 + A12*Pk_x22);
+ Pk_x12 = A21*(A11*Pk_x11 + A12*Pk_x21) + A22*(A11*Pk_x12 + A12*Pk_x22);
+ Pk_x21 = A11*(A21*Pk_x11 + A22*Pk_x21) + A12*(A21*Pk_x12 + A22*Pk_x22);
+ Pk_x22 = Q22 + A21*(A21*Pk_x11 + A22*Pk_x21) + A22*(A21*Pk_x12 + A22*Pk_x22);
+ // y
+ alpha_p_y = alpha_p_y + SAMPLE_TIME*omega_y;
+ omega_p_y = omega_p_y;
+ Pk_y11 = Q11 + A11*(A11*Pk_y11 + A12*Pk_y21) + A12*(A11*Pk_y12 + A12*Pk_y22);
+ Pk_y12 = A21*(A11*Pk_y11 + A12*Pk_y21) + A22*(A11*Pk_y12 + A12*Pk_y22);
+ Pk_y21 = A11*(A21*Pk_y11 + A22*Pk_y21) + A12*(A21*Pk_y12 + A22*Pk_y22);
+ Pk_y22 = Q22 + A21*(A21*Pk_y11 + A22*Pk_y21) + A22*(A21*Pk_y12 + A22*Pk_y22);
+ // z
+ alpha_p_z = alpha_p_z + SAMPLE_TIME*omega_z;
+ omega_p_z = omega_p_z;
+ Pk_z11 = Q11 + A11*(A11*Pk_z11 + A12*Pk_z21) + A12*(A11*Pk_z12 + A12*Pk_z22);
+ Pk_z12 = A21*(A11*Pk_z11 + A12*Pk_z21) + A22*(A11*Pk_z12 + A12*Pk_z22);
+ Pk_z21 = A11*(A21*Pk_z11 + A22*Pk_z21) + A12*(A21*Pk_z12 + A22*Pk_z22);
+ Pk_z22 = Q22 + A21*(A21*Pk_z11 + A22*Pk_z21) + A22*(A21*Pk_z12 + A22*Pk_z22);
+
+ // Correction
+ // x
+ 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);
+ 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);
+ 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);
+ 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);
+ alpha_korr_x = alpha_p_x + Kk_x11*(alpha_x-alpha_p_x) + Kk_x12*(omega_x - omega_p_x);
+ omega_korr_x = omega_p_x + Kk_x21*(alpha_x-alpha_p_x) + Kk_x22*(omega_x-omega_p_x);
+
+ // y
+ 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);
+ 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);
+ 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);
+ 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);
+ alpha_korr_y = alpha_p_y + Kk_y11*(alpha_y-alpha_p_y) + Kk_y12*(omega_y - omega_p_y);
+ omega_korr_y = omega_p_y + Kk_y21*(alpha_y-alpha_p_y) + Kk_y22*(omega_y-omega_p_y);
+
+ // z
+ 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);
+ 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);
+ 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);
+ 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);
+ alpha_korr_z = alpha_p_z + Kk_z11*(alpha_z-alpha_p_z) + Kk_z12*(omega_z - omega_p_z);
+ omega_korr_z = omega_p_z + Kk_z21*(alpha_z-alpha_p_z) + Kk_z22*(omega_z-omega_p_z);
+
+ // rot in z simple integration
+ gamma_z_int = gz*0.05f + gamma_z_int;
+ float f = t*t*(0.000000014370490f)+t*(-0.0012f) + 0.03f;
+ t++;
+
+ //printf("%.7f %.7f\r\n",mx,my);
+
+ //Complementary filter
+ //--------------------
+ float gammaY_compl = atan2(-FilterACCx(ax), FilterACCz(az)) - FilterGYRY(gy);
+ float gammaX_compl = atan2(-FilterACCy(ay), FilterACCz(az)) + FilterGYRX(gx);
+
+
+
+ //intf("%.7f %.7f\r\n",gammaY_compl,gammaX_compl);
+ this->gammaX = gammaX_compl; //gammaXFilter.filter(alpha_korr_x);
+ this->gammaY = gammaY_compl; //gammaYFilter.filter(alpha_korr_y);
+ this->gammaZ = gamma_z_int-f;
+ this->d_gammaX = gx;
+ this->d_gammaY = -gy;
+ this->d_gammaZ = gz;
+
+
+ if(t<20001) {
+ if (t % 10 == 0) {
+ *(T+a) = t;
+ *(Mes1+a) = gammaX_compl;//alpha_korr_x; //M1_AOUT1.read()*3.3f*4000.0f/3.0f - 2000.0f;
+ *(Mes2+a) = gammaY_compl;//gammaXFilter.filter(alpha_korr_x);
+ *(Mes3+a) = gammaY_compl;//medX;
+ *(Mes4+a) = ax;//d_gammaX;
+ *(Mes5+a) = az;//d_gammaY;
+ *(Mes6+a) = gy;//d_gammaZ;
+ a++;
+ }
+ }
+
+
+
+ //printf("%.2f %.2f %.2f %.2f %.2f %.2f\r\n",gammaX,gammaY,gammaZ,d_gammaX,d_gammaY,d_gammaZ);
+ }
+}
+
+