src/main/flight/smith_predictor.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  * This code is a derivative of work done in EmuFlight Project https://github.com/emuflight/EmuFlight
  25  *
  26  */
  27
  28 #include "platform.h"
  29 #ifdef USE_SMITH_PREDICTOR
  30
  31 #include <stdbool.h>
  32 #include "common/axis.h"
  33 #include "common/utils.h"
  34 #include "flight/smith_predictor.h"
  35 #include "build/debug.h"
  36
  37 float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample) {
  38     UNUSED(axis);
  39     if (predictor->enabled) {
  40         predictor->data[predictor->idx] = sample;
  41
  42         predictor->idx++;
  43         if (predictor->idx > predictor->samples) {
  44             predictor->idx = 0;
  45         }
  46
  47         // filter the delayed data to help reduce the overall noise this prediction adds
  48         float delayed = pt1FilterApply(&predictor->smithPredictorFilter, predictor->data[predictor->idx]);
  49         float delayCompensatedSample = predictor->smithPredictorStrength * (sample - delayed);
  50
  51         sample += delayCompensatedSample;
  52     }
  53     return sample;
  54 }
  55
  56 void smithPredictorInit(smithPredictor_t *predictor, float delay, float strength, uint16_t filterLpfHz, uint32_t looptime) {
  57     if (delay > 0.1f) {
  58         predictor->enabled = true;
  59         predictor->samples = (delay * 1000) / looptime;
  60         predictor->idx = 0;
  61         predictor->smithPredictorStrength = strength;
  62         pt1FilterInit(&predictor->smithPredictorFilter, filterLpfHz, US2S(looptime));
  63     } else {
  64         predictor->enabled = false;
  65     }
  66 }
  67
  68 #endif