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 #include <string.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef USE_RPM_FILTER
  29
  30 #include "build/debug.h"
  31
  32 #include "common/filter.h"
  33 #include "common/maths.h"
  34
  35 #include "drivers/dshot.h"
  36
  37 #include "flight/mixer.h"
  38 #include "flight/pid.h"
  39
  40 #include "pg/motor.h"
  41
  42 #include "scheduler/scheduler.h"
  43
  44 #include "sensors/gyro.h"
  45
  46 #include "rpm_filter.h"
  47
  48 #define RPM_FILTER_HARMONICS_MAX 3
  49 #define RPM_FILTER_DURATION_S    0.001f  // Maximum duration allowed to update all RPM notches once
  50 #define SECONDS_PER_MINUTE       60.0f
  51 #define ERPM_PER_LSB             100.0f
  52
  53
  54 typedef struct rpmFilter_s {
  55
  56     int numHarmonics;
  57     float minHz;
  58     float maxHz;
  59     float fadeRangeHz;
  60     float q;
  61
  62     timeUs_t looptimeUs;
  63     biquadFilter_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_HARMONICS_MAX];
  64
  65 } rpmFilter_t;
  66
  67 // Singleton
  68 FAST_DATA_ZERO_INIT static rpmFilter_t rpmFilter;
  69
  70 FAST_DATA_ZERO_INIT static pt1Filter_t motorFreqLpf[MAX_SUPPORTED_MOTORS];
  71 FAST_DATA_ZERO_INIT static float motorFrequencyHz[MAX_SUPPORTED_MOTORS];
  72 FAST_DATA_ZERO_INIT static float minMotorFrequencyHz;
  73 FAST_DATA_ZERO_INIT static float erpmToHz;
  74
  75 // batch processing of RPM notches
  76 FAST_DATA_ZERO_INIT static int notchUpdatesPerIteration;
  77 FAST_DATA_ZERO_INIT static int motorIndex;
  78 FAST_DATA_ZERO_INIT static int harmonicIndex;
  79
  80
  81 void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs)
  82 {
  83     motorIndex = 0;
  84     harmonicIndex = 0;
  85     minMotorFrequencyHz = 0;
  86     rpmFilter.numHarmonics = 0; // disable RPM Filtering
  87
  88     // if bidirectional DShot is not available
  89     if (!motorConfig()->dev.useDshotTelemetry) {
  90         return;
  91     }
  92
  93     // init LPFs for RPM data
  94     for (int i = 0; i < getMotorCount(); i++) {
  95         pt1FilterInit(&motorFreqLpf[i], pt1FilterGain(config->rpm_filter_lpf_hz, looptimeUs * 1e-6f));
  96     }
  97
  98     // if RPM Filtering is configured to be off
  99     if (!config->rpm_filter_harmonics) {
 100         return;
 101     }
 102
 103     // if we get to this point, enable and init RPM filtering
 104     rpmFilter.numHarmonics = config->rpm_filter_harmonics;
 105     rpmFilter.minHz = config->rpm_filter_min_hz;
 106     rpmFilter.maxHz = 0.48f * 1e6f / looptimeUs; // don't go quite to nyquist to avoid oscillations
 107     rpmFilter.fadeRangeHz = config->rpm_filter_fade_range_hz;
 108     rpmFilter.q = config->rpm_filter_q / 100.0f;
 109     rpmFilter.looptimeUs = looptimeUs;
 110
 111     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 112         for (int motor = 0; motor < getMotorCount(); motor++) {
 113             for (int i = 0; i < rpmFilter.numHarmonics; i++) {
 114                 biquadFilterInit(&rpmFilter.notch[axis][motor][i], rpmFilter.minHz * i, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, 0.0f);
 115             }
 116         }
 117     }
 118
 119     erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE  / (motorConfig()->motorPoleCount / 2.0f);
 120
 121     const float loopIterationsPerUpdate = RPM_FILTER_DURATION_S / (looptimeUs * 1e-6f);
 122     const float numNotchesPerAxis = getMotorCount() * rpmFilter.numHarmonics;
 123     notchUpdatesPerIteration = ceilf(numNotchesPerAxis / loopIterationsPerUpdate); // round to ceiling
 124 }
 125
 126 FAST_CODE_NOINLINE void rpmFilterUpdate(void)
 127 {
 128     if (!motorConfig()->dev.useDshotTelemetry) {
 129         return;
 130     }
 131
 132     // update motor RPM data
 133     minMotorFrequencyHz = FLT_MAX;
 134     for (int motor = 0; motor < getMotorCount(); motor++) {
 135         motorFrequencyHz[motor] = pt1FilterApply(&motorFreqLpf[motor], erpmToHz * getDshotTelemetry(motor));
 136         minMotorFrequencyHz = MIN(minMotorFrequencyHz, motorFrequencyHz[motor]);
 137         if (motor < 4) {
 138             DEBUG_SET(DEBUG_RPM_FILTER, motor, motorFrequencyHz[motor]);
 139         }
 140     }
 141
 142     if (!isRpmFilterEnabled()) {
 143         return;
 144     }
 145
 146     // update RPM notches
 147     for (int i = 0; i < notchUpdatesPerIteration; i++) {
 148
 149         // select current notch on ROLL
 150         biquadFilter_t *template = &rpmFilter.notch[0][motorIndex][harmonicIndex];
 151
 152         const float frequencyHz = constrainf((harmonicIndex + 1) * motorFrequencyHz[motorIndex], rpmFilter.minHz, rpmFilter.maxHz);
 153         const float marginHz = frequencyHz - rpmFilter.minHz;
 154
 155         // fade out notch when approaching minHz (turn it off)
 156         float weight = 1.0f;
 157         if (marginHz < rpmFilter.fadeRangeHz) {
 158             weight = marginHz / rpmFilter.fadeRangeHz;
 159         }
 160
 161         // update notch
 162         biquadFilterUpdate(template, frequencyHz, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, weight);
 163
 164         // copy notch properties to corresponding notches (clones) on PITCH and YAW
 165         for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) {
 166             biquadFilter_t *clone = &rpmFilter.notch[axis][motorIndex][harmonicIndex];
 167             memcpy(clone, template, sizeof(biquadFilter_t));
 168         }
 169
 170         // cycle through all notches on ROLL (takes RPM_FILTER_DURATION_S at max.)
 171         harmonicIndex = (harmonicIndex + 1) % rpmFilter.numHarmonics;
 172         if (harmonicIndex == 0) {
 173             motorIndex = (motorIndex + 1) % getMotorCount();
 174         }
 175     }
 176 }
 177
 178 FAST_CODE float rpmFilterApply(const int axis, float value)
 179 {
 180     for (int i = 0; i < rpmFilter.numHarmonics; i++) {
 181         for (int motor = 0; motor < getMotorCount(); motor++) {
 182             value = biquadFilterApplyDF1Weighted(&rpmFilter.notch[axis][motor][i], value);
 183         }
 184     }
 185
 186     return value;
 187 }
 188
 189 bool isRpmFilterEnabled(void)
 190 {
 191     return rpmFilter.numHarmonics > 0;
 192 }
 193
 194 float getMinMotorFrequency(void)
 195 {
 196     return minMotorFrequencyHz;
 197 }
 198
 199 #endif // USE_RPM_FILTER