the fish that looks like a jet
Dependencies: ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo
IMU.cpp
00001 /** 00002 * @section LICENSE 00003 * 00004 * Copyright (c) 2010 ARM Limited 00005 * 00006 * Permission is hereby granted, free of charge, to any person obtaining a copy 00007 * of this software and associated documentation files (the "Software"), to deal 00008 * in the Software without restriction, including without limitation the rights 00009 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 00010 * copies of the Software, and to permit persons to whom the Software is 00011 * furnished to do so, subject to the following conditions: 00012 * 00013 * The above copyright notice and this permission notice shall be included in 00014 * all copies or substantial portions of the Software. 00015 * 00016 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 00017 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00018 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 00019 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 00020 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 00021 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 00022 * THE SOFTWARE. 00023 * 00024 * @section DESCRIPTION 00025 * 00026 * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope using 00027 * orientation filter developed by Sebastian Madgwick. 00028 * 00029 * Find more details about his paper here: 00030 * 00031 * http://code.google.com/p/imumargalgorithm30042010sohm/ 00032 */ 00033 00034 00035 /** 00036 * Includes 00037 */ 00038 #include "IMU.h" 00039 00040 IMU::IMU(float imuRate, 00041 double gyroscopeMeasurementError, 00042 float accelerometerRate, 00043 float gyroscopeRate) : accelerometer(p9, p10), 00044 gyroscope(p9, p10), imuFilter(imuRate, gyroscopeMeasurementError) { 00045 00046 imuRate_ = imuRate; 00047 accelerometerRate_ = accelerometerRate; 00048 gyroscopeRate_ = gyroscopeRate; 00049 00050 a_xAccumulator = 0; 00051 a_yAccumulator = 0; 00052 a_zAccumulator = 0; 00053 w_xAccumulator = 0; 00054 w_yAccumulator = 0; 00055 w_zAccumulator = 0; 00056 00057 accelerometerSamples = 0; 00058 gyroscopeSamples = 0; 00059 00060 initializeAccelerometer(); 00061 calibrateAccelerometer(); 00062 00063 initializeGyroscope(); 00064 calibrateGyroscope(); 00065 00066 accelerometerTicker.attach(this, &IMU::sampleAccelerometer, accelerometerRate_); 00067 gyroscopeTicker.attach(this, &IMU::sampleGyroscope, gyroscopeRate_); 00068 filterTicker.attach(this, &IMU::filter, imuRate_); 00069 00070 } 00071 00072 double IMU::getRoll(void) { 00073 00074 return toDegrees(imuFilter.getRoll()); 00075 00076 } 00077 00078 double IMU::getPitch(void) { 00079 00080 return toDegrees(imuFilter.getPitch()); 00081 00082 } 00083 00084 double IMU::getYaw(void) { 00085 00086 return toDegrees(imuFilter.getYaw()); 00087 00088 } 00089 00090 void IMU::initializeAccelerometer(void) { 00091 00092 //Go into standby mode to configure the device. 00093 accelerometer.setPowerControl(0x00); 00094 //Full resolution, +/-16g, 4mg/LSB. 00095 accelerometer.setDataFormatControl(0x0B); 00096 //200Hz data rate. 00097 accelerometer.setDataRate(ADXL345_200HZ); 00098 //Measurement mode. 00099 accelerometer.setPowerControl(0x08); 00100 //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf 00101 wait_ms(22); 00102 00103 } 00104 00105 void IMU::sampleAccelerometer(void) { 00106 00107 if (accelerometerSamples == SAMPLES) { 00108 00109 a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN; 00110 a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN; 00111 a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN; 00112 00113 a_xAccumulator = 0; 00114 a_yAccumulator = 0; 00115 a_zAccumulator = 0; 00116 accelerometerSamples = 0; 00117 00118 } else { 00119 00120 accelerometer.getOutput(readings); 00121 00122 a_xAccumulator += (int16_t) readings[0]; 00123 a_yAccumulator += (int16_t) readings[1]; 00124 a_zAccumulator += (int16_t) readings[2]; 00125 00126 accelerometerSamples++; 00127 00128 } 00129 00130 } 00131 00132 void IMU::calibrateAccelerometer(void) { 00133 00134 a_xAccumulator = 0; 00135 a_yAccumulator = 0; 00136 a_zAccumulator = 0; 00137 00138 for (int i = 0; i < CALIBRATION_SAMPLES; i++) { 00139 00140 accelerometer.getOutput(readings); 00141 00142 a_xAccumulator += (int16_t) readings[0]; 00143 a_yAccumulator += (int16_t) readings[1]; 00144 a_zAccumulator += (int16_t) readings[2]; 00145 00146 wait(accelerometerRate_); 00147 00148 } 00149 00150 a_xAccumulator /= CALIBRATION_SAMPLES; 00151 a_yAccumulator /= CALIBRATION_SAMPLES; 00152 a_zAccumulator /= CALIBRATION_SAMPLES; 00153 00154 a_xBias = a_xAccumulator; 00155 a_yBias = a_yAccumulator; 00156 a_zBias = (a_zAccumulator - 250); 00157 00158 a_xAccumulator = 0; 00159 a_yAccumulator = 0; 00160 a_zAccumulator = 0; 00161 00162 } 00163 00164 void IMU::initializeGyroscope(void) { 00165 00166 //Low pass filter bandwidth of 42Hz. 00167 gyroscope.setLpBandwidth(LPFBW_42HZ); 00168 //Internal sample rate of 200Hz. 00169 gyroscope.setSampleRateDivider(4); 00170 00171 } 00172 00173 void IMU::calibrateGyroscope(void) { 00174 00175 w_xAccumulator = 0; 00176 w_yAccumulator = 0; 00177 w_zAccumulator = 0; 00178 00179 for (int i = 0; i < CALIBRATION_SAMPLES; i++) { 00180 00181 w_xAccumulator += gyroscope.getGyroX(); 00182 w_yAccumulator += gyroscope.getGyroY(); 00183 w_zAccumulator += gyroscope.getGyroZ(); 00184 wait(gyroscopeRate_); 00185 00186 } 00187 00188 //Average the samples. 00189 w_xAccumulator /= CALIBRATION_SAMPLES; 00190 w_yAccumulator /= CALIBRATION_SAMPLES; 00191 w_zAccumulator /= CALIBRATION_SAMPLES; 00192 00193 w_xBias = w_xAccumulator; 00194 w_yBias = w_yAccumulator; 00195 w_zBias = w_zAccumulator; 00196 00197 w_xAccumulator = 0; 00198 w_yAccumulator = 0; 00199 w_zAccumulator = 0; 00200 00201 } 00202 00203 void IMU::sampleGyroscope(void) { 00204 00205 if (gyroscopeSamples == SAMPLES) { 00206 00207 w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN); 00208 w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN); 00209 w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN); 00210 00211 w_xAccumulator = 0; 00212 w_yAccumulator = 0; 00213 w_zAccumulator = 0; 00214 gyroscopeSamples = 0; 00215 00216 } else { 00217 00218 w_xAccumulator += gyroscope.getGyroX(); 00219 w_yAccumulator += gyroscope.getGyroY(); 00220 w_zAccumulator += gyroscope.getGyroZ(); 00221 00222 gyroscopeSamples++; 00223 00224 } 00225 00226 } 00227 00228 void IMU::filter(void) { 00229 00230 //Update the filter variables. 00231 imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z); 00232 //Calculate the new Euler angles. 00233 imuFilter.computeEuler(); 00234 00235 }
Generated on Mon Jul 25 2022 21:18:46 by 1.7.2