ROME2 Lab7
Embed:
(wiki syntax)
Show/hide line numbers
TiltAngle.cpp
00001 /* 00002 * TiltAngle.cpp 00003 * Copyright (c) 2020, ZHAW 00004 * All rights reserved. 00005 */ 00006 00007 #include "TiltAngle.h" 00008 00009 using namespace std; 00010 00011 const float TiltAngle::PERIOD = 0.002f; // the period of the timer interrupt, given in [s] 00012 const float TiltAngle::M_PI = 3.14159265358979323846f; // the mathematical constant PI 00013 00014 const float TiltAngle::S_Q_ALPHA = 0.000010f; // standard deviation of process parameter (angle) 00015 const float TiltAngle::S_Q_OMEGA = 0.010000f; // standard deviation of process parameter (rotation) 00016 const float TiltAngle::S_R_ALPHA = 0.000100f; // standard deviation of angle measurement 00017 const float TiltAngle::S_R_OMEGA = 0.000001f; // standard deviation of gyro measurement 00018 00019 const float TiltAngle::LOWPASS_FILTER_FREQUENCY = 1.0f; // frequency of the lowpass filter, given in [rad/s] 00020 const float TiltAngle::HIGHPASS_FILTER_FREQUENCY = 10.0f; // frequency of the highpass filter, given in [rad/s] 00021 00022 /** 00023 * Creates a TiltAngle object. 00024 * @param imu a reference to the IMU to use. 00025 */ 00026 TiltAngle::TiltAngle(IMU& imu) : imu(imu), thread(osPriorityHigh, STACK_SIZE) { 00027 00028 // initialize local values 00029 00030 tiltAngleA = 0.0f; 00031 tiltAngleG = 0.0f; 00032 tiltAngleK = 0.0f; 00033 tiltAngleC = 0.0f; 00034 00035 // initialize parameters for kalman filter 00036 00037 p11 = 0.0f; 00038 p12 = 0.0f; 00039 p21 = 0.0f; 00040 p22 = 0.0f; 00041 xAlpha = 0.0f; 00042 xOmega = 0.0f; 00043 00044 // initialize parameters for complementary filter 00045 00046 alphaAccFiltered = 0.0f; 00047 alphaGyro = 0.0f; 00048 alphaGyroFiltered = 0.0f; 00049 00050 // start thread and timer interrupt 00051 00052 thread.start(callback(this, &TiltAngle::run)); 00053 ticker.attach(callback(this, &TiltAngle::sendThreadFlag), PERIOD); 00054 } 00055 00056 /** 00057 * Deletes the TiltAngle object. 00058 */ 00059 TiltAngle::~TiltAngle() { 00060 00061 ticker.detach(); 00062 } 00063 00064 /** 00065 * Reads the tilt angle around the x-axis, calculated from accelerometer readings. 00066 * @return the tilt angle, given in [rad]. 00067 */ 00068 float TiltAngle::readTiltAngleA() { 00069 00070 return tiltAngleA; 00071 } 00072 00073 /** 00074 * Reads the tilt angle around the x-axis, integrated from gyro readings. 00075 * @return the tilt angle, given in [rad]. 00076 */ 00077 float TiltAngle::readTiltAngleG() { 00078 00079 return tiltAngleG; 00080 } 00081 00082 /** 00083 * Reads the tilt angle around the x-axis, obtained with a Kalman filter. 00084 * @return the tilt angle, given in [rad]. 00085 */ 00086 float TiltAngle::readTiltAngleK() { 00087 00088 return tiltAngleK; 00089 } 00090 00091 /** 00092 * Reads the tilt angle around the x-axis, obtained with a complementary filter. 00093 * @return the tilt angle, given in [rad]. 00094 */ 00095 float TiltAngle::readTiltAngleC() { 00096 00097 return tiltAngleC; 00098 } 00099 00100 /** 00101 * This method is called by the ticker timer interrupt service routine. 00102 * It sends a flag to the thread to make it run again. 00103 */ 00104 void TiltAngle::sendThreadFlag() { 00105 00106 thread.flags_set(threadFlag); 00107 } 00108 00109 /** 00110 * This <code>run()</code> method contains an infinite loop with the run logic. 00111 */ 00112 void TiltAngle::run() { 00113 00114 while (true) { 00115 00116 // wait for the periodic thread flag 00117 00118 ThisThread::flags_wait_any(threadFlag); 00119 00120 // read acceleration and gyro 00121 00122 float accelerationY = -imu.readAccelerationY(); 00123 float accelerationZ = imu.readAccelerationZ(); 00124 float gyroX = imu.readGyroX(); 00125 00126 // calculate tilt angle from acceleration sensors and from gyro 00127 00128 tiltAngleA = atan2(accelerationY, accelerationZ); 00129 tiltAngleG += gyroX*PERIOD; 00130 00131 // calculate prediction for sensor fusion with Kalman-filter 00132 00133 float zAlpha = atan2(accelerationY, accelerationZ); 00134 float zOmega = gyroX; 00135 00136 xAlpha = xAlpha+PERIOD*xOmega; 00137 00138 float p11 = this->p11+this->p12*PERIOD+this->p21*PERIOD+this->p22*PERIOD*PERIOD+S_Q_ALPHA*S_Q_ALPHA; 00139 float p12 = this->p12+this->p22*PERIOD; 00140 float p21 = this->p21+this->p22*PERIOD; 00141 float p22 = this->p22+S_Q_OMEGA*S_Q_OMEGA; 00142 00143 this->p11 = p11; 00144 this->p12 = p12; 00145 this->p21 = p21; 00146 this->p22 = p22; 00147 00148 // calculate correction for sensor fusion with Kalman-filter 00149 00150 float k11 = p11/(p11+S_R_ALPHA*S_R_ALPHA); 00151 float k22 = p22/(p22+S_R_OMEGA*S_R_OMEGA); 00152 00153 xAlpha = xAlpha+k11*(zAlpha-xAlpha); 00154 xOmega = xOmega+k22*(zOmega-xOmega); 00155 00156 p11 = this->p11*(1.0-this->p11/(this->p11+S_R_ALPHA*S_R_ALPHA)); 00157 p12 = 0.0; 00158 p21 = 0.0; 00159 p22 = this->p22*(1.0-this->p22/(this->p22+S_R_ALPHA*S_R_ALPHA)); 00160 00161 this->p11 = p11; 00162 this->p12 = p12; 00163 this->p21 = p21; 00164 this->p22 = p22; 00165 00166 // set tilt angle from Kalman filter 00167 00168 tiltAngleK = xAlpha; 00169 00170 // set tilt angle from complementary filter 00171 00172 float sf = LOWPASS_FILTER_FREQUENCY*PERIOD/(1.0f+LOWPASS_FILTER_FREQUENCY*PERIOD); 00173 alphaAccFiltered = sf*atan2(accelerationY, accelerationZ)+(1.0f-sf)*alphaAccFiltered; 00174 float alphaGyroNew = alphaGyro+PERIOD*gyroX; 00175 alphaGyroFiltered = 1.0f/(1.0f+HIGHPASS_FILTER_FREQUENCY*PERIOD)*(alphaGyroFiltered+alphaGyroNew-alphaGyro); 00176 alphaGyro = alphaGyroNew; 00177 00178 tiltAngleC = alphaAccFiltered+alphaGyroFiltered; 00179 } 00180 } 00181
Generated on Fri Jul 29 2022 01:28:48 by
1.7.2