src/main/flight/secondary_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 FILE_COMPILE_FOR_SPEED
  30
  31 #include <stdint.h>
  32 #include "secondary_dynamic_gyro_notch.h"
  33 #include "fc/config.h"
  34 #include "build/debug.h"
  35 #include "sensors/gyro.h"
  36
  37 #define SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 150
  38
  39 void secondaryDynamicGyroNotchFiltersInit(secondaryDynamicGyroNotchState_t *state) {
  40
  41     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
  42         state->filtersApplyFn[axis] = nullFilterApply;
  43     }
  44
  45     state->dynNotchQ = gyroConfig()->dynamicGyroNotch3dQ / 100.0f;
  46     state->enabled = gyroConfig()->dynamicGyroNotchMode != DYNAMIC_NOTCH_MODE_2D;
  47     state->looptime = getLooptime();
  48
  49     if (
  50         gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_R ||
  51         gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RP ||
  52         gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RY ||
  53         gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D
  54     ) {
  55         /*
  56          * Enable ROLL filter
  57          */
  58         biquadFilterInit(&state->filters[FD_ROLL], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
  59         state->filtersApplyFn[FD_ROLL] = (filterApplyFnPtr)biquadFilterApplyDF1;
  60     }
  61
  62     if (
  63         gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_P ||
  64         gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RP ||
  65         gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_PY ||
  66         gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D
  67     ) {
  68         /*
  69          * Enable PITCH filter
  70          */
  71         biquadFilterInit(&state->filters[FD_PITCH], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
  72         state->filtersApplyFn[FD_PITCH] = (filterApplyFnPtr)biquadFilterApplyDF1;
  73     }
  74
  75     if (
  76         gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_Y ||
  77         gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_PY ||
  78         gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RY ||
  79         gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D
  80     ) {
  81         /*
  82          * Enable YAW filter
  83          */
  84         biquadFilterInit(&state->filters[FD_YAW], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
  85         state->filtersApplyFn[FD_YAW] = (filterApplyFnPtr)biquadFilterApplyDF1;
  86     }
  87
  88
  89 }
  90
  91 void secondaryDynamicGyroNotchFiltersUpdate(secondaryDynamicGyroNotchState_t *state, int axis, float frequency[]) {
  92
  93     if (state->enabled) {
  94
  95         /*
  96          * The secondary dynamic notch uses only the first detected frequency
  97          * The rest of peaks are ignored
  98          */
  99         state->frequency[axis] = frequency[0];
 100
 101         // Filter update happens only if peak was detected
 102         if (frequency[0] > 0.0f) {
 103             biquadFilterUpdate(&state->filters[axis], state->frequency[axis], state->looptime, state->dynNotchQ, FILTER_NOTCH);
 104         }
 105     }
 106 }
 107
 108 float secondaryDynamicGyroNotchFiltersApply(secondaryDynamicGyroNotchState_t *state, int axis, float input) {
 109     return state->filtersApplyFn[axis]((filter_t *)&state->filters[axis], input);
 110 }
 111
 112 #endif