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::PI = 3.14159265f;                  // the constant PI
00013 
00014 /**
00015  * Creates an IMU object.
00016  * @param spi a reference to an spi controller to use.
00017  * @param csG the chip select output for the gyro sensor.
00018  * @param csXM the chip select output for the accelerometer and the magnetometer.
00019  */
00020 IMU::IMU(SPI& spi, DigitalOut& csG, DigitalOut& csXM) : spi(spi), csG(csG), csXM(csXM) {
00021     
00022     // initialize SPI interface
00023     
00024     spi.format(8, 3);
00025     spi.frequency(1000000);
00026     
00027     // reset chip select lines to logical high
00028     
00029     csG = 1;
00030     csXM = 1;
00031     
00032     // initialize gyro
00033     
00034     writeRegister(csG, CTRL_REG1_G, 0x0F);  // enable gyro in all 3 axis
00035     
00036     // initialize accelerometer
00037     
00038     writeRegister(csXM, CTRL_REG0_XM, 0x00);
00039     writeRegister(csXM, CTRL_REG1_XM, 0x5F);
00040     writeRegister(csXM, CTRL_REG2_XM, 0x00);
00041     writeRegister(csXM, CTRL_REG3_XM, 0x04);
00042     
00043     // initialize magnetometer
00044     
00045     writeRegister(csXM, CTRL_REG5_XM, 0x94);
00046     writeRegister(csXM, CTRL_REG6_XM, 0x00);
00047     writeRegister(csXM, CTRL_REG7_XM, 0x00);
00048     writeRegister(csXM, CTRL_REG4_XM, 0x04);
00049     writeRegister(csXM, INT_CTRL_REG_M, 0x09);
00050 }
00051 
00052 /**
00053  * Deletes the IMU object.
00054  */
00055 IMU::~IMU() {}
00056 
00057 /**
00058  * This private method allows to write a register value.
00059  * @param cs the chip select output to use, either csG or csXM.
00060  * @param address the 7 bit address of the register.
00061  * @param value the value to write into the register.
00062  */
00063 void IMU::writeRegister(DigitalOut& cs, char address, char value) {
00064     
00065     cs = 0;
00066     
00067     spi.write(0x7F & address);
00068     spi.write(value & 0xFF);
00069     
00070     cs = 1;
00071 }
00072 
00073 /**
00074  * This private method allows to read a register value.
00075  * @param cs the chip select output to use, either csG or csXM.
00076  * @param address the 7 bit address of the register.
00077  * @return the value read from the register.
00078  */
00079 char IMU::readRegister(DigitalOut& cs, char address) {
00080     
00081     cs = 0;
00082     
00083     spi.write(0x80 | address);
00084     int value = spi.write(0xFF);
00085     
00086     cs = 1;
00087     
00088     return (char)(value & 0xFF);
00089 }
00090 
00091 /**
00092  * Reads the gyroscope about the x-axis.
00093  * @return the rotational speed about the x-axis given in [rad/s].
00094  */
00095 float IMU::readGyroX() {
00096     
00097     char low = readRegister(csG, OUT_X_L_G);
00098     char high = readRegister(csG, OUT_X_H_G);
00099     
00100     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00101     
00102     return (float)value/32768.0f*245.0f*PI/180.0f;
00103 }
00104 
00105 /**
00106  * Reads the gyroscope about the y-axis.
00107  * @return the rotational speed about the y-axis given in [rad/s].
00108  */
00109 float IMU::readGyroY() {
00110 
00111     char low = readRegister(csG, OUT_Y_L_G);
00112     char high = readRegister(csG, OUT_Y_H_G);
00113     
00114     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00115     
00116     return (float)value/32768.0f*245.0f*PI/180.0f;
00117 }
00118 
00119 /**
00120  * Reads the gyroscope about the z-axis.
00121  * @return the rotational speed about the z-axis given in [rad/s].
00122  */
00123 float IMU::readGyroZ() {
00124     
00125     char low = readRegister(csG, OUT_Z_L_G);
00126     char high = readRegister(csG, OUT_Z_H_G);
00127     
00128     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00129     
00130     return (float)value/32768.0f*245.0f*PI/180.0f;
00131 }
00132 
00133 /**
00134  * Reads the acceleration in x-direction.
00135  * @return the acceleration in x-direction, given in [m/s2].
00136  */
00137 float IMU::readAccelerationX() {
00138     
00139     char low = readRegister(csXM, OUT_X_L_A);
00140     char high = readRegister(csXM, OUT_X_H_A);
00141     
00142     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00143     
00144     return (float)value/32768.0f*2.0f*9.81f;
00145 }
00146 
00147 /**
00148  * Reads the acceleration in y-direction.
00149  * @return the acceleration in y-direction, given in [m/s2].
00150  */
00151 float IMU::readAccelerationY() {
00152     
00153     char low = readRegister(csXM, OUT_Y_L_A);
00154     char high = readRegister(csXM, OUT_Y_H_A);
00155     
00156     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00157     
00158     return (float)value/32768.0f*2.0f*9.81f;
00159 }
00160 
00161 /**
00162  * Reads the acceleration in z-direction.
00163  * @return the acceleration in z-direction, given in [m/s2].
00164  */
00165 float IMU::readAccelerationZ() {
00166     
00167     char low = readRegister(csXM, OUT_Z_L_A);
00168     char high = readRegister(csXM, OUT_Z_H_A);
00169     
00170     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00171     
00172     return (float)value/32768.0f*2.0f*9.81f;
00173 }
00174 
00175 /**
00176  * Reads the magnetic field in x-direction.
00177  * @return the magnetic field in x-direction, given in [Gauss].
00178  */
00179 float IMU::readMagnetometerX() {
00180     
00181     // bitte implementieren!
00182     
00183     return 0.0f;
00184 }
00185 
00186 /**
00187  * Reads the magnetic field in x-direction.
00188  * @return the magnetic field in x-direction, given in [Gauss].
00189  */
00190 float IMU::readMagnetometerY() {
00191     
00192     // bitte implementieren!
00193     
00194     return 0.0f;
00195 }
00196 
00197 /**
00198  * Reads the magnetic field in x-direction.
00199  * @return the magnetic field in x-direction, given in [Gauss].
00200  */
00201 float IMU::readMagnetometerZ() {
00202     
00203     // bitte implementieren!
00204     
00205     return 0.0f;
00206 }
00207 
00208 /**
00209  * Reads the compass heading about the z-axis.
00210  * @return the compass heading in the range -PI to +PI, given in [rad].
00211  */
00212 float IMU::readHeading() {
00213     
00214     // bitte implementieren!
00215     
00216     return 0.0f;
00217 }
00218