File content as of revision 3:6fe17b8a6d62:

/*
 * Main.cpp
 * Copyright (c) 2018, ZHAW
 * All rights reserved.
 */

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



using namespace std;

const float IMU::M_PI = 3.14159265358979323846f;    // the mathematical constant PI
const float IMU::SAMPLE_TIME = 0.001f;
const float IMU::STD_ALPHA = 0.02f; // standard deviation Measurement noise sensor gx - R
const float IMU::STD_OMEGA = 0.034f; // standard deviation Measurement noise sensor 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(100000); //90000000

    // reset chip select lines to logical high

    csAG = 1;
    csM = 1;

    // initialize accelerometer and gyro

    writeRegister(csAG, CTRL_REG1_G, 0xCB);     // 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

    gainAx = 0.9921997f;
    gainAy = 1.0f;
    gainAz = 1.0f;
    gainGx = 2.391f; //2.4648f
    gainGy = 2.3896f;
    gainGz = 2.4f;

    offsGx = -0.1162f; //-0.0057f
    offsGy = -0.0374f;
    offsGz = -0.0087f;
    offsAx = 0.0925285f;
    offsAy = 0.0f;
    offsAz = 0.0f;
}

/**
 * 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*gainGx + offsGx; // --> Fullscale +-500 (bevor: +-245) + scale factor: 2.4648
}

/**
 * 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*gainGy + offsGy; // --> Fullscale +-500 (bevor: +-245) + scale factor: 2.3896
}

/**
 * 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*gainGz + offsGz;
}

/**
 * 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*gainAx + offsAx; // added scale factor 0.9911414 and offset -0.3793863
}

/**
 * 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*gainAy + offsAy; // added scale factor 1.0011989 and offset -0.0116007
}

/**
 * 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*gainAz + offsAz; // added scale factor 0.9921997 and offset -0.0925285f
}

/**
 * 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;
}