ROME2 Lab7
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Fri Jul 29 2022 01:28:48 by
1.7.2