src/main/navigation/navigation_pos_estimator_agl.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27 #include <math.h>
  28 #include <string.h>
  29
  30 #include "platform.h"
  31
  32 #include "build/build_config.h"
  33 #include "build/debug.h"
  34
  35 #if defined(USE_NAV)
  36
  37 #include "navigation/navigation.h"
  38 #include "navigation/navigation_private.h"
  39 #include "navigation/navigation_pos_estimator_private.h"
  40
  41 #include "sensors/rangefinder.h"
  42 #include "sensors/barometer.h"
  43
  44 extern navigationPosEstimator_t posEstimator;
  45
  46 #ifdef USE_RANGEFINDER
  47 /**
  48  * Read surface and update alt/vel topic
  49  *  Function is called from TASK_RANGEFINDER at arbitrary rate - as soon as new measurements are available
  50  */
  51 void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt)
  52 {
  53     const float surfaceDtUs = currentTimeUs - posEstimator.surface.lastUpdateTime;
  54     float newReliabilityMeasurement = 0;
  55     bool surfaceMeasurementWithinRange = false;
  56
  57     posEstimator.surface.lastUpdateTime = currentTimeUs;
  58
  59     if (newSurfaceAlt >= 0) {
  60         if (newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) {
  61             newReliabilityMeasurement = 1.0f;
  62             surfaceMeasurementWithinRange = true;
  63             posEstimator.surface.alt = newSurfaceAlt;
  64         }
  65         else {
  66             newReliabilityMeasurement = 0.0f;
  67         }
  68     }
  69     else {
  70         // Negative values - out of range or failed hardware
  71         newReliabilityMeasurement = 0.0f;
  72     }
  73
  74     /* Reliability is a measure of confidence of rangefinder measurement. It's increased with each valid sample and decreased with each invalid sample */
  75     if (surfaceDtUs > MS2US(INAV_SURFACE_TIMEOUT_MS)) {
  76         posEstimator.surface.reliability = 0.0f;
  77     }
  78     else {
  79         const float surfaceDt = US2S(surfaceDtUs);
  80         const float relAlpha = surfaceDt / (surfaceDt + RANGEFINDER_RELIABILITY_RC_CONSTANT);
  81         posEstimator.surface.reliability = posEstimator.surface.reliability * (1.0f - relAlpha) + newReliabilityMeasurement * relAlpha;
  82
  83         // Update average sonar altitude if range is good
  84         if (surfaceMeasurementWithinRange) {
  85             pt1FilterApply3(&posEstimator.surface.avgFilter, newSurfaceAlt, surfaceDt);
  86         }
  87     }
  88 }
  89 #endif
  90
  91 void estimationCalculateAGL(estimationContext_t * ctx)
  92 {
  93 #if defined(USE_RANGEFINDER) && defined(USE_BARO)
  94     if ((ctx->newFlags & EST_SURFACE_VALID) && (ctx->newFlags & EST_BARO_VALID)) {
  95         navAGLEstimateQuality_e newAglQuality = posEstimator.est.aglQual;
  96         bool resetSurfaceEstimate = false;
  97         switch (posEstimator.est.aglQual) {
  98             case SURFACE_QUAL_LOW:
  99                 if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
 100                     newAglQuality = SURFACE_QUAL_HIGH;
 101                     resetSurfaceEstimate = true;
 102                 }
 103                 else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
 104                     newAglQuality = SURFACE_QUAL_LOW;
 105                 }
 106                 else {
 107                     newAglQuality = SURFACE_QUAL_LOW;
 108                 }
 109                 break;
 110
 111             case SURFACE_QUAL_MID:
 112                 if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
 113                     newAglQuality = SURFACE_QUAL_HIGH;
 114                 }
 115                 else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
 116                     newAglQuality = SURFACE_QUAL_MID;
 117                 }
 118                 else {
 119                     newAglQuality = SURFACE_QUAL_LOW;
 120                 }
 121                 break;
 122
 123             case SURFACE_QUAL_HIGH:
 124                 if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
 125                     newAglQuality = SURFACE_QUAL_HIGH;
 126                 }
 127                 else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
 128                     newAglQuality = SURFACE_QUAL_MID;
 129                 }
 130                 else {
 131                     newAglQuality = SURFACE_QUAL_LOW;
 132                 }
 133                 break;
 134         }
 135
 136         posEstimator.est.aglQual = newAglQuality;
 137
 138         if (resetSurfaceEstimate) {
 139             posEstimator.est.aglAlt = pt1FilterGetLastOutput(&posEstimator.surface.avgFilter);
 140             // If we have acceptable average estimate
 141             if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
 142                 posEstimator.est.aglVel = posEstimator.est.vel.z;
 143                 posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt;
 144             }
 145             else {
 146                 posEstimator.est.aglVel = 0;
 147                 posEstimator.est.aglOffset = 0;
 148             }
 149         }
 150
 151         // Update estimate
 152         const float accWeight = navGetAccelerometerWeight();
 153         posEstimator.est.aglAlt += posEstimator.est.aglVel * ctx->dt;
 154         posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
 155         posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
 156
 157         // Apply correction
 158         if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {
 159             // Correct estimate from rangefinder
 160             const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
 161             const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 75.0f), 0.0f, 1.0f, 0.1f, 1.0f);
 162
 163             posEstimator.est.aglAlt += surfaceResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * posEstimator.surface.reliability * ctx->dt;
 164             posEstimator.est.aglVel += surfaceResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * sq(posEstimator.surface.reliability) * ctx->dt;
 165
 166             // Update estimate offset
 167             if ((posEstimator.est.aglQual == SURFACE_QUAL_HIGH) && (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv)) {
 168                 posEstimator.est.aglOffset = posEstimator.est.pos.z - pt1FilterGetLastOutput(&posEstimator.surface.avgFilter);
 169             }
 170         }
 171         else if (posEstimator.est.aglQual == SURFACE_QUAL_MID) {
 172             // Correct estimate from altitude fused from rangefinder and global altitude
 173             const float estAltResidual = (posEstimator.est.pos.z - posEstimator.est.aglOffset) - posEstimator.est.aglAlt;
 174             const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
 175             const float surfaceWeightScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f) * posEstimator.surface.reliability;
 176             const float mixedResidual = surfaceResidual * surfaceWeightScaler + estAltResidual * (1.0f - surfaceWeightScaler);
 177
 178             posEstimator.est.aglAlt += mixedResidual * positionEstimationConfig()->w_z_surface_p * ctx->dt;
 179             posEstimator.est.aglVel += mixedResidual * positionEstimationConfig()->w_z_surface_v * ctx->dt;
 180         }
 181         else {  // SURFACE_QUAL_LOW
 182             // In this case rangefinder can't be trusted - simply use global altitude
 183             posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
 184             posEstimator.est.aglVel = posEstimator.est.vel.z;
 185         }
 186     }
 187     else {
 188         posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
 189         posEstimator.est.aglVel = posEstimator.est.vel.z;
 190         posEstimator.est.aglQual = SURFACE_QUAL_LOW;
 191     }
 192
 193     DEBUG_SET(DEBUG_AGL, 0, posEstimator.surface.reliability * 1000);
 194     DEBUG_SET(DEBUG_AGL, 1, posEstimator.est.aglQual);
 195     DEBUG_SET(DEBUG_AGL, 2, posEstimator.est.aglAlt);
 196     DEBUG_SET(DEBUG_AGL, 3, posEstimator.est.aglVel);
 197
 198 #else
 199     UNUSED(ctx);
 200     posEstimator.est.aglAlt = posEstimator.est.pos.z;
 201     posEstimator.est.aglVel = posEstimator.est.vel.z;
 202     posEstimator.est.aglQual = SURFACE_QUAL_LOW;
 203 #endif
 204 }
 205
 206 #endif  // NAV