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