ROME2 Lab7

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers TiltAngle.cpp Source File

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