IMU.cpp

00001 /*
00002  * IMU.cpp
00003  * Copyright (c) 2018, ZHAW
00004  * All rights reserved.
00005  */
00006 
00007 #include <cmath>
00008 #include "IMU.h"
00009 
00010 using namespace std;
00011 
00012 const float IMU::PERIOD = 0.001f;                   // period of filter task, given in [s]
00013 const float IMU::PI = 3.14159265f;                  // the constant PI
00014 const float IMU::LOWPASS_FILTER_FREQUENCY = 6.3f;   // frequency of lowpass filter, given in [rad/s]
00015 
00016 /**
00017  * Creates an IMU object.
00018  * @param spi a reference to an spi controller to use.
00019  * @param csG the chip select output for the gyro sensor.
00020  * @param csXM the chip select output for the accelerometer and the magnetometer.
00021  */
00022 IMU::IMU(SPI& spi, DigitalOut& csG, DigitalOut& csXM) : spi(spi), csG(csG), csXM(csXM) {
00023     
00024     // initialize SPI interface
00025     
00026     spi.format(8, 3);
00027     spi.frequency(1000000);
00028     
00029     // reset chip select lines to logical high
00030     
00031     csG = 1;
00032     csXM = 1;
00033     
00034     // initialize gyro
00035     
00036     writeRegister(csG, CTRL_REG1_G, 0x0F);  // enable gyro in all 3 axis
00037     
00038     // initialize accelerometer
00039     
00040     writeRegister(csXM, CTRL_REG0_XM, 0x00);
00041     writeRegister(csXM, CTRL_REG1_XM, 0x5F);
00042     writeRegister(csXM, CTRL_REG2_XM, 0x00);
00043     writeRegister(csXM, CTRL_REG3_XM, 0x04);
00044     
00045     // initialize magnetometer
00046     
00047     writeRegister(csXM, CTRL_REG5_XM, 0x94);
00048     writeRegister(csXM, CTRL_REG6_XM, 0x00);
00049     writeRegister(csXM, CTRL_REG7_XM, 0x00);
00050     writeRegister(csXM, CTRL_REG4_XM, 0x04);
00051     writeRegister(csXM, INT_CTRL_REG_M, 0x09);
00052     
00053     // initialize local variables
00054     
00055     magnetometerXMin = 1000.0f;
00056     magnetometerXMax = -1000.0f;
00057     magnetometerYMin = 1000.0f;
00058     magnetometerYMax = -1000.0f;
00059     
00060     magnetometerXFilter.setPeriod(PERIOD);
00061     magnetometerXFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00062     magnetometerXFilter.reset(readMagnetometerX());
00063     
00064     magnetometerYFilter.setPeriod(PERIOD);
00065     magnetometerYFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00066     magnetometerYFilter.reset(readMagnetometerY());
00067     
00068     heading = 0.0f;
00069     
00070     // start periodic task
00071     
00072     ticker.attach(callback(this, &IMU::run), PERIOD);
00073 }
00074 
00075 /**
00076  * Deletes the IMU object.
00077  */
00078 IMU::~IMU() {}
00079 
00080 /**
00081  * This private method allows to write a register value.
00082  * @param cs the chip select output to use, either csG or csXM.
00083  * @param address the 7 bit address of the register.
00084  * @param value the value to write into the register.
00085  */
00086 void IMU::writeRegister(DigitalOut& cs, char address, char value) {
00087     
00088     cs = 0;
00089     
00090     spi.write(0x7F & address);
00091     spi.write(value & 0xFF);
00092     
00093     cs = 1;
00094 }
00095 
00096 /**
00097  * This private method allows to read a register value.
00098  * @param cs the chip select output to use, either csG or csXM.
00099  * @param address the 7 bit address of the register.
00100  * @return the value read from the register.
00101  */
00102 char IMU::readRegister(DigitalOut& cs, char address) {
00103     
00104     cs = 0;
00105     
00106     spi.write(0x80 | address);
00107     int value = spi.write(0xFF);
00108     
00109     cs = 1;
00110     
00111     return (char)(value & 0xFF);
00112 }
00113 
00114 /**
00115  * Reads the gyroscope about the x-axis.
00116  * @return the rotational speed about the x-axis given in [rad/s].
00117  */
00118 float IMU::readGyroX() {
00119     
00120     char low = readRegister(csG, OUT_X_L_G);
00121     char high = readRegister(csG, OUT_X_H_G);
00122     
00123     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00124     
00125     return (float)value/32768.0f*245.0f*PI/180.0f;
00126 }
00127 
00128 /**
00129  * Reads the gyroscope about the y-axis.
00130  * @return the rotational speed about the y-axis given in [rad/s].
00131  */
00132 float IMU::readGyroY() {
00133 
00134     char low = readRegister(csG, OUT_Y_L_G);
00135     char high = readRegister(csG, OUT_Y_H_G);
00136     
00137     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00138     
00139     return (float)value/32768.0f*245.0f*PI/180.0f;
00140 }
00141 
00142 /**
00143  * Reads the gyroscope about the z-axis.
00144  * @return the rotational speed about the z-axis given in [rad/s].
00145  */
00146 float IMU::readGyroZ() {
00147     
00148     char low = readRegister(csG, OUT_Z_L_G);
00149     char high = readRegister(csG, OUT_Z_H_G);
00150     
00151     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00152     
00153     return (float)value/32768.0f*245.0f*PI/180.0f;
00154 }
00155 
00156 /**
00157  * Reads the acceleration in x-direction.
00158  * @return the acceleration in x-direction, given in [m/s2].
00159  */
00160 float IMU::readAccelerationX() {
00161     
00162     char low = readRegister(csXM, OUT_X_L_A);
00163     char high = readRegister(csXM, OUT_X_H_A);
00164     
00165     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00166     
00167     return (float)value/32768.0f*2.0f*9.81f;
00168 }
00169 
00170 /**
00171  * Reads the acceleration in y-direction.
00172  * @return the acceleration in y-direction, given in [m/s2].
00173  */
00174 float IMU::readAccelerationY() {
00175     
00176     char low = readRegister(csXM, OUT_Y_L_A);
00177     char high = readRegister(csXM, OUT_Y_H_A);
00178     
00179     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00180     
00181     return (float)value/32768.0f*2.0f*9.81f;
00182 }
00183 
00184 /**
00185  * Reads the acceleration in z-direction.
00186  * @return the acceleration in z-direction, given in [m/s2].
00187  */
00188 float IMU::readAccelerationZ() {
00189     
00190     char low = readRegister(csXM, OUT_Z_L_A);
00191     char high = readRegister(csXM, OUT_Z_H_A);
00192     
00193     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00194     
00195     return (float)value/32768.0f*2.0f*9.81f;
00196 }
00197 
00198 /**
00199  * Reads the magnetic field in x-direction.
00200  * @return the magnetic field in x-direction, given in [Gauss].
00201  */
00202 float IMU::readMagnetometerX() {
00203     
00204     char low = readRegister(csXM, OUT_X_L_M);
00205     char high = readRegister(csXM, OUT_X_H_M);
00206     
00207     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00208     
00209     return (float)value/32768.0f*2.0f;
00210 }
00211 
00212 /**
00213  * Reads the magnetic field in x-direction.
00214  * @return the magnetic field in x-direction, given in [Gauss].
00215  */
00216 float IMU::readMagnetometerY() {
00217     
00218     char low = readRegister(csXM, OUT_Y_L_M);
00219     char high = readRegister(csXM, OUT_Y_H_M);
00220     
00221     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00222     
00223     return (float)value/32768.0f*2.0f;
00224 }
00225 
00226 /**
00227  * Reads the magnetic field in x-direction.
00228  * @return the magnetic field in x-direction, given in [Gauss].
00229  */
00230 float IMU::readMagnetometerZ() {
00231     
00232     char low = readRegister(csXM, OUT_Z_L_M);
00233     char high = readRegister(csXM, OUT_Z_H_M);
00234     
00235     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00236     
00237     return (float)value/32768.0f*2.0f;
00238 }
00239 
00240 /**
00241  * Reads the compass heading about the z-axis.
00242  * @return the compass heading in the range -PI to +PI, given in [rad].
00243  */
00244 float IMU::readHeading() {
00245     
00246     return heading;
00247 }
00248 
00249 /**
00250  * This method is called periodically by the ticker object and contains the
00251  * calculation and filtering of the heading information.
00252  */
00253 void IMU::run() {
00254     
00255     // read actual measurements from magnetometer registers
00256     
00257     float magnetometerX = magnetometerXFilter.filter(readMagnetometerX());
00258     float magnetometerY = magnetometerYFilter.filter(readMagnetometerY());
00259     
00260     // adjust the minimum and maximum limits, if needed
00261     
00262     if (magnetometerXMin > magnetometerX) magnetometerXMin = magnetometerX;
00263     if (magnetometerXMax < magnetometerX) magnetometerXMax = magnetometerX;
00264     if (magnetometerYMin > magnetometerY) magnetometerYMin = magnetometerY;
00265     if (magnetometerYMax < magnetometerY) magnetometerYMax = magnetometerY;
00266     
00267     // calculate adjusted magnetometer values (gain and offset compensation)
00268     
00269     if (magnetometerXMin < magnetometerXMax) magnetometerX = (magnetometerX-magnetometerXMin)/(magnetometerXMax-magnetometerXMin)-0.5f;
00270     if (magnetometerYMin < magnetometerYMax) magnetometerY = (magnetometerY-magnetometerYMin)/(magnetometerYMax-magnetometerYMin)-0.5f;
00271     
00272     // calculate heading with atan2 from x and y magnetometer measurements
00273     
00274     heading = atan2(magnetometerX, magnetometerY);
00275 }
00276