src/main/navigation/navigation_pos_estimator_flow.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/opflow.h"
  41
  42 #include "flight/imu.h"
  43
  44 extern navigationPosEstimator_t posEstimator;
  45
  46 #ifdef USE_OPFLOW
  47 /**
  48  * Read optical flow topic
  49  *  Function is called by OPFLOW task as soon as new update is available
  50  */
  51 void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs)
  52 {
  53     posEstimator.flow.lastUpdateTime = currentTimeUs;
  54     posEstimator.flow.isValid = opflow.isHwHealty && (opflow.flowQuality == OPFLOW_QUALITY_VALID);
  55     posEstimator.flow.flowRate[X] = opflow.flowRate[X];
  56     posEstimator.flow.flowRate[Y] = opflow.flowRate[Y];
  57     posEstimator.flow.bodyRate[X] = opflow.bodyRate[X];
  58     posEstimator.flow.bodyRate[Y] = opflow.bodyRate[Y];
  59 }
  60 #endif
  61
  62 bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx)
  63 {
  64 #if defined(USE_RANGEFINDER) && defined(USE_OPFLOW)
  65     if (!((ctx->newFlags & EST_FLOW_VALID) && (ctx->newFlags & EST_SURFACE_VALID) && (ctx->newFlags & EST_Z_VALID))) {
  66         return false;
  67     }
  68
  69     // FIXME: flow may use AGL estimate if available
  70     const bool canUseFlow = (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD);
  71
  72     if (!canUseFlow) {
  73         return false;
  74     }
  75
  76     // Calculate linear velocity based on angular velocity and altitude
  77     // Technically we should calculate arc length here, but for fast sampling this is accurate enough
  78     fpVector3_t flowVel = {
  79         .x = - (posEstimator.flow.flowRate[Y] - posEstimator.flow.bodyRate[Y]) * posEstimator.surface.alt,
  80         .y =   (posEstimator.flow.flowRate[X] - posEstimator.flow.bodyRate[X]) * posEstimator.surface.alt,
  81         .z =    posEstimator.est.vel.z
  82     };
  83
  84     // At this point flowVel will hold linear velocities in earth frame
  85     imuTransformVectorBodyToEarth(&flowVel);
  86
  87     // Calculate velocity correction
  88     const float flowVelXInnov = flowVel.x - posEstimator.est.vel.x;
  89     const float flowVelYInnov = flowVel.y - posEstimator.est.vel.y;
  90
  91     ctx->estVelCorr.x = flowVelXInnov * positionEstimationConfig()->w_xy_flow_v * ctx->dt;
  92     ctx->estVelCorr.y = flowVelYInnov * positionEstimationConfig()->w_xy_flow_v * ctx->dt;
  93
  94     // Calculate position correction if possible/allowed
  95     if ((ctx->newFlags & EST_GPS_XY_VALID)) {
  96         // If GPS is valid - reset flow estimated coordinates to GPS
  97         posEstimator.est.flowCoordinates[X] = posEstimator.gps.pos.x;
  98         posEstimator.est.flowCoordinates[Y] = posEstimator.gps.pos.y;
  99     }
 100     else if (positionEstimationConfig()->allow_dead_reckoning) {
 101         posEstimator.est.flowCoordinates[X] += flowVel.x * ctx->dt;
 102         posEstimator.est.flowCoordinates[Y] += flowVel.y * ctx->dt;
 103
 104         const float flowResidualX = posEstimator.est.flowCoordinates[X] - posEstimator.est.pos.x;
 105         const float flowResidualY = posEstimator.est.flowCoordinates[Y] - posEstimator.est.pos.y;
 106
 107         ctx->estPosCorr.x = flowResidualX * positionEstimationConfig()->w_xy_flow_p * ctx->dt;
 108         ctx->estPosCorr.y = flowResidualY * positionEstimationConfig()->w_xy_flow_p * ctx->dt;
 109
 110         ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, calc_length_pythagorean_2D(flowResidualX, flowResidualY), positionEstimationConfig()->w_xy_flow_p);
 111     }
 112
 113     DEBUG_SET(DEBUG_FLOW, 0, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[X]));
 114     DEBUG_SET(DEBUG_FLOW, 1, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[Y]));
 115     DEBUG_SET(DEBUG_FLOW, 2, posEstimator.est.flowCoordinates[X]);
 116     DEBUG_SET(DEBUG_FLOW, 3, posEstimator.est.flowCoordinates[Y]);
 117
 118     return true;
 119 #else
 120     UNUSED(ctx);
 121     return false;
 122 #endif
 123 }