src/main/flight/wind_estimator.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include "platform.h"
  19
  20 #if defined(USE_WIND_ESTIMATOR)
  21
  22 #include <stdbool.h>
  23 #include <stdint.h>
  24 #include <string.h>
  25 #include <math.h>
  26
  27 #include "build/build_config.h"
  28 #include "build/debug.h"
  29
  30 #include "common/axis.h"
  31 #include "common/filter.h"
  32 #include "common/maths.h"
  33
  34 #include "drivers/time.h"
  35
  36 #include "fc/config.h"
  37 #include "fc/runtime_config.h"
  38
  39 #include "flight/imu.h"
  40
  41 #include "navigation/navigation_pos_estimator_private.h"
  42
  43 #include "io/gps.h"
  44
  45
  46 #define WINDESTIMATOR_TIMEOUT       60*15 // 15min with out altitude change
  47 #define WINDESTIMATOR_ALTITUDE_SCALE WINDESTIMATOR_TIMEOUT/500.0f //or 500m altitude change
  48 // Based on WindEstimation.pdf paper
  49
  50 static bool hasValidWindEstimate = false;
  51 static float estimatedWind[XYZ_AXIS_COUNT] = {0, 0, 0};    // wind velocity vectors in cm / sec in earth frame
  52 static float lastGroundVelocity[XYZ_AXIS_COUNT];
  53 static float lastFuselageDirection[XYZ_AXIS_COUNT];
  54
  55 bool isEstimatedWindSpeedValid(void)
  56 {
  57     return hasValidWindEstimate
  58 #ifdef USE_GPS_FIX_ESTIMATION
  59         || STATE(GPS_ESTIMATED_FIX)  //use any wind estimate with GPS fix estimation.
  60 #endif
  61         ;
  62 }
  63
  64 float getEstimatedWindSpeed(int axis)
  65 {
  66     return estimatedWind[axis];
  67 }
  68
  69 float getEstimatedHorizontalWindSpeed(uint16_t *angle)
  70 {
  71     float xWindSpeed = getEstimatedWindSpeed(X);
  72     float yWindSpeed = getEstimatedWindSpeed(Y);
  73     if (angle) {
  74         float horizontalWindAngle = atan2_approx(yWindSpeed, xWindSpeed);
  75         // atan2 returns [-M_PI, M_PI], with 0 indicating the vector points in the X direction
  76         // We want [0, 360) in degrees
  77         if (horizontalWindAngle < 0) {
  78             horizontalWindAngle += 2 * M_PIf;
  79         }
  80         *angle = RADIANS_TO_CENTIDEGREES(horizontalWindAngle);
  81     }
  82     return calc_length_pythagorean_2D(xWindSpeed, yWindSpeed);
  83 }
  84
  85 void updateWindEstimator(timeUs_t currentTimeUs)
  86 {
  87     static timeUs_t lastUpdateUs = 0;
  88     static timeUs_t lastValidWindEstimate = 0;
  89     static float lastValidEstimateAltitude = 0.0f;
  90     float currentAltitude = gpsSol.llh.alt / 100.0f; // altitude in m
  91
  92     if ((US2S(currentTimeUs - lastValidWindEstimate) + WINDESTIMATOR_ALTITUDE_SCALE * fabsf(currentAltitude - lastValidEstimateAltitude)) > WINDESTIMATOR_TIMEOUT) {
  93         hasValidWindEstimate = false;
  94     }
  95
  96     if (!STATE(FIXED_WING_LEGACY) ||
  97         !isGPSHeadingValid() ||
  98         !gpsSol.flags.validVelNE ||
  99         !gpsSol.flags.validVelD
 100 #ifdef USE_GPS_FIX_ESTIMATION
 101             || STATE(GPS_ESTIMATED_FIX)
 102 #endif
 103             ) {
 104         return;
 105     }
 106
 107     float groundVelocity[XYZ_AXIS_COUNT];
 108     float groundVelocityDiff[XYZ_AXIS_COUNT];
 109     float groundVelocitySum[XYZ_AXIS_COUNT];
 110
 111     float fuselageDirection[XYZ_AXIS_COUNT];
 112     float fuselageDirectionDiff[XYZ_AXIS_COUNT];
 113     float fuselageDirectionSum[XYZ_AXIS_COUNT];
 114
 115     // Get current 3D velocity from GPS in cm/s
 116     // relative to earth frame
 117     groundVelocity[X] = posEstimator.gps.vel.x;
 118     groundVelocity[Y] = posEstimator.gps.vel.y;
 119     groundVelocity[Z] = posEstimator.gps.vel.z;
 120
 121     // Fuselage direction in earth frame
 122     fuselageDirection[X] = HeadVecEFFiltered.x;
 123     fuselageDirection[Y] = -HeadVecEFFiltered.y;
 124     fuselageDirection[Z] = -HeadVecEFFiltered.z;
 125
 126     timeDelta_t timeDelta = cmpTimeUs(currentTimeUs, lastUpdateUs);
 127     // scrap our data and start over if we're taking too long to get a direction change
 128     if (lastUpdateUs == 0 || timeDelta > 10 * USECS_PER_SEC) {
 129
 130         lastUpdateUs = currentTimeUs;
 131
 132         memcpy(lastFuselageDirection, fuselageDirection, sizeof(lastFuselageDirection));
 133         memcpy(lastGroundVelocity, groundVelocity, sizeof(lastGroundVelocity));
 134         return;
 135     }
 136
 137     fuselageDirectionDiff[X] = fuselageDirection[X] - lastFuselageDirection[X];
 138     fuselageDirectionDiff[Y] = fuselageDirection[Y] - lastFuselageDirection[Y];
 139     fuselageDirectionDiff[Z] = fuselageDirection[Z] - lastFuselageDirection[Z];
 140
 141     float diffLengthSq = sq(fuselageDirectionDiff[X]) + sq(fuselageDirectionDiff[Y]) + sq(fuselageDirectionDiff[Z]);
 142
 143     // Very small changes in attitude will result in a denominator
 144     // very close to zero which will introduce too much error in the
 145     // estimation.
 146     //
 147     // TODO: Is 0.2f an adequate threshold?
 148     if (diffLengthSq > sq(0.2f)) {
 149         // when turning, use the attitude response to estimate wind speed
 150         groundVelocityDiff[X] = groundVelocity[X] - lastGroundVelocity[X];
 151         groundVelocityDiff[Y] = groundVelocity[Y] - lastGroundVelocity[Y];
 152         groundVelocityDiff[Z] = groundVelocity[Z] - lastGroundVelocity[Z];
 153
 154         // estimate airspeed it using equation 6
 155         float V = (calc_length_pythagorean_3D(groundVelocityDiff[X], groundVelocityDiff[Y], groundVelocityDiff[Z])) / fast_fsqrtf(diffLengthSq);
 156
 157         fuselageDirectionSum[X] = fuselageDirection[X] + lastFuselageDirection[X];
 158         fuselageDirectionSum[Y] = fuselageDirection[Y] + lastFuselageDirection[Y];
 159         fuselageDirectionSum[Z] = fuselageDirection[Z] + lastFuselageDirection[Z];
 160
 161         groundVelocitySum[X] = groundVelocity[X] + lastGroundVelocity[X];
 162         groundVelocitySum[Y] = groundVelocity[Y] + lastGroundVelocity[Y];
 163         groundVelocitySum[Z] = groundVelocity[Z] + lastGroundVelocity[Z];
 164
 165         memcpy(lastFuselageDirection, fuselageDirection, sizeof(lastFuselageDirection));
 166         memcpy(lastGroundVelocity, groundVelocity, sizeof(lastGroundVelocity));
 167
 168         float theta = atan2f(groundVelocityDiff[Y], groundVelocityDiff[X]) - atan2f(fuselageDirectionDiff[Y], fuselageDirectionDiff[X]);// equation 9
 169         float sintheta = sinf(theta);
 170         float costheta = cosf(theta);
 171
 172         float wind[XYZ_AXIS_COUNT];
 173         wind[X] = (groundVelocitySum[X] - V * (costheta * fuselageDirectionSum[X] - sintheta * fuselageDirectionSum[Y])) * 0.5f;// equation 10
 174         wind[Y] = (groundVelocitySum[Y] - V * (sintheta * fuselageDirectionSum[X] + costheta * fuselageDirectionSum[Y])) * 0.5f;// equation 11
 175         wind[Z] = (groundVelocitySum[Z] - V * fuselageDirectionSum[Z]) * 0.5f;// equation 12
 176
 177         float prevWindLength = calc_length_pythagorean_3D(estimatedWind[X], estimatedWind[Y], estimatedWind[Z]);
 178         float windLength = calc_length_pythagorean_3D(wind[X], wind[Y], wind[Z]);
 179
 180         //is this really needed? The reason it is here might be above equation was wrong in early implementations
 181         if (windLength < prevWindLength + 4000) {
 182             // TODO: Better filtering
 183             estimatedWind[X] = estimatedWind[X] * 0.98f + wind[X] * 0.02f;
 184             estimatedWind[Y] = estimatedWind[Y] * 0.98f + wind[Y] * 0.02f;
 185             estimatedWind[Z] = estimatedWind[Z] * 0.98f + wind[Z] * 0.02f;
 186         }
 187
 188         lastUpdateUs = currentTimeUs;
 189         lastValidWindEstimate = currentTimeUs;
 190         hasValidWindEstimate = true;
 191         lastValidEstimateAltitude = currentAltitude;
 192     }
 193 }
 194
 195 #endif