File content as of revision 0:380207fcb5c1:

/*
 * IMU.cpp
 * Copyright (c) 2018, ZHAW
 * All righSAMPLE_TIME reserved.
 */

#include "IMU.h"
#include "mbed.h"


using namespace std;

const float IMU::M_PI = 3.14159265358979323846f;    // the mathematical constant PI

// Nick ====================================
const float IMU::SAMPLE_TIME = 0.01f;
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) {
    
    // 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
    
    thread.start(callback(this, &IMU::kalman));
}

/**
 * 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::kalman(){
        
    // Messrauschen sensor standardabweichung gx - R
    float R11 = STD_ALPHA*STD_ALPHA;
    float R22 = STD_OMEGA*STD_OMEGA;
    
    // Messrauschen prozessor - Q
    float Q11 = 0.001f;
    float Q22 = 0.001f;
    
    // 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.0f;
    float Pk_x12 = 0.0f;
    float Pk_x21 = 0.0f;
    float Pk_x22 = 0.0f;
    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.0f;
    float Pk_y12 = 0.0f;
    float Pk_y21 = 0.0f;
    float Pk_y22 = 0.0f;
    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.0f;
    float Pk_z12 = 0.0f;
    float Pk_z21 = 0.0f;
    float Pk_z22 = 0.0f;
    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();
    
    while (true) {
        /*Kalman Filter--------------------------------------------------*/

        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);
        
        this->gammaX = alpha_korr_x;
        this->gammaY = alpha_korr_y;
        this->gammaZ = alpha_korr_z;
        this->d_gammaX = omega_korr_x;
        this->d_gammaY = omega_korr_y;
        this->d_gammaZ = omega_korr_z;
        
        thread.wait(50);
        }
}