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 <math.h>
  23 #include <stdint.h>
  24
  25 #include "platform.h"
  26
  27 #if defined(USE_RPM_FILTER)
  28
  29 #include "build/debug.h"
  30
  31 #include "common/filter.h"
  32 #include "common/maths.h"
  33 #include "common/time.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_MAXHARMONICS 3
  49 #define SECONDS_PER_MINUTE      60.0f
  50 #define ERPM_PER_LSB            100.0f
  51 #define MIN_UPDATE_T            0.001f
  52
  53
  54 static pt1Filter_t rpmFilters[MAX_SUPPORTED_MOTORS];
  55
  56 typedef struct rpmNotchFilter_s {
  57
  58     uint8_t  harmonics;
  59     float    minHz;
  60     float    maxHz;
  61     float    fadeRangeHz;
  62     float    q;
  63     timeUs_t looptimeUs;
  64
  65     biquadFilter_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_MAXHARMONICS];
  66
  67 } rpmNotchFilter_t;
  68
  69 FAST_DATA_ZERO_INIT static float   erpmToHz;
  70 FAST_DATA_ZERO_INIT static float   filteredMotorErpm[MAX_SUPPORTED_MOTORS];
  71 FAST_DATA_ZERO_INIT static float   motorFrequency[MAX_SUPPORTED_MOTORS];
  72 FAST_DATA_ZERO_INIT static float   minMotorFrequency;
  73 FAST_DATA_ZERO_INIT static uint8_t numberFilters;
  74 FAST_DATA_ZERO_INIT static uint8_t numberRpmNotchFilters;
  75 FAST_DATA_ZERO_INIT static uint8_t filterUpdatesPerIteration;
  76 FAST_DATA_ZERO_INIT static float   pidLooptime;
  77 FAST_DATA_ZERO_INIT static rpmNotchFilter_t filters[2];
  78 FAST_DATA_ZERO_INIT static rpmNotchFilter_t *gyroFilter;
  79
  80 FAST_DATA_ZERO_INIT static uint8_t currentMotor;
  81 FAST_DATA_ZERO_INIT static uint8_t currentHarmonic;
  82 FAST_DATA_ZERO_INIT static uint8_t currentFilterNumber;
  83 FAST_DATA static rpmNotchFilter_t *currentFilter = &filters[0];
  84
  85
  86
  87 PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 5);
  88
  89 void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config)
  90 {
  91     config->rpm_filter_harmonics = 3;
  92     config->rpm_filter_min_hz = 100;
  93     config->rpm_filter_fade_range_hz = 50;
  94     config->rpm_filter_q = 500;
  95
  96     config->rpm_filter_lpf_hz = 150;
  97 }
  98
  99 static void rpmNotchFilterInit(rpmNotchFilter_t *filter, const rpmFilterConfig_t *config, const timeUs_t looptimeUs)
 100 {
 101     filter->harmonics = config->rpm_filter_harmonics;
 102     filter->minHz = config->rpm_filter_min_hz;
 103     filter->maxHz = 0.48f * 1e6f / looptimeUs; // don't go quite to nyquist to avoid oscillations
 104     filter->fadeRangeHz = config->rpm_filter_fade_range_hz;
 105     filter->q = config->rpm_filter_q / 100.0f;
 106     filter->looptimeUs = looptimeUs;
 107
 108     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 109         for (int motor = 0; motor < getMotorCount(); motor++) {
 110             for (int i = 0; i < filter->harmonics; i++) {
 111                 biquadFilterInit(
 112                     &filter->notch[axis][motor][i], filter->minHz * i, filter->looptimeUs, filter->q, FILTER_NOTCH, 0.0f);
 113             }
 114         }
 115     }
 116 }
 117
 118 void rpmFilterInit(const rpmFilterConfig_t *config)
 119 {
 120     currentFilter = &filters[0];
 121     currentMotor = currentHarmonic = currentFilterNumber = 0;
 122
 123     numberRpmNotchFilters = 0;
 124     if (!motorConfig()->dev.useDshotTelemetry) {
 125         gyroFilter = NULL;
 126         return;
 127     }
 128
 129     pidLooptime = gyro.targetLooptime;
 130     if (config->rpm_filter_harmonics) {
 131         gyroFilter = &filters[numberRpmNotchFilters++];
 132         rpmNotchFilterInit(gyroFilter, config, pidLooptime);
 133     } else {
 134         gyroFilter = NULL;
 135     }
 136
 137     for (int i = 0; i < getMotorCount(); i++) {
 138         pt1FilterInit(&rpmFilters[i], pt1FilterGain(config->rpm_filter_lpf_hz, pidLooptime * 1e-6f));
 139     }
 140
 141     erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE  / (motorConfig()->motorPoleCount / 2.0f);
 142
 143     const float loopIterationsPerUpdate = MIN_UPDATE_T / (pidLooptime * 1e-6f);
 144     numberFilters = getMotorCount() * (filters[0].harmonics + filters[1].harmonics);
 145     const float filtersPerLoopIteration = numberFilters / loopIterationsPerUpdate;
 146     filterUpdatesPerIteration = rintf(filtersPerLoopIteration + 0.49f);
 147 }
 148
 149 static float applyFilter(rpmNotchFilter_t *filter, const int axis, float value)
 150 {
 151     if (filter == NULL) {
 152         return value;
 153     }
 154     for (int motor = 0; motor < getMotorCount(); motor++) {
 155         for (int i = 0; i < filter->harmonics; i++) {
 156             value = biquadFilterApplyDF1Weighted(&filter->notch[axis][motor][i], value);
 157         }
 158     }
 159     return value;
 160 }
 161
 162 float rpmFilterGyro(const int axis, float value)
 163 {
 164     return applyFilter(gyroFilter, axis, value);
 165 }
 166
 167 FAST_CODE_NOINLINE void rpmFilterUpdate(void)
 168 {
 169     for (int motor = 0; motor < getMotorCount(); motor++) {
 170         filteredMotorErpm[motor] = pt1FilterApply(&rpmFilters[motor], getDshotTelemetry(motor));
 171         if (motor < 4) {
 172             DEBUG_SET(DEBUG_RPM_FILTER, motor, motorFrequency[motor]);
 173         }
 174         motorFrequency[motor] = erpmToHz * filteredMotorErpm[motor];
 175     }
 176
 177     if (gyroFilter == NULL) {
 178         minMotorFrequency = 0.0f;
 179         return;
 180     }
 181
 182     for (int i = 0; i < filterUpdatesPerIteration; i++) {
 183
 184         float frequency = constrainf(
 185             (currentHarmonic + 1) * motorFrequency[currentMotor], currentFilter->minHz, currentFilter->maxHz);
 186         biquadFilter_t *template = &currentFilter->notch[0][currentMotor][currentHarmonic];
 187         // uncomment below to debug filter stepping. Need to also comment out motor rpm DEBUG_SET above
 188         /* DEBUG_SET(DEBUG_RPM_FILTER, 0, harmonic); */
 189         /* DEBUG_SET(DEBUG_RPM_FILTER, 1, motor); */
 190         /* DEBUG_SET(DEBUG_RPM_FILTER, 2, currentFilter == &gyroFilter); */
 191         /* DEBUG_SET(DEBUG_RPM_FILTER, 3, frequency) */
 192
 193         // fade out notch when approaching minHz (turn it off)
 194         float weight = 1.0f;
 195         if (frequency < currentFilter->minHz + currentFilter->fadeRangeHz) {
 196             weight = (frequency - currentFilter->minHz) / currentFilter->fadeRangeHz;
 197         }
 198
 199         biquadFilterUpdate(
 200             template, frequency, currentFilter->looptimeUs, currentFilter->q, FILTER_NOTCH, weight);
 201
 202         for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) {
 203             biquadFilter_t *clone = &currentFilter->notch[axis][currentMotor][currentHarmonic];
 204             clone->b0 = template->b0;
 205             clone->b1 = template->b1;
 206             clone->b2 = template->b2;
 207             clone->a1 = template->a1;
 208             clone->a2 = template->a2;
 209             clone->weight = template->weight;
 210         }
 211
 212         if (++currentHarmonic == currentFilter->harmonics) {
 213             currentHarmonic = 0;
 214             if (++currentFilterNumber == numberRpmNotchFilters) {
 215                 currentFilterNumber = 0;
 216                 if (++currentMotor == getMotorCount()) {
 217                     currentMotor = 0;
 218                 }
 219                 minMotorFrequency = 0.0f;
 220             }
 221             currentFilter = &filters[currentFilterNumber];
 222         }
 223     }
 224 }
 225
 226 bool isRpmFilterEnabled(void)
 227 {
 228     return (motorConfig()->dev.useDshotTelemetry && rpmFilterConfig()->rpm_filter_harmonics);
 229 }
 230
 231 float rpmMinMotorFrequency(void)
 232 {
 233     if (minMotorFrequency == 0.0f) {
 234         minMotorFrequency = 10000.0f; // max RPM reported in Hz = 600,000RPM
 235         for (int i = getMotorCount(); i--;) {
 236             if (motorFrequency[i] < minMotorFrequency) {
 237                 minMotorFrequency = motorFrequency[i];
 238             }
 239         }
 240     }
 241     return minMotorFrequency;
 242 }
 243
 244 #endif