src/main/fc/rc_smoothing.c

   1 /*
   2  * This file is part of INAV.
   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 <stdlib.h>
  27 #include <stdint.h>
  28
  29 #include "platform.h"
  30
  31 #include "blackbox/blackbox.h"
  32
  33 #include "build/debug.h"
  34
  35 #include "common/maths.h"
  36 #include "common/filter.h"
  37
  38 #include "drivers/time.h"
  39
  40 #include "rx/rx.h"
  41
  42 #include "fc/config.h"
  43 #include "fc/fc_core.h"
  44 #include "fc/rc_controls.h"
  45 #include "fc/rc_smoothing.h"
  46 #include "fc/runtime_config.h"
  47
  48 #include "flight/mixer.h"
  49
  50 static biquadFilter_t rcSmoothFilter[4];
  51 static float rcStickUnfiltered[4];
  52
  53 static void rcInterpolationInit(int rcFilterFreqency)
  54 {
  55     for (int stick = 0; stick < 4; stick++) {
  56         biquadFilterInitLPF(&rcSmoothFilter[stick], rcFilterFreqency, getLooptime());
  57     }
  58 }
  59
  60 void rcInterpolationApply(bool isRXDataNew)
  61 {
  62     static bool initDone = false;
  63     static float initFilterFreqency = 0;
  64
  65     if (isRXDataNew) {
  66         if (!initDone || (initFilterFreqency != rxConfig()->rcFilterFrequency)) {
  67             rcInterpolationInit(rxConfig()->rcFilterFrequency);
  68             initFilterFreqency = rxConfig()->rcFilterFrequency;
  69             initDone = true;
  70         }
  71
  72         for (int stick = 0; stick < 4; stick++) {
  73             rcStickUnfiltered[stick] = rcCommand[stick];
  74         }
  75     }
  76
  77     // Don't filter if not initialized
  78     if (!initDone) {
  79         return;
  80     }
  81
  82     for (int stick = 0; stick < 4; stick++) {
  83         rcCommand[stick] = biquadFilterApply(&rcSmoothFilter[stick], rcStickUnfiltered[stick]);
  84     }
  85 }
  86
  87
  88
  89
  90
  91
  92
  93
  94
  95
  96
  97
  98
  99
 100
 101
 102
 103
 104
 105
 106
 107
 108
 109
 110