File content as of revision 0:1a79273bc3e6:

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

#include <cmath>
#include "IMU.h"

using namespace std;

const float IMU::PI = 3.14159265f;                  // the constant PI


/**
 * Creates an IMU object.
 * @param spi a reference to an spi controller to use.
 * @param csG the chip select output for the gyro sensor.
 * @param csXM the chip select output for the accelerometer and the magnetometer.
 */
IMU::IMU(SPI& spi, DigitalOut& csG, DigitalOut& csXM) : spi(spi), csG(csG), csXM(csXM) {
    
    // initialize SPI interface
    
    lowMagX = 1000.0f;
    maxMagX = -1000.0f;
    lowMagY = 1000.0f;
    maxMagY = -1000.0f;
    
    spi.format(8, 3);
    spi.frequency(1000000);
    
    // reset chip select lines to logical high
    
    csG = 1;
    csXM = 1;
    
    // initialize gyro
    
    writeRegister(csG, CTRL_REG1_G, 0x0F);  // enable gyro in all 3 axis
    
    // initialize accelerometer
    
    writeRegister(csXM, CTRL_REG0_XM, 0x00);
    writeRegister(csXM, CTRL_REG1_XM, 0x5F);
    writeRegister(csXM, CTRL_REG2_XM, 0x00);
    writeRegister(csXM, CTRL_REG3_XM, 0x04);
    
    // initialize magnetometer
    
    writeRegister(csXM, CTRL_REG5_XM, 0x94);
    writeRegister(csXM, CTRL_REG6_XM, 0x00);
    writeRegister(csXM, CTRL_REG7_XM, 0x00);
    writeRegister(csXM, CTRL_REG4_XM, 0x04);
    writeRegister(csXM, INT_CTRL_REG_M, 0x09);
}

/**
 * Deletes the IMU object.
 */
IMU::~IMU() {}

/**
 * This private method allows to write a register value.
 * @param cs the chip select output to use, either csG or csXM.
 * @param address the 7 bit address of the register.
 * @param value the value to write into the register.
 */
void IMU::writeRegister(DigitalOut& cs, char address, char 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 csG or csXM.
 * @param address the 7 bit address of the register.
 * @return the value read from the register.
 */
char IMU::readRegister(DigitalOut& cs, char address) {
    
    cs = 0;
    
    spi.write(0x80 | address);
    int value = spi.write(0xFF);
    
    cs = 1;
    
    return (char)(value & 0xFF);
}

/**
 * Reads the gyroscope about the x-axis.
 * @return the rotational speed about the x-axis given in [rad/s].
 */
float IMU::readGyroX() {
    
    char low = readRegister(csG, OUT_X_L_G);
    char high = readRegister(csG, OUT_X_H_G);
    
    short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
    
    return (float)value/32768.0f*245.0f*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() {

    char low = readRegister(csG, OUT_Y_L_G);
    char high = readRegister(csG, OUT_Y_H_G);
    
    short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
    
    return (float)value/32768.0f*245.0f*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() {
    
    char low = readRegister(csG, OUT_Z_L_G);
    char high = readRegister(csG, OUT_Z_H_G);
    
    short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
    
    return (float)value/32768.0f*245.0f*PI/180.0f;
}

/**
 * Reads the acceleration in x-direction.
 * @return the acceleration in x-direction, given in [m/s2].
 */
float IMU::readAccelerationX() {
    
    char low = readRegister(csXM, OUT_X_L_A);
    char high = readRegister(csXM, OUT_X_H_A);
    
    short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
    
    return (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() {
    
    char low = readRegister(csXM, OUT_Y_L_A);
    char high = readRegister(csXM, OUT_Y_H_A);
    
    short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
    
    return (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() {
    
    char low = readRegister(csXM, OUT_Z_L_A);
    char high = readRegister(csXM, OUT_Z_H_A);
    
    short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
    
    return (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() {
    
    char low = readRegister(csXM, OUT_X_L_M);
    char high = readRegister(csXM, OUT_X_H_M);
    
    short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
    
    return (float)value/32768.0f*2.0f;
}

/**
 * Reads the magnetic field in x-direction.
 * @return the magnetic field in x-direction, given in [Gauss].
 */
float IMU::readMagnetometerY() {
    
    char low = readRegister(csXM, OUT_Y_L_M);
    char high = readRegister(csXM, OUT_Y_H_M);
    
    short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
    
    return (float)value/32768.0f*2.0f;
}

/**
 * Reads the magnetic field in x-direction.
 * @return the magnetic field in x-direction, given in [Gauss].
 */
float IMU::readMagnetometerZ() {
    
    char low = readRegister(csXM, OUT_Z_L_M);
    char high = readRegister(csXM, OUT_Z_H_M);
    
    short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
    
    return (float)value/32768.0f*2.0f;
}

/**
 * Reads the compass heading about the z-axis.
 * @return the compass heading in the range -PI to +PI, given in [rad].
 */
float IMU::readHeading() {
    float magX = readMagnetometerX();
    float magY = readMagnetometerY();
    
    // X-Achse
    if (magX < lowMagX) lowMagX = magX;    
    if (magX > maxMagX) maxMagX = magX;
    
    float magX2 = (magX-lowMagX)/(maxMagX-lowMagX)-0.5f;
    
    //printf ("minX in Gauss = %f\n\r",lowMagX);
    //printf ("maxX in Gauss = %f\n\r",maxMagX);
    //printf ("magX2 in Gauss = %f\n\r",magX2);
    
    // Y-Achse
    if (magY < lowMagY) lowMagY = magY;    
    if (magY > maxMagY) maxMagY = magY;
    
    float magY2 = (magY-lowMagY)/(maxMagY-lowMagY)-0.5f;
    
   
   // printf ("minY in Gauss = %f\n\r",lowMagY);
   // printf ("maxY in Gauss = %f\n\r",maxMagY);
   // printf ("magY2 in Gauss = %f\n\r",magY2);
    
    return atan2(magY2,magX2);
}