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 // RC Interpolation is not allowed to go below this value.
  51 #define RC_INTERPOLATION_MIN_FREQUENCY 15
  52
  53 static pt3Filter_t rcSmoothFilter[4];
  54 static float rcStickUnfiltered[4];
  55 static uint16_t rcUpdateFrequency;
  56
  57 uint16_t getRcUpdateFrequency(void) {
  58     return rcUpdateFrequency;
  59 }
  60
  61 static int32_t applyRcUpdateFrequencyMedianFilter(int32_t newReading)
  62 {
  63     #define RC_FILTER_SAMPLES_MEDIAN 9
  64     static int32_t filterSamples[RC_FILTER_SAMPLES_MEDIAN];
  65     static int filterSampleIndex = 0;
  66     static bool medianFilterReady = false;
  67
  68     filterSamples[filterSampleIndex] = newReading;
  69     ++filterSampleIndex;
  70     if (filterSampleIndex == RC_FILTER_SAMPLES_MEDIAN) {
  71         filterSampleIndex = 0;
  72         medianFilterReady = true;
  73     }
  74
  75     return medianFilterReady ? quickMedianFilter9(filterSamples) : newReading;
  76 }
  77
  78 void rcInterpolationApply(bool isRXDataNew, timeUs_t currentTimeUs)
  79 {
  80     // Compute the RC update frequency
  81     static timeUs_t previousRcData;
  82     static int filterFrequency;
  83     static bool initDone = false;
  84
  85     const float dT = US2S(getLooptime());
  86
  87     if (isRXDataNew) {
  88         if (!initDone) {
  89
  90             filterFrequency = rxConfig()->rcFilterFrequency;
  91
  92             // Initialize the RC smooth filter
  93             for (int stick = 0; stick < 4; stick++) {
  94                 pt3FilterInit(&rcSmoothFilter[stick], pt3FilterGain(filterFrequency, dT));
  95             }
  96
  97             initDone = true;
  98         }
  99
 100         for (int stick = 0; stick < 4; stick++) {
 101             rcStickUnfiltered[stick] = rcCommand[stick];
 102         }
 103     }
 104
 105     // Don't filter if not initialized
 106     if (!initDone) {
 107         return;
 108     }
 109
 110     if (isRXDataNew) {
 111         const timeDelta_t delta = cmpTimeUs(currentTimeUs, previousRcData);
 112         rcUpdateFrequency = applyRcUpdateFrequencyMedianFilter(1.0f / (delta * 0.000001f));
 113         previousRcData = currentTimeUs;
 114
 115         /*
 116          * If auto smoothing is enabled, update the filters
 117          */
 118         if (rxConfig()->autoSmooth) {
 119             const int nyquist = rcUpdateFrequency / 2;
 120
 121             int newFilterFrequency = scaleRange(
 122                 rxConfig()->autoSmoothFactor,
 123                 1,
 124                 100,
 125                 nyquist,
 126                 rcUpdateFrequency / 10
 127             );
 128
 129             // Do not allow filter frequency to go below RC_INTERPOLATION_MIN_FREQUENCY or above nuyquist frequency.
 130             newFilterFrequency = constrain(newFilterFrequency, RC_INTERPOLATION_MIN_FREQUENCY, nyquist);
 131
 132             if (newFilterFrequency != filterFrequency) {
 133
 134                 for (int stick = 0; stick < 4; stick++) {
 135                     pt3FilterUpdateCutoff(&rcSmoothFilter[stick], pt3FilterGain(newFilterFrequency, dT));
 136                 }
 137                 filterFrequency = newFilterFrequency;
 138             }
 139         }
 140
 141     }
 142
 143     for (int stick = 0; stick < 4; stick++) {
 144         rcCommand[stick] = pt3FilterApply(&rcSmoothFilter[stick], rcStickUnfiltered[stick]);
 145     }
 146 }