src/main/flight/rpm_filter.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 #include "flight/rpm_filter.h"
  28
  29 #include "config/parameter_group.h"
  30 #include "config/parameter_group_ids.h"
  31
  32 #include "build/debug.h"
  33
  34 #include "common/axis.h"
  35 #include "common/utils.h"
  36 #include "common/maths.h"
  37 #include "common/filter.h"
  38 #include "flight/mixer.h"
  39 #include "sensors/esc_sensor.h"
  40 #include "fc/config.h"
  41 #include "fc/settings.h"
  42
  43 #ifdef USE_RPM_FILTER
  44
  45 #define HZ_TO_RPM 1/60.0f
  46 #define RPM_FILTER_RPM_LPF_HZ 150
  47 #define RPM_FILTER_HARMONICS 3
  48
  49 PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 1);
  50
  51 PG_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig,
  52                   .gyro_filter_enabled = SETTING_RPM_GYRO_FILTER_ENABLED_DEFAULT,
  53                   .gyro_harmonics = SETTING_RPM_GYRO_HARMONICS_DEFAULT,
  54                   .gyro_min_hz = SETTING_RPM_GYRO_MIN_HZ_DEFAULT,
  55                   .gyro_q = SETTING_RPM_GYRO_Q_DEFAULT, );
  56
  57 typedef struct
  58 {
  59     float q;
  60     float minHz;
  61     float maxHz;
  62     uint8_t harmonics;
  63     biquadFilter_t filters[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_HARMONICS];
  64 } rpmFilterBank_t;
  65
  66 typedef float (*rpmFilterApplyFnPtr)(rpmFilterBank_t *filter, uint8_t axis, float input);
  67 typedef void (*rpmFilterUpdateFnPtr)(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency);
  68
  69 static EXTENDED_FASTRAM pt1Filter_t motorFrequencyFilter[MAX_SUPPORTED_MOTORS];
  70 static EXTENDED_FASTRAM rpmFilterBank_t gyroRpmFilters;
  71 static EXTENDED_FASTRAM rpmFilterApplyFnPtr rpmGyroApplyFn;
  72 static EXTENDED_FASTRAM rpmFilterUpdateFnPtr rpmGyroUpdateFn;
  73
  74 float nullRpmFilterApply(rpmFilterBank_t *filter, uint8_t axis, float input)
  75 {
  76     UNUSED(filter);
  77     UNUSED(axis);
  78     return input;
  79 }
  80
  81 void nullRpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency) {
  82     UNUSED(filterBank);
  83     UNUSED(motor);
  84     UNUSED(baseFrequency);
  85 }
  86
  87 float rpmFilterApply(rpmFilterBank_t *filterBank, uint8_t axis, float input)
  88 {
  89     float output = input;
  90
  91     for (uint8_t motor = 0; motor < getMotorCount(); motor++)
  92     {
  93         for (int harmonicIndex = 0; harmonicIndex < filterBank->harmonics; harmonicIndex++)
  94         {
  95             output = biquadFilterApplyDF1(
  96                 &filterBank->filters[axis][motor][harmonicIndex],
  97                 output
  98             );
  99         }
 100     }
 101
 102     return output;
 103 }
 104
 105 static void rpmFilterInit(rpmFilterBank_t *filter, uint16_t q, uint8_t minHz, uint8_t harmonics)
 106 {
 107     filter->q = q / 100.0f;
 108     filter->minHz = minHz;
 109     filter->harmonics = harmonics;
 110     /*
 111      * Max frequency has to be lower than Nyquist frequency for looptime
 112      */
 113     filter->maxHz = 0.48f * 1000000.0f / getLooptime();
 114
 115     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
 116     {
 117         for (int motor = 0; motor < getMotorCount(); motor++)
 118         {
 119
 120             /*
 121              * Harmonics are indexed from 1 where 1 means base frequency
 122              * C indexes arrays from 0, so we need to shift
 123              */
 124             for (int harmonicIndex = 0; harmonicIndex < harmonics; harmonicIndex++)
 125             {
 126                 biquadFilterInit(
 127                     &filter->filters[axis][motor][harmonicIndex],
 128                     filter->minHz * (harmonicIndex + 1),
 129                     getLooptime(),
 130                     filter->q,
 131                     FILTER_NOTCH);
 132             }
 133         }
 134     }
 135 }
 136
 137 void disableRpmFilters(void) {
 138     rpmGyroApplyFn = (rpmFilterApplyFnPtr)nullRpmFilterApply;
 139 }
 140
 141 void rpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency)
 142 {
 143     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
 144     {
 145         for (int harmonicIndex = 0; harmonicIndex < filterBank->harmonics; harmonicIndex++)
 146         {
 147             float harmonicFrequency = baseFrequency * (harmonicIndex + 1);
 148             harmonicFrequency = constrainf(harmonicFrequency, filterBank->minHz, filterBank->maxHz);
 149
 150             biquadFilterUpdate(
 151                 &filterBank->filters[axis][motor][harmonicIndex],
 152                 harmonicFrequency,
 153                 getLooptime(),
 154                 filterBank->q,
 155                 FILTER_NOTCH);
 156         }
 157     }
 158 }
 159
 160 void rpmFiltersInit(void)
 161 {
 162     for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++)
 163     {
 164         pt1FilterInit(&motorFrequencyFilter[i], RPM_FILTER_RPM_LPF_HZ, US2S(RPM_FILTER_UPDATE_RATE_US));
 165     }
 166
 167     rpmGyroUpdateFn = (rpmFilterUpdateFnPtr)nullRpmFilterUpdate;
 168
 169     if (rpmFilterConfig()->gyro_filter_enabled)
 170     {
 171         rpmFilterInit(
 172             &gyroRpmFilters,
 173             rpmFilterConfig()->gyro_q,
 174             rpmFilterConfig()->gyro_min_hz,
 175             rpmFilterConfig()->gyro_harmonics);
 176         rpmGyroApplyFn = (rpmFilterApplyFnPtr)rpmFilterApply;
 177         rpmGyroUpdateFn = (rpmFilterUpdateFnPtr)rpmFilterUpdate;
 178     }
 179 }
 180
 181 void rpmFilterUpdateTask(timeUs_t currentTimeUs)
 182 {
 183     UNUSED(currentTimeUs);
 184
 185     uint8_t motorCount = getMotorCount();
 186     /*
 187      * For each motor, read ERPM, filter it and update motor frequency
 188      */
 189     for (uint8_t i = 0; i < motorCount; i++)
 190     {
 191         const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry
 192         const float baseFrequency = pt1FilterApply(&motorFrequencyFilter[i], escState->rpm * HZ_TO_RPM); //Filter motor frequency
 193
 194         rpmGyroUpdateFn(&gyroRpmFilters, i, baseFrequency);
 195     }
 196 }
 197
 198 float rpmFilterGyroApply(uint8_t axis, float input)
 199 {
 200     return rpmGyroApplyFn(&gyroRpmFilters, axis, input);
 201 }
 202
 203 #endif