Diff: IMU.cpp

Revision:

0:351a2fb21235

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.cpp	Fri May 18 12:05:32 2018 +0000
@@ -0,0 +1,276 @@
+/*
+ * IMU.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "IMU.h"
+
+using namespace std;
+
+const float IMU::PERIOD = 0.001f;                   // period of filter task, given in [s]
+const float IMU::PI = 3.14159265f;                  // the constant PI
+const float IMU::LOWPASS_FILTER_FREQUENCY = 6.3f;   // frequency of lowpass filter, given in [rad/s]
+
+/**
+ * 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
+    
+    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);
+    
+    // initialize local variables
+    
+    magnetometerXMin = 1000.0f;
+    magnetometerXMax = -1000.0f;
+    magnetometerYMin = 1000.0f;
+    magnetometerYMax = -1000.0f;
+    
+    magnetometerXFilter.setPeriod(PERIOD);
+    magnetometerXFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+    magnetometerXFilter.reset(readMagnetometerX());
+    
+    magnetometerYFilter.setPeriod(PERIOD);
+    magnetometerYFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+    magnetometerYFilter.reset(readMagnetometerY());
+    
+    heading = 0.0f;
+    
+    // start periodic task
+    
+    ticker.attach(callback(this, &IMU::run), PERIOD);
+}
+
+/**
+ * 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() {
+    
+    return heading;
+}
+
+/**
+ * This method is called periodically by the ticker object and contains the
+ * calculation and filtering of the heading information.
+ */
+void IMU::run() {
+    
+    // read actual measurements from magnetometer registers
+    
+    float magnetometerX = magnetometerXFilter.filter(readMagnetometerX());
+    float magnetometerY = magnetometerYFilter.filter(readMagnetometerY());
+    
+    // adjust the minimum and maximum limits, if needed
+    
+    if (magnetometerXMin > magnetometerX) magnetometerXMin = magnetometerX;
+    if (magnetometerXMax < magnetometerX) magnetometerXMax = magnetometerX;
+    if (magnetometerYMin > magnetometerY) magnetometerYMin = magnetometerY;
+    if (magnetometerYMax < magnetometerY) magnetometerYMax = magnetometerY;
+    
+    // calculate adjusted magnetometer values (gain and offset compensation)
+    
+    if (magnetometerXMin < magnetometerXMax) magnetometerX = (magnetometerX-magnetometerXMin)/(magnetometerXMax-magnetometerXMin)-0.5f;
+    if (magnetometerYMin < magnetometerYMax) magnetometerY = (magnetometerY-magnetometerYMin)/(magnetometerYMax-magnetometerYMin)-0.5f;
+    
+    // calculate heading with atan2 from x and y magnetometer measurements
+    
+    heading = atan2(magnetometerX, magnetometerY);
+}
+