IMU.cpp

00001 /*
00002  * IMU.cpp
00003  * Copyright (c) 2020, ZHAW
00004  * All rights reserved.
00005  */
00006 
00007 #include "IMU.h"
00008 
00009 using namespace std;
00010 
00011 const float IMU::M_PI = 3.14159265358979323846f;  // the mathematical constant PI
00012 
00013 /**
00014  * Creates an IMU object.
00015  * @param spi a reference to an spi controller to use.
00016  * @param csAG the chip select output for the accelerometer and the gyro sensor.
00017  * @param csM the chip select output for the magnetometer.
00018  */
00019 IMU::IMU(SPI& spi, DigitalOut& csAG, DigitalOut& csM) : spi(spi), csAG(csAG), csM(csM) {
00020     
00021     // initialize SPI interface
00022     
00023     spi.format(8, 3);
00024     spi.frequency(1000000);
00025     
00026     // reset chip select lines to logical high
00027     
00028     csAG = 1;
00029     csM = 1;
00030     
00031     // initialize accelerometer and gyro
00032     
00033     writeRegister(csAG, CTRL_REG1_G, 0xC3);     // ODR 952 Hz, full scale 245 deg/s
00034     writeRegister(csAG, CTRL_REG2_G, 0x00);     // disable interrupt generation
00035     writeRegister(csAG, CTRL_REG3_G, 0x00);     // disable low power mode, disable high pass filter, high pass cutoff frequency 57 Hz
00036     writeRegister(csAG, CTRL_REG4, 0x38);       // enable gyro in all 3 axis
00037     writeRegister(csAG, CTRL_REG5_XL, 0x38);    // no decimation, enable accelerometer in all 3 axis
00038     writeRegister(csAG, CTRL_REG6_XL, 0xC0);    // ODR 952 Hz, full scale 2g
00039     writeRegister(csAG, CTRL_REG7_XL, 0x00);    // high res mode disabled, filter bypassed
00040     writeRegister(csAG, CTRL_REG8, 0x00);       // 4-wire SPI interface, LSB at lower address
00041     writeRegister(csAG, CTRL_REG9, 0x04);       // disable gyro sleep mode, disable I2C interface, disable FIFO
00042     writeRegister(csAG, CTRL_REG10, 0x00);      // self test disabled
00043     
00044     // initialize magnetometer
00045     
00046     writeRegister(csM, CTRL_REG1_M, 0x10);      // temperature not compensated, low power mode for x & y axis, data rate 10 Hz
00047     writeRegister(csM, CTRL_REG2_M, 0x00);      // full scale 4 gauss
00048     writeRegister(csM, CTRL_REG3_M, 0x80);      // disable I2C interface, low power mode, SPI write only, continuous conversion mode
00049     writeRegister(csM, CTRL_REG4_M, 0x00);      // low power mode for z axis, LSB at lower address
00050     writeRegister(csM, CTRL_REG5_M, 0x00);      // fast read disabled
00051 }
00052 
00053 /**
00054  * Deletes the IMU object.
00055  */
00056 IMU::~IMU() {}
00057 
00058 /**
00059  * This private method allows to write a register value.
00060  * @param cs the chip select output to use, either csAG or csM.
00061  * @param address the 7 bit address of the register.
00062  * @param value the value to write into the register.
00063  */
00064 void IMU::writeRegister(DigitalOut& cs, char address, char value) {
00065     
00066     cs = 0;
00067     
00068     spi.write(0x7F & address);
00069     spi.write(value & 0xFF);
00070     
00071     cs = 1;
00072 }
00073 
00074 /**
00075  * This private method allows to read a register value.
00076  * @param cs the chip select output to use, either csAG or csM.
00077  * @param address the 7 bit address of the register.
00078  * @return the value read from the register.
00079  */
00080 char IMU::readRegister(DigitalOut& cs, char address) {
00081     
00082     cs = 0;
00083     
00084     spi.write(0x80 | address);
00085     int value = spi.write(0xFF);
00086     
00087     cs = 1;
00088     
00089     return (char)(value & 0xFF);
00090 }
00091 
00092 /**
00093  * Reads the acceleration in x-direction.
00094  * @return the acceleration in x-direction, given in [m/s2].
00095  */
00096 float IMU::readAccelerationX() {
00097     
00098     mutex.lock();
00099     
00100     char low = readRegister(csAG, OUT_X_L_XL);
00101     char high = readRegister(csAG, OUT_X_H_XL);
00102     
00103     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00104     
00105     mutex.unlock();
00106     
00107     return (float)value/32768.0f*2.0f*9.81f;
00108 }
00109 
00110 /**
00111  * Reads the acceleration in y-direction.
00112  * @return the acceleration in y-direction, given in [m/s2].
00113  */
00114 float IMU::readAccelerationY() {
00115     
00116     mutex.lock();
00117     
00118     char low = readRegister(csAG, OUT_Y_L_XL);
00119     char high = readRegister(csAG, OUT_Y_H_XL);
00120     
00121     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00122     
00123     mutex.unlock();
00124     
00125     return (float)value/32768.0f*2.0f*9.81f;
00126 }
00127 
00128 /**
00129  * Reads the acceleration in z-direction.
00130  * @return the acceleration in z-direction, given in [m/s2].
00131  */
00132 float IMU::readAccelerationZ() {
00133     
00134     mutex.lock();
00135     
00136     char low = readRegister(csAG, OUT_Z_L_XL);
00137     char high = readRegister(csAG, OUT_Z_H_XL);
00138     
00139     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00140     
00141     mutex.unlock();
00142     
00143     return (float)value/32768.0f*2.0f*9.81f;
00144 }
00145 
00146 /**
00147  * Reads the gyroscope about the x-axis.
00148  * @return the rotational speed about the x-axis given in [rad/s].
00149  */
00150 float IMU::readGyroX() {
00151     
00152     mutex.lock();
00153     
00154     char low = readRegister(csAG, OUT_X_L_G);
00155     char high = readRegister(csAG, OUT_X_H_G);
00156     
00157     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00158     
00159     mutex.unlock();
00160     
00161     return (float)value/32768.0f*245.0f*M_PI/180.0f;
00162 }
00163 
00164 /**
00165  * Reads the gyroscope about the y-axis.
00166  * @return the rotational speed about the y-axis given in [rad/s].
00167  */
00168 float IMU::readGyroY() {
00169     
00170     mutex.lock();
00171     
00172     char low = readRegister(csAG, OUT_Y_L_G);
00173     char high = readRegister(csAG, OUT_Y_H_G);
00174     
00175     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00176     
00177     mutex.unlock();
00178     
00179     return (float)value/32768.0f*245.0f*M_PI/180.0f;
00180 }
00181 
00182 /**
00183  * Reads the gyroscope about the z-axis.
00184  * @return the rotational speed about the z-axis given in [rad/s].
00185  */
00186 float IMU::readGyroZ() {
00187     
00188     mutex.lock();
00189     
00190     char low = readRegister(csAG, OUT_Z_L_G);
00191     char high = readRegister(csAG, OUT_Z_H_G);
00192     
00193     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00194     
00195     mutex.unlock();
00196     
00197     return (float)value/32768.0f*245.0f*M_PI/180.0f;
00198 }
00199 
00200 /**
00201  * Reads the magnetic field in x-direction.
00202  * @return the magnetic field in x-direction, given in [Gauss].
00203  */
00204 float IMU::readMagnetometerX() {
00205     
00206     mutex.lock();
00207     
00208     char low = readRegister(csM, OUT_X_L_M);
00209     char high = readRegister(csM, OUT_X_H_M);
00210     
00211     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00212     
00213     mutex.unlock();
00214     
00215     return (float)value/32768.0f*4.0f;
00216 }
00217 
00218 /**
00219  * Reads the magnetic field in y-direction.
00220  * @return the magnetic field in y-direction, given in [Gauss].
00221  */
00222 float IMU::readMagnetometerY() {
00223     
00224     mutex.lock();
00225     
00226     char low = readRegister(csM, OUT_Y_L_M);
00227     char high = readRegister(csM, OUT_Y_H_M);
00228     
00229     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00230     
00231     mutex.unlock();
00232     
00233     return (float)value/32768.0f*4.0f;
00234 }
00235 
00236 /**
00237  * Reads the magnetic field in z-direction.
00238  * @return the magnetic field in z-direction, given in [Gauss].
00239  */
00240 float IMU::readMagnetometerZ() {
00241     
00242     mutex.lock();
00243     
00244     char low = readRegister(csM, OUT_Z_L_M);
00245     char high = readRegister(csM, OUT_Z_H_M);
00246     
00247     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00248     
00249     mutex.unlock();
00250     
00251     return (float)value/32768.0f*4.0f;
00252 }
00253