ROME 2 Praktikum 4

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers IMU.cpp Source File

IMU.cpp

00001 /*
00002  * IMU.cpp
00003  * Copyright (c) 2020, ZHAW
00004  * All rights reserved.
00005  */
00006 #include <mbed.h>
00007 #include "IMU.h"
00008 #include "LowpassFilter.h"
00009 #include "stdio.h"
00010 #include <mbed.h>
00011 #include <EthernetInterface.h>
00012 #include "IMU.h"
00013 #include "HTTPServer.h"
00014 #include "HTTPScriptIMU.h"
00015 
00016 using namespace std;
00017 
00018 const float IMU::PERIOD = 0.002f;                   // the period of the timer interrupt, given in [s]
00019 const float IMU::M_PI = 3.14159265358979323846f;    // the mathematical constant PI
00020 const float IMU::LOWPASS_FILTER_FREQUENCY = 2.0f*M_PI;  // given in [rad/s]
00021 
00022 // init lowpasses
00023     
00024 
00025 /**
00026  * Creates an IMU object.
00027  * @param spi a reference to an spi controller to use.
00028  * @param csAG the chip select output for the accelerometer and the gyro sensor.
00029  * @param csM the chip select output for the magnetometer.
00030  */
00031 IMU::IMU(SPI& spi, DigitalOut& csAG, DigitalOut& csM) : spi(spi), csAG(csAG), csM(csM), thread(osPriorityHigh, STACK_SIZE) {
00032     
00033     // initialize SPI interface
00034     
00035     spi.format(8, 3);
00036     spi.frequency(1000000);
00037     
00038     // reset chip select lines to logical high
00039     
00040     csAG = 1;
00041     csM = 1;
00042     
00043     // initialize accelerometer and gyro
00044     
00045     writeRegister(csAG, CTRL_REG1_G, 0xC3);     // ODR 952 Hz, full scale 245 deg/s
00046     writeRegister(csAG, CTRL_REG2_G, 0x00);     // disable interrupt generation
00047     writeRegister(csAG, CTRL_REG3_G, 0x00);     // disable low power mode, disable high pass filter, high pass cutoff frequency 57 Hz
00048     writeRegister(csAG, CTRL_REG4, 0x38);       // enable gyro in all 3 axis
00049     writeRegister(csAG, CTRL_REG5_XL, 0x38);    // no decimation, enable accelerometer in all 3 axis
00050     writeRegister(csAG, CTRL_REG6_XL, 0xC0);    // ODR 952 Hz, full scale 2g
00051     writeRegister(csAG, CTRL_REG7_XL, 0x00);    // high res mode disabled, filter bypassed
00052     writeRegister(csAG, CTRL_REG8, 0x00);       // 4-wire SPI interface, LSB at lower address
00053     writeRegister(csAG, CTRL_REG9, 0x04);       // disable gyro sleep mode, disable I2C interface, disable FIFO
00054     writeRegister(csAG, CTRL_REG10, 0x00);      // self test disabled
00055     
00056     // initialize magnetometer
00057     
00058     writeRegister(csM, CTRL_REG1_M, 0x10);      // temperature not compensated, low power mode for x & y axis, data rate 10 Hz
00059     writeRegister(csM, CTRL_REG2_M, 0x00);      // full scale 4 gauss
00060     writeRegister(csM, CTRL_REG3_M, 0x80);      // disable I2C interface, low power mode, SPI write only, continuous conversion mode
00061     writeRegister(csM, CTRL_REG4_M, 0x00);      // low power mode for z axis, LSB at lower address
00062     writeRegister(csM, CTRL_REG5_M, 0x00);      // fast read disabled
00063     
00064     // start thread and timer interrupt
00065     
00066     thread.start(callback(this, &IMU::run));
00067     ticker.attach(callback(this, &IMU::sendThreadFlag), PERIOD);
00068     
00069     FilterX.setPeriod(PERIOD);
00070     FilterX.setFrequency(LOWPASS_FILTER_FREQUENCY);
00071     FilterY.setPeriod(PERIOD);
00072     FilterY.setFrequency(LOWPASS_FILTER_FREQUENCY);
00073     
00074     mxMin = 0.05;
00075     mxMax = 0.1;
00076     myMin = 0.05;
00077     myMax = 0.1;
00078     
00079     myX_korr = 0;
00080     myY_korr = 0;
00081 }
00082 
00083 /**
00084  * Deletes the IMU object.
00085  */
00086 IMU::~IMU() {
00087     
00088     ticker.detach();
00089 }
00090 
00091 /**
00092  * This private method allows to write a register value.
00093  * @param cs the chip select output to use, either csAG or csM.
00094  * @param address the 7 bit address of the register.
00095  * @param value the value to write into the register.
00096  */
00097 void IMU::writeRegister(DigitalOut& cs, char address, char value) {
00098     
00099     cs = 0;
00100     
00101     spi.write(0x7F & address);
00102     spi.write(value & 0xFF);
00103     
00104     cs = 1;
00105 }
00106 
00107 /**
00108  * This private method allows to read a register value.
00109  * @param cs the chip select output to use, either csAG or csM.
00110  * @param address the 7 bit address of the register.
00111  * @return the value read from the register.
00112  */
00113 char IMU::readRegister(DigitalOut& cs, char address) {
00114     
00115     cs = 0;
00116     
00117     spi.write(0x80 | address);
00118     int value = spi.write(0xFF);
00119     
00120     cs = 1;
00121     
00122     return (char)(value & 0xFF);
00123 }
00124 
00125 /**
00126  * Reads the acceleration in x-direction.
00127  * @return the acceleration in x-direction, given in [m/s2].
00128  */
00129 float IMU::readAccelerationX() {
00130     
00131     mutex.lock();
00132     
00133     char low = readRegister(csAG, OUT_X_L_XL);
00134     char high = readRegister(csAG, OUT_X_H_XL);
00135     
00136     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00137     
00138     mutex.unlock();
00139     
00140     return (float)value/32768.0f*2.0f*9.81f;
00141 }
00142 
00143 /**
00144  * Reads the acceleration in y-direction.
00145  * @return the acceleration in y-direction, given in [m/s2].
00146  */
00147 float IMU::readAccelerationY() {
00148     
00149     mutex.lock();
00150     
00151     char low = readRegister(csAG, OUT_Y_L_XL);
00152     char high = readRegister(csAG, OUT_Y_H_XL);
00153     
00154     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00155     
00156     mutex.unlock();
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     mutex.lock();
00168     
00169     char low = readRegister(csAG, OUT_Z_L_XL);
00170     char high = readRegister(csAG, OUT_Z_H_XL);
00171     
00172     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00173     
00174     mutex.unlock();
00175     
00176     return (float)value/32768.0f*2.0f*9.81f;
00177 }
00178 
00179 /**
00180  * Reads the gyroscope about the x-axis.
00181  * @return the rotational speed about the x-axis given in [rad/s].
00182  */
00183 float IMU::readGyroX() {
00184     
00185     mutex.lock();
00186     
00187     char low = readRegister(csAG, OUT_X_L_G);
00188     char high = readRegister(csAG, OUT_X_H_G);
00189     
00190     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00191     
00192     mutex.unlock();
00193     
00194     return (float)value/32768.0f*245.0f*M_PI/180.0f;
00195 }
00196 
00197 /**
00198  * Reads the gyroscope about the y-axis.
00199  * @return the rotational speed about the y-axis given in [rad/s].
00200  */
00201 float IMU::readGyroY() {
00202     
00203     mutex.lock();
00204     
00205     char low = readRegister(csAG, OUT_Y_L_G);
00206     char high = readRegister(csAG, OUT_Y_H_G);
00207     
00208     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00209     
00210     mutex.unlock();
00211     
00212     return (float)value/32768.0f*245.0f*M_PI/180.0f;
00213 }
00214 
00215 /**
00216  * Reads the gyroscope about the z-axis.
00217  * @return the rotational speed about the z-axis given in [rad/s].
00218  */
00219 float IMU::readGyroZ() {
00220     
00221     mutex.lock();
00222     
00223     char low = readRegister(csAG, OUT_Z_L_G);
00224     char high = readRegister(csAG, OUT_Z_H_G);
00225     
00226     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00227     
00228     mutex.unlock();
00229     
00230     return (float)value/32768.0f*245.0f*M_PI/180.0f;
00231 }
00232 
00233 /**
00234  * Reads the magnetic field in x-direction.
00235  * @return the magnetic field in x-direction, given in [Gauss].
00236  */
00237 float IMU::readMagnetometerX() {
00238     
00239     mutex.lock();
00240     
00241     char low = readRegister(csM, OUT_X_L_M);
00242     char high = readRegister(csM, OUT_X_H_M);
00243     
00244     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00245     
00246     mutex.unlock();
00247     
00248     return (float)value/32768.0f*4.0f;
00249     
00250 
00251 }
00252 
00253 /**
00254  * Reads the magnetic field in y-direction.
00255  * @return the magnetic field in y-direction, given in [Gauss].
00256  */
00257 float IMU::readMagnetometerY() {
00258     
00259     mutex.lock();
00260     
00261     char low = readRegister(csM, OUT_Y_L_M);
00262     char high = readRegister(csM, OUT_Y_H_M);
00263     
00264     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00265     
00266     mutex.unlock();
00267     
00268     return (float)value/32768.0f*4.0f;
00269 }
00270 
00271 /**
00272  * Reads the magnetic field in z-direction.
00273  * @return the magnetic field in z-direction, given in [Gauss].
00274  */
00275 float IMU::readMagnetometerZ() {
00276     
00277     /*mutex.lock();
00278     
00279     char low = readRegister(csM, OUT_Z_L_M);
00280     char high = readRegister(csM, OUT_Z_H_M);
00281     
00282     short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
00283     
00284     mutex.unlock();
00285     
00286     return (float)value/32768.0f*4.0f;*/
00287     return readHeading();
00288 }
00289 
00290 float IMU::readHeading() {
00291     
00292     float orient = atan2(myX_korr,myY_korr);
00293     
00294     if (orient < -M_PI){
00295         orient = orient + M_PI;
00296         return orient;
00297         }
00298     else if (orient > M_PI){
00299         orient = orient - M_PI;
00300         return orient;
00301         }
00302     else {
00303     return orient;
00304     }
00305 }
00306 
00307 /**
00308  * This method is called by the ticker timer interrupt service routine.
00309  * It sends a flag to the thread to make it run again.
00310  */
00311 void IMU::sendThreadFlag() {
00312     
00313     thread.flags_set(threadFlag);
00314 }
00315 
00316 /**
00317  * This <code>run()</code> method contains an infinite loop with the run logic.
00318  */
00319 void IMU::run() {
00320     
00321     while (true) {
00322         
00323         // wait for the periodic thread flag
00324         
00325         ThisThread::flags_wait_any(threadFlag);
00326         float myX = FilterX.filter(readMagnetometerX());
00327         float myY = FilterY.filter(readMagnetometerY());
00328         // filter and process sensor data...
00329         if (myX < mxMin) {
00330             mxMin = myX;
00331             }
00332         if (myX > mxMax) {
00333             mxMax = myX;
00334             }
00335         if (myY < myMin) {
00336             myMin = myY;
00337             }
00338         if (myY > myMax) {
00339             myMax = myY;
00340             }        
00341         
00342         myX_korr = (2*(myX-mxMin)/(mxMax-mxMin)) - 1;
00343         myY_korr = (2*(myY-myMin)/(myMax-myMin)) - 1;
00344         
00345         
00346     }
00347 }
00348