ROME 2 Praktikum 4
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Wed Jul 13 2022 21:00:44 by
1.7.2