src/main/flight/dynamic_lpf.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 "flight/dynamic_lpf.h"
  26 #include "sensors/gyro.h"
  27 #include "flight/mixer.h"
  28 #include "fc/rc_controls.h"
  29 #include "build/debug.h"
  30
  31 static float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo) {
  32     const float expof = expo / 10.0f;
  33     static float curve;
  34     curve = throttle * (1 - throttle) * expof + throttle;
  35     return (dynLpfMax - dynLpfMin) * curve + dynLpfMin;
  36 }
  37
  38 void dynamicLpfGyroTask(void) {
  39
  40     if (gyroConfig()->gyroFilterMode != GYRO_FILTER_MODE_DYNAMIC) {
  41         return;
  42     }
  43
  44     const float throttleConstrained = (float) constrain(rcCommand[THROTTLE], getThrottleIdleValue(), getMaxThrottle());
  45     const float throttle = scaleRangef(throttleConstrained, getThrottleIdleValue(), getMaxThrottle(), 0.0f, 1.0f);
  46     const float cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfCurveExpo);
  47
  48     DEBUG_SET(DEBUG_DYNAMIC_GYRO_LPF, 0, cutoffFreq);
  49
  50     gyroUpdateDynamicLpf(cutoffFreq);
  51 }