src/main/flight/dynamic_gyro_notch.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 "platform.h"
  26
  27 #ifdef USE_DYNAMIC_FILTERS
  28
  29 #include <stdint.h>
  30 #include "dynamic_gyro_notch.h"
  31 #include "fc/config.h"
  32 #include "build/debug.h"
  33 #include "sensors/gyro.h"
  34
  35 void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) {
  36     state->filtersApplyFn = nullFilterApply;
  37
  38     state->dynNotchQ = gyroConfig()->dynamicGyroNotchQ / 100.0f;
  39     state->enabled = gyroConfig()->dynamicGyroNotchEnabled;
  40     state->looptime = getLooptime();
  41
  42     if (state->enabled) {
  43         /*
  44          * Step 1 - init all filters even if they will not be used further down the road
  45          */
  46         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
  47             //Any initial notch Q is valid sice it will be updated immediately after
  48             biquadFilterInit(&state->filters[axis][0], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
  49             biquadFilterInit(&state->filters[axis][1], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
  50             biquadFilterInit(&state->filters[axis][2], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
  51         }
  52
  53         state->filtersApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
  54     }
  55 }
  56
  57 void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, uint16_t frequency) {
  58
  59     state->frequency[axis] = frequency;
  60
  61     DEBUG_SET(DEBUG_DYNAMIC_FILTER_FREQUENCY, axis, frequency);
  62
  63     if (state->enabled) {
  64         biquadFilterUpdate(&state->filters[0][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH);
  65         biquadFilterUpdate(&state->filters[1][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH);
  66         biquadFilterUpdate(&state->filters[2][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH);
  67     }
  68
  69 }
  70
  71 float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, float input) {
  72     float output = input;
  73
  74     /*
  75      * We always apply all filters. If a filter dimension is disabled, one of
  76      * the function pointers will be a null apply function
  77      */
  78     output = state->filtersApplyFn((filter_t *)&state->filters[axis][0], output);
  79     output = state->filtersApplyFn((filter_t *)&state->filters[axis][1], output);
  80     output = state->filtersApplyFn((filter_t *)&state->filters[axis][2], output);
  81
  82     return output;
  83 }
  84
  85 #endif