src/main/flight/rpm_filter.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21
  22 #include <float.h>
  23 #include <math.h>
  24
  25 #include "platform.h"
  26
  27 #ifdef USE_RPM_FILTER
  28
  29 #include "build/debug.h"
  30
  31 #include "common/filter.h"
  32 #include "common/maths.h"
  33
  34 #include "drivers/dshot.h"
  35
  36 #include "flight/mixer.h"
  37 #include "flight/pid.h"
  38
  39 #include "pg/motor.h"
  40
  41 #include "scheduler/scheduler.h"
  42
  43 #include "sensors/gyro.h"
  44
  45 #include "rpm_filter.h"
  46
  47 #define RPM_FILTER_HARMONICS_MAX 3
  48 #define RPM_FILTER_DURATION_S    0.001f  // Maximum duration allowed to update all RPM notches once
  49 #define SECONDS_PER_MINUTE       60.0f
  50 #define ERPM_PER_LSB             100.0f
  51
  52
  53 typedef struct rpmFilter_s {
  54
  55     int numHarmonics;
  56     float minHz;
  57     float maxHz;
  58     float fadeRangeHz;
  59     float q;
  60
  61     timeUs_t looptimeUs;
  62     biquadFilter_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_HARMONICS_MAX];
  63
  64 } rpmFilter_t;
  65
  66 // Singleton
  67 FAST_DATA_ZERO_INIT static rpmFilter_t rpmFilter;
  68
  69 FAST_DATA_ZERO_INIT static pt1Filter_t motorFreqLpf[MAX_SUPPORTED_MOTORS];
  70 FAST_DATA_ZERO_INIT static float motorFrequencyHz[MAX_SUPPORTED_MOTORS];
  71 FAST_DATA_ZERO_INIT static float minMotorFrequencyHz;
  72 FAST_DATA_ZERO_INIT static float erpmToHz;
  73
  74 // batch processing of RPM notches
  75 FAST_DATA_ZERO_INIT static int notchUpdatesPerIteration;
  76 FAST_DATA_ZERO_INIT static int motorIndex;
  77 FAST_DATA_ZERO_INIT static int harmonicIndex;
  78
  79
  80 void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs)
  81 {
  82     motorIndex = 0;
  83     harmonicIndex = 0;
  84     minMotorFrequencyHz = 0;
  85     rpmFilter.numHarmonics = 0; // disable RPM Filtering
  86
  87     // if bidirectional DShot is not available
  88     if (!motorConfig()->dev.useDshotTelemetry) {
  89         return;
  90     }
  91
  92     // init LPFs for RPM data
  93     for (int i = 0; i < getMotorCount(); i++) {
  94         pt1FilterInit(&motorFreqLpf[i], pt1FilterGain(config->rpm_filter_lpf_hz, looptimeUs * 1e-6f));
  95     }
  96
  97     // if RPM Filtering is configured to be off
  98     if (!config->rpm_filter_harmonics) {
  99         return;
 100     }
 101
 102     // if we get to this point, enable and init RPM filtering
 103     rpmFilter.numHarmonics = config->rpm_filter_harmonics;
 104     rpmFilter.minHz = config->rpm_filter_min_hz;
 105     rpmFilter.maxHz = 0.48f * 1e6f / looptimeUs; // don't go quite to nyquist to avoid oscillations
 106     rpmFilter.fadeRangeHz = config->rpm_filter_fade_range_hz;
 107     rpmFilter.q = config->rpm_filter_q / 100.0f;
 108     rpmFilter.looptimeUs = looptimeUs;
 109
 110     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 111         for (int motor = 0; motor < getMotorCount(); motor++) {
 112             for (int i = 0; i < rpmFilter.numHarmonics; i++) {
 113                 biquadFilterInit(&rpmFilter.notch[axis][motor][i], rpmFilter.minHz * i, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, 0.0f);
 114             }
 115         }
 116     }
 117
 118     erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE  / (motorConfig()->motorPoleCount / 2.0f);
 119
 120     const float loopIterationsPerUpdate = RPM_FILTER_DURATION_S / (looptimeUs * 1e-6f);
 121     const float numNotchesPerAxis = getMotorCount() * rpmFilter.numHarmonics;
 122     notchUpdatesPerIteration = ceilf(numNotchesPerAxis / loopIterationsPerUpdate); // round to ceiling
 123 }
 124
 125 FAST_CODE_NOINLINE void rpmFilterUpdate(void)
 126 {
 127     if (!motorConfig()->dev.useDshotTelemetry) {
 128         return;
 129     }
 130
 131     // update motor RPM data
 132     minMotorFrequencyHz = FLT_MAX;
 133     for (int motor = 0; motor < getMotorCount(); motor++) {
 134         motorFrequencyHz[motor] = pt1FilterApply(&motorFreqLpf[motor], erpmToHz * getDshotTelemetry(motor));
 135         minMotorFrequencyHz = MIN(minMotorFrequencyHz, motorFrequencyHz[motor]);
 136         if (motor < 4) {
 137             DEBUG_SET(DEBUG_RPM_FILTER, motor, motorFrequencyHz[motor]);
 138         }
 139     }
 140
 141     if (!isRpmFilterEnabled()) {
 142         return;
 143     }
 144
 145     // update RPM notches
 146     for (int i = 0; i < notchUpdatesPerIteration; i++) {
 147
 148         // select current notch on ROLL
 149         biquadFilter_t *template = &rpmFilter.notch[0][motorIndex][harmonicIndex];
 150
 151         const float frequencyHz = constrainf((harmonicIndex + 1) * motorFrequencyHz[motorIndex], rpmFilter.minHz, rpmFilter.maxHz);
 152         const float marginHz = frequencyHz - rpmFilter.minHz;
 153
 154         // fade out notch when approaching minHz (turn it off)
 155         float weight = 1.0f;
 156         if (marginHz < rpmFilter.fadeRangeHz) {
 157             weight = marginHz / rpmFilter.fadeRangeHz;
 158         }
 159
 160         // update notch
 161         biquadFilterUpdate(template, frequencyHz, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, weight);
 162
 163         // copy notch properties to corresponding notches on PITCH and YAW
 164         for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) {
 165             biquadFilter_t *dest = &rpmFilter.notch[axis][motorIndex][harmonicIndex];
 166             dest->b0 = template->b0;
 167             dest->b1 = template->b1;
 168             dest->b2 = template->b2;
 169             dest->a1 = template->a1;
 170             dest->a2 = template->a2;
 171             dest->weight = template->weight;
 172         }
 173
 174         // cycle through all notches on ROLL (takes RPM_FILTER_DURATION_S at max.)
 175         harmonicIndex = (harmonicIndex + 1) % rpmFilter.numHarmonics;
 176         if (harmonicIndex == 0) {
 177             motorIndex = (motorIndex + 1) % getMotorCount();
 178         }
 179     }
 180 }
 181
 182 FAST_CODE float rpmFilterApply(const int axis, float value)
 183 {
 184     // Iterate over all notches on axis and apply each one to value.
 185     // Order of application doesn't matter because biquads are linear time-invariant filters.
 186     for (int i = 0; i < rpmFilter.numHarmonics; i++) {
 187         for (int motor = 0; motor < getMotorCount(); motor++) {
 188             value = biquadFilterApplyDF1Weighted(&rpmFilter.notch[axis][motor][i], value);
 189         }
 190     }
 191
 192     return value;
 193 }
 194
 195 bool isRpmFilterEnabled(void)
 196 {
 197     return rpmFilter.numHarmonics > 0;
 198 }
 199
 200 float getMinMotorFrequency(void)
 201 {
 202     return minMotorFrequencyHz;
 203 }
 204
 205 #endif // USE_RPM_FILTER