flight/modules/Airspeed/imu_airspeed.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup AirspeedModule Airspeed Module
   6  * @brief Use attitude and velocity data to estimate airspeed
   7  * @{
   8  *
   9  * @file       imu_airspeed.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
  11  * @brief      IMU based airspeed calculation
  12  *
  13  * @see        The GNU Public License (GPL) Version 3
  14  *
  15  *****************************************************************************/
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31
  32
  33 #include "openpilot.h"
  34 #include "velocitystate.h"
  35 #include "attitudestate.h"
  36 #include "airspeedsensor.h"
  37 #include "airspeedsettings.h"
  38 #include "imu_airspeed.h"
  39 #include "CoordinateConversions.h"
  40 #include "butterworth.h"
  41 #include <pios_math.h>
  42
  43
  44 // Private constants
  45 #define EPS               1e-6f
  46 #define EPS_REORIENTATION 1e-10f
  47 #define EPS_VELOCITY      1.f
  48
  49 // Private types
  50 // structure with smoothed fuselage orientation, ground speed, wind vector and their changes in time
  51 struct IMUGlobals {
  52     // Butterworth filters
  53     struct ButterWorthDF2Filter filter;
  54     struct ButterWorthDF2Filter prefilter;
  55     float ff, ffV;
  56
  57     // storage variables for Butterworth filter
  58     float pn1, pn2;
  59     float yn1, yn2;
  60     float v1n1, v1n2;
  61     float v2n1, v2n2;
  62     float v3n1, v3n2;
  63     float Vw1n1, Vw1n2;
  64     float Vw2n1, Vw2n2;
  65     float Vw3n1, Vw3n2;
  66     float Vw1, Vw2, Vw3;
  67
  68     // storage variables for derivative calculation
  69     float pOld, yOld;
  70     float v1Old, v2Old, v3Old;
  71 };
  72
  73
  74 // Private variables
  75 static struct IMUGlobals *imu;
  76
  77 // Private functions
  78 // a simple square inline function based on multiplication faster than powf(x,2.0f)
  79 static inline float Sq(float x)
  80 {
  81     return x * x;
  82 }
  83
  84 // ****** find pitch, yaw from quaternion ********
  85 static void Quaternion2PY(const float q0, const float q1, const float q2, const float q3, float *pPtr, float *yPtr, bool principalArg)
  86 {
  87     float R13, R11, R12;
  88     const float q0s = q0 * q0;
  89     const float q1s = q1 * q1;
  90     const float q2s = q2 * q2;
  91     const float q3s = q3 * q3;
  92
  93     R13   = 2.0f * (q1 * q3 - q0 * q2);
  94     R11   = q0s + q1s - q2s - q3s;
  95     R12   = 2.0f * (q1 * q2 + q0 * q3);
  96
  97     *pPtr = asinf(-R13); // pitch always between -pi/2 to pi/2
  98
  99     const float y_ = atan2f(R12, R11);
 100     // use old yaw contained in y to add multiples of 2pi to have a continuous yaw if user does not want the principal argument
 101     // else simply copy atan2 result into result
 102     if (principalArg) {
 103         *yPtr = y_;
 104     } else {
 105         // calculate needed mutliples of 2pi to avoid jumps
 106         // number of cycles accumulated in old yaw
 107         const int32_t cycles = (int32_t)(*yPtr / M_2PI_F);
 108         // look for a jump by substracting the modulus, i.e. there is maximally one jump.
 109         // take slightly less than 2pi, because the jump will always be lower than 2pi
 110         const int32_t mod    = (int32_t)((y_ - (*yPtr - cycles * M_2PI_F)) / (M_2PI_F * 0.8f));
 111         *yPtr = y_ + M_2PI_F * (cycles - mod);
 112     }
 113 }
 114
 115 static void PY2xB(const float p, const float y, float x[3])
 116 {
 117     const float cosp = cosf(p);
 118
 119     x[0] = cosp * cosf(y);
 120     x[1] = cosp * sinf(y);
 121     x[2] = -sinf(p);
 122 }
 123
 124
 125 static void PY2DeltaxB(const float p, const float y, const float xB[3], float x[3])
 126 {
 127     const float cosp = cosf(p);
 128
 129     x[0] = xB[0] - cosp * cosf(y);
 130     x[1] = xB[1] - cosp * sinf(y);
 131     x[2] = xB[2] - -sinf(p);
 132 }
 133
 134
 135 /*
 136  * Initialize function loads first data sets, and allocates memory for structure.
 137  */
 138 void imu_airspeedInitialize(const AirspeedSettingsData *airspeedSettings)
 139 {
 140     // pre-filter frequency rate
 141     const float ff  = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod1;
 142     // filter frequency rate
 143     const float ffV = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod2;
 144
 145     // This method saves memory in case we don't use the module.
 146     imu = (struct IMUGlobals *)pios_malloc(sizeof(struct IMUGlobals));
 147
 148     // airspeed calculation variables
 149     VelocityStateInitialize();
 150     VelocityStateData velData;
 151     VelocityStateGet(&velData);
 152
 153     AttitudeStateData attData;
 154     AttitudeStateGet(&attData);
 155
 156     // initialize filters for given ff and ffV
 157     InitButterWorthDF2Filter(ffV, &(imu->filter));
 158     InitButterWorthDF2Filter(ff, &(imu->prefilter));
 159     imu->ffV = ffV;
 160     imu->ff  = ff;
 161
 162     // get pitch and yaw from quarternion; principal argument for yaw
 163     Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4, &(imu->pOld), &(imu->yOld), true);
 164     InitButterWorthDF2Values(imu->pOld, &(imu->prefilter), &(imu->pn1), &(imu->pn2));
 165     InitButterWorthDF2Values(imu->yOld, &(imu->prefilter), &(imu->yn1), &(imu->yn2));
 166
 167     // use current NED speed as vOld vector and as initial value for filter
 168     imu->v1Old = velData.North;
 169     imu->v2Old = velData.East;
 170     imu->v3Old = velData.Down;
 171     InitButterWorthDF2Values(imu->v1Old, &(imu->prefilter), &(imu->v1n1), &(imu->v1n2));
 172     InitButterWorthDF2Values(imu->v2Old, &(imu->prefilter), &(imu->v2n1), &(imu->v2n2));
 173     InitButterWorthDF2Values(imu->v3Old, &(imu->prefilter), &(imu->v3n1), &(imu->v3n2));
 174
 175     // initial guess for windspeed is zero
 176     imu->Vw3   = imu->Vw2 = imu->Vw1 = 0.0f;
 177     InitButterWorthDF2Values(0.0f, &(imu->filter), &(imu->Vw1n1), &(imu->Vw1n2));
 178     imu->Vw3n1 = imu->Vw2n1 = imu->Vw1n1;
 179     imu->Vw3n2 = imu->Vw2n2 = imu->Vw1n2;
 180 }
 181
 182 /*
 183  * Calculate airspeed as a function of groundspeed and vehicle attitude.
 184  *  Adapted from "IMU Wind Estimation (Theory)", by William Premerlani.
 185  *  The idea is that V_gps=V_air+V_wind. If we assume wind constant, =>
 186  *  V_gps_2-V_gps_1 = (V_air_2+V_wind_2) -(V_air_1+V_wind_1) = V_air_2 - V_air_1.
 187  *  If we assume airspeed constant, => V_gps_2-V_gps_1 = |V|*(f_2 - f1),
 188  *  where "f" is the fuselage vector in earth coordinates.
 189  *  We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|.
 190  *  Adapted to: |V| = (V_gps_2-V_gps_1) dot (f2_-f_1) / |f_2 - f1|^2.
 191  *
 192  * See OP-1317 imu_wind_estimation.pdf for details on the adaptation
 193  * Need a low pass filter to filter out spikes in non coordinated maneuvers
 194  * A two step Butterworth second order filter is used. In the first step fuselage vector xB
 195  * and ground speed vector Vel are filtered. The fuselage vector is filtered through its pitch
 196  * and yaw to keep a unit length. After building the differenced dxB and dVel are produced and
 197  * the airspeed calculated. The calculated airspeed is filtered again with a Butterworth filter
 198  */
 199 void imu_airspeedGet(AirspeedSensorData *airspeedData, const AirspeedSettingsData *airspeedSettings)
 200 {
 201     // pre-filter frequency rate
 202     const float ff  = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod1;
 203     // filter frequency rate
 204     const float ffV = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod2;
 205
 206     // check for a change in filter frequency rate. if yes, then actualize filter constants and intermediate values
 207     if (fabsf(ffV - imu->ffV) > EPS) {
 208         InitButterWorthDF2Filter(ffV, &(imu->filter));
 209         InitButterWorthDF2Values(imu->Vw1, &(imu->filter), &(imu->Vw1n1), &(imu->Vw1n2));
 210         InitButterWorthDF2Values(imu->Vw2, &(imu->filter), &(imu->Vw2n1), &(imu->Vw2n2));
 211         InitButterWorthDF2Values(imu->Vw3, &(imu->filter), &(imu->Vw3n1), &(imu->Vw3n2));
 212     }
 213     if (fabsf(ff - imu->ff) > EPS) {
 214         InitButterWorthDF2Filter(ff, &(imu->prefilter));
 215         InitButterWorthDF2Values(imu->pOld, &(imu->prefilter), &(imu->pn1), &(imu->pn2));
 216         InitButterWorthDF2Values(imu->yOld, &(imu->prefilter), &(imu->yn1), &(imu->yn2));
 217         InitButterWorthDF2Values(imu->v1Old, &(imu->prefilter), &(imu->v1n1), &(imu->v1n2));
 218         InitButterWorthDF2Values(imu->v2Old, &(imu->prefilter), &(imu->v2n1), &(imu->v2n2));
 219         InitButterWorthDF2Values(imu->v3Old, &(imu->prefilter), &(imu->v3n1), &(imu->v3n2));
 220     }
 221
 222     float normVel2;
 223     float normDiffAttitude2;
 224     float dvdtDotdfdt;
 225
 226     float xB[3];
 227     // get values and conduct smoothing of ground speed and orientation independently of the calculation of airspeed
 228     { // Scoping to save memory
 229         AttitudeStateData attData;
 230         AttitudeStateGet(&attData);
 231         VelocityStateData velData;
 232         VelocityStateGet(&velData);
 233         float p = imu->pOld, y = imu->yOld;
 234         float dxB[3];
 235
 236         // get pitch and roll Euler angles from quaternion
 237         // do not calculate the principlal argument of yaw, i.e. use old yaw to add multiples of 2pi to have a continuous yaw
 238         Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4, &p, &y, false);
 239
 240         // filter pitch and roll Euler angles instead of fuselage vector to guarantee a unit length at all times
 241         p = FilterButterWorthDF2(p, &(imu->prefilter), &(imu->pn1), &(imu->pn2));
 242         y = FilterButterWorthDF2(y, &(imu->prefilter), &(imu->yn1), &(imu->yn2));
 243         // transform pitch and yaw into fuselage vector xB and xBold
 244         PY2xB(p, y, xB);
 245         // calculate change in fuselage vector by substraction of old value
 246         PY2DeltaxB(imu->pOld, imu->yOld, xB, dxB);
 247
 248         // filter ground speed from VelocityState
 249         const float fv1n = FilterButterWorthDF2(velData.North, &(imu->prefilter), &(imu->v1n1), &(imu->v1n2));
 250         const float fv2n = FilterButterWorthDF2(velData.East, &(imu->prefilter), &(imu->v2n1), &(imu->v2n2));
 251         const float fv3n = FilterButterWorthDF2(velData.Down, &(imu->prefilter), &(imu->v3n1), &(imu->v3n2));
 252
 253         // calculate norm of ground speed
 254         normVel2 = Sq(fv1n) + Sq(fv2n) + Sq(fv3n);
 255         // calculate norm of orientation change
 256         normDiffAttitude2 = Sq(dxB[0]) + Sq(dxB[1]) + Sq(dxB[2]);
 257         // cauclate scalar product between groundspeed change and orientation change
 258         dvdtDotdfdt = (fv1n - imu->v1Old) * dxB[0] + (fv2n - imu->v2Old) * dxB[1] + (fv3n - imu->v3Old) * dxB[2];
 259
 260         // actualise old values
 261         imu->pOld   = p;
 262         imu->yOld   = y;
 263         imu->v1Old  = fv1n;
 264         imu->v2Old  = fv2n;
 265         imu->v3Old  = fv3n;
 266     }
 267
 268     // Some reorientation needed to be able to calculate airspeed, calculate only for sufficient velocity
 269     // a negative scalar product is a clear sign that we are not really able to calculate the airspeed
 270     // NOTE: normVel2 check against EPS_VELOCITY might make problems during hovering maneuvers in fixed wings
 271     if (normDiffAttitude2 > EPS_REORIENTATION && normVel2 > EPS_VELOCITY && dvdtDotdfdt > 0.f) {
 272         // Airspeed modulus: |v| = dv/dt * dxB/dt / |dxB/dt|^2
 273         // airspeed is always REAL because  normDiffAttitude2 > EPS_REORIENTATION > 0 and REAL dvdtDotdfdt
 274         const float airspeed = dvdtDotdfdt / normDiffAttitude2;
 275
 276         // groundspeed = airspeed + wind ---> wind = groundspeed - airspeed
 277         const float wind[3]  = { imu->v1Old - xB[0] * airspeed,
 278                                  imu->v2Old - xB[1] * airspeed,
 279                                  imu->v3Old - xB[2] * airspeed };
 280         // filter raw wind
 281         imu->Vw1 = FilterButterWorthDF2(wind[0], &(imu->filter), &(imu->Vw1n1), &(imu->Vw1n2));
 282         imu->Vw2 = FilterButterWorthDF2(wind[1], &(imu->filter), &(imu->Vw2n1), &(imu->Vw2n2));
 283         imu->Vw3 = FilterButterWorthDF2(wind[2], &(imu->filter), &(imu->Vw3n1), &(imu->Vw3n2));
 284     } // else leave wind estimation unchanged
 285
 286     { // Scoping to save memory
 287       // airspeed = groundspeed - wind
 288         const float Vair[3] = {
 289             imu->v1Old - imu->Vw1,
 290             imu->v2Old - imu->Vw2,
 291             imu->v3Old - imu->Vw3
 292         };
 293
 294         // project airspeed into fuselage vector
 295         airspeedData->CalibratedAirspeed = Vair[0] * xB[0] + Vair[1] * xB[1] + Vair[2] * xB[2];
 296     }
 297
 298     airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
 299     AlarmsClear(SYSTEMALARMS_ALARM_AIRSPEED);
 300 }
 301
 302
 303 /**
 304  * @}
 305  * @}
 306  */