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 #include "navigation/navigation.h"
  36 #include "navigation/navigation_private.h"
  37 #include "navigation/navigation_pos_estimator_private.h"
  38
  39 #include "sensors/rangefinder.h"
  40 #include "sensors/barometer.h"
  41
  42 extern navigationPosEstimator_t posEstimator;
  43
  44 #ifdef USE_RANGEFINDER
  45 /**
  46  * Read surface and update alt/vel topic
  47  *  Function is called from TASK_RANGEFINDER at arbitrary rate - as soon as new measurements are available
  48  */
  49 void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt)
  50 {
  51     const float surfaceDtUs = currentTimeUs - posEstimator.surface.lastUpdateTime;
  52     float newReliabilityMeasurement = 0;
  53     bool surfaceMeasurementWithinRange = false;
  54
  55     posEstimator.surface.lastUpdateTime = currentTimeUs;
  56
  57     if (newSurfaceAlt >= 0) {
  58         if (newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) {
  59             newReliabilityMeasurement = 1.0f;
  60             surfaceMeasurementWithinRange = true;
  61             posEstimator.surface.alt = newSurfaceAlt;
  62         }
  63         else {
  64             newReliabilityMeasurement = 0.0f;
  65         }
  66     }
  67     else {
  68         // Negative values - out of range or failed hardware
  69         newReliabilityMeasurement = 0.0f;
  70     }
  71
  72     /* Reliability is a measure of confidence of rangefinder measurement. It's increased with each valid sample and decreased with each invalid sample */
  73     if (surfaceDtUs > MS2US(INAV_SURFACE_TIMEOUT_MS)) {
  74         posEstimator.surface.reliability = 0.0f;
  75     }
  76     else {
  77         const float surfaceDt = US2S(surfaceDtUs);
  78         const float relAlpha = surfaceDt / (surfaceDt + RANGEFINDER_RELIABILITY_RC_CONSTANT);
  79         posEstimator.surface.reliability = posEstimator.surface.reliability * (1.0f - relAlpha) + newReliabilityMeasurement * relAlpha;
  80
  81         // Update average sonar altitude if range is good
  82         if (surfaceMeasurementWithinRange) {
  83             pt1FilterApply3(&posEstimator.surface.avgFilter, newSurfaceAlt, surfaceDt);
  84         }
  85     }
  86 }
  87 #endif
  88
  89 void estimationCalculateAGL(estimationContext_t * ctx)
  90 {
  91 #if defined(USE_RANGEFINDER) && defined(USE_BARO)
  92     if ((ctx->newFlags & EST_SURFACE_VALID) && (ctx->newFlags & EST_BARO_VALID)) {
  93         navAGLEstimateQuality_e newAglQuality = posEstimator.est.aglQual;
  94         bool resetSurfaceEstimate = false;
  95         switch (posEstimator.est.aglQual) {
  96             case SURFACE_QUAL_LOW:
  97                 if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
  98                     newAglQuality = SURFACE_QUAL_HIGH;
  99                     resetSurfaceEstimate = true;
 100                 }
 101                 else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
 102                     newAglQuality = SURFACE_QUAL_LOW;
 103                 }
 104                 else {
 105                     newAglQuality = SURFACE_QUAL_LOW;
 106                 }
 107                 break;
 108
 109             case SURFACE_QUAL_MID:
 110                 if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
 111                     newAglQuality = SURFACE_QUAL_HIGH;
 112                 }
 113                 else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
 114                     newAglQuality = SURFACE_QUAL_MID;
 115                 }
 116                 else {
 117                     newAglQuality = SURFACE_QUAL_LOW;
 118                 }
 119                 break;
 120
 121             case SURFACE_QUAL_HIGH:
 122                 if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
 123                     newAglQuality = SURFACE_QUAL_HIGH;
 124                 }
 125                 else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
 126                     newAglQuality = SURFACE_QUAL_MID;
 127                 }
 128                 else {
 129                     newAglQuality = SURFACE_QUAL_LOW;
 130                 }
 131                 break;
 132         }
 133
 134         posEstimator.est.aglQual = newAglQuality;
 135
 136         if (resetSurfaceEstimate) {
 137             posEstimator.est.aglAlt = pt1FilterGetLastOutput(&posEstimator.surface.avgFilter);
 138             // If we have acceptable average estimate
 139             if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
 140                 posEstimator.est.aglVel = posEstimator.est.vel.z;
 141                 posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt;
 142             }
 143             else {
 144                 posEstimator.est.aglVel = 0;
 145                 posEstimator.est.aglOffset = 0;
 146             }
 147         }
 148
 149         // Update estimate
 150         const float accWeight = navGetAccelerometerWeight();
 151         posEstimator.est.aglAlt += posEstimator.est.aglVel * ctx->dt;
 152         posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
 153         posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
 154
 155         // Apply correction
 156         if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {
 157             // Correct estimate from rangefinder
 158             const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
 159             const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 75.0f), 0.0f, 1.0f, 0.1f, 1.0f);
 160
 161             posEstimator.est.aglAlt += surfaceResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * posEstimator.surface.reliability * ctx->dt;
 162             posEstimator.est.aglVel += surfaceResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * sq(posEstimator.surface.reliability) * ctx->dt;
 163
 164             // Update estimate offset
 165             if ((posEstimator.est.aglQual == SURFACE_QUAL_HIGH) && (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv)) {
 166                 posEstimator.est.aglOffset = posEstimator.est.pos.z - pt1FilterGetLastOutput(&posEstimator.surface.avgFilter);
 167             }
 168         }
 169         else if (posEstimator.est.aglQual == SURFACE_QUAL_MID) {
 170             // Correct estimate from altitude fused from rangefinder and global altitude
 171             const float estAltResidual = (posEstimator.est.pos.z - posEstimator.est.aglOffset) - posEstimator.est.aglAlt;
 172             const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
 173             const float surfaceWeightScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f) * posEstimator.surface.reliability;
 174             const float mixedResidual = surfaceResidual * surfaceWeightScaler + estAltResidual * (1.0f - surfaceWeightScaler);
 175
 176             posEstimator.est.aglAlt += mixedResidual * positionEstimationConfig()->w_z_surface_p * ctx->dt;
 177             posEstimator.est.aglVel += mixedResidual * positionEstimationConfig()->w_z_surface_v * ctx->dt;
 178         }
 179         else {  // SURFACE_QUAL_LOW
 180             // In this case rangefinder can't be trusted - simply use global altitude
 181             posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
 182             posEstimator.est.aglVel = posEstimator.est.vel.z;
 183         }
 184     }
 185     else {
 186         posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
 187         posEstimator.est.aglVel = posEstimator.est.vel.z;
 188         posEstimator.est.aglQual = SURFACE_QUAL_LOW;
 189     }
 190
 191     DEBUG_SET(DEBUG_AGL, 0, posEstimator.surface.reliability * 1000);
 192     DEBUG_SET(DEBUG_AGL, 1, posEstimator.est.aglQual);
 193     DEBUG_SET(DEBUG_AGL, 2, posEstimator.est.aglAlt);
 194     DEBUG_SET(DEBUG_AGL, 3, posEstimator.est.aglVel);
 195
 196 #else
 197     UNUSED(ctx);
 198     posEstimator.est.aglAlt = posEstimator.est.pos.z;
 199     posEstimator.est.aglVel = posEstimator.est.vel.z;
 200     posEstimator.est.aglQual = SURFACE_QUAL_LOW;
 201 #endif
 202 }