src/main/flight/adaptive_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 #ifdef USE_ADAPTIVE_FILTER
  28
  29 #include <stdlib.h>
  30 #include "flight/adaptive_filter.h"
  31 #include "arm_math.h"
  32 #include <math.h>
  33 #include "common/maths.h"
  34 #include "common/axis.h"
  35 #include "common/filter.h"
  36 #include "build/debug.h"
  37 #include "fc/config.h"
  38 #include "fc/runtime_config.h"
  39 #include "fc/rc_controls.h"
  40 #include "sensors/gyro.h"
  41
  42 STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE];
  43 STATIC_FASTRAM uint8_t adaptiveFilterSampleIndex = 0;
  44
  45 STATIC_FASTRAM pt1Filter_t stdFilter[XYZ_AXIS_COUNT];
  46 STATIC_FASTRAM pt1Filter_t hpfFilter[XYZ_AXIS_COUNT];
  47
  48 /*
  49     We want to run adaptive filter only when UAV is commanded to stay stationary
  50     Any rotation request on axis will add noise that we are not interested in as it will
  51     automatically cause LPF frequency to be lowered
  52 */
  53 STATIC_FASTRAM float axisAttenuationFactor[XYZ_AXIS_COUNT];
  54
  55 STATIC_FASTRAM uint8_t adaptiveFilterInitialized = 0;
  56 STATIC_FASTRAM uint8_t hpfFilterInitialized = 0;
  57
  58 //Defines if current, min and max  values for the filter were set and filter is ready to work
  59 STATIC_FASTRAM uint8_t targetsSet = 0;
  60 STATIC_FASTRAM float currentLpf;
  61 STATIC_FASTRAM float initialLpf;
  62 STATIC_FASTRAM float minLpf;
  63 STATIC_FASTRAM float maxLpf;
  64
  65 STATIC_FASTRAM float adaptiveFilterIntegrator;
  66 STATIC_FASTRAM float adaptiveIntegratorTarget;
  67
  68 /**
  69  * This function is called at pid rate, so has to be initialized at PID loop frequency
  70 */
  71 void adaptiveFilterPush(const flight_dynamics_index_t index, const float value) {
  72
  73     if (!hpfFilterInitialized) {
  74         //Initialize the filter
  75         for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
  76             pt1FilterInit(&hpfFilter[axis], gyroConfig()->adaptiveFilterHpfHz, US2S(getLooptime()));
  77         }
  78         hpfFilterInitialized = 1;
  79     }
  80
  81     //Apply high pass filter, we are not interested in slowly changing values, only noise
  82     const float filteredGyro = value - pt1FilterApply(&hpfFilter[index], value);
  83
  84     //Push new sample to the buffer so later we can compute RMS and other measures
  85     adaptiveFilterSamples[index][adaptiveFilterSampleIndex] = filteredGyro;
  86     adaptiveFilterSampleIndex = (adaptiveFilterSampleIndex + 1) % ADAPTIVE_FILTER_BUFFER_SIZE;
  87 }
  88
  89 void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rate, const uint8_t configRate) {
  90     const float maxRate = configRate * 10.0f;
  91     axisAttenuationFactor[index] = scaleRangef(fabsf(rate), 0.0f, maxRate, 1.0f, 0.0f);
  92     axisAttenuationFactor[index] = constrainf(axisAttenuationFactor[index], 0.0f, 1.0f);
  93 }
  94
  95 void adaptiveFilterResetIntegrator(void) {
  96     adaptiveFilterIntegrator = 0.0f;
  97 }
  98
  99 void adaptiveFilterSetDefaultFrequency(int lpf, int min, int max) {
 100     currentLpf = lpf;
 101     minLpf = min;
 102     maxLpf = max;
 103     initialLpf = currentLpf;
 104
 105     targetsSet = 1;
 106 }
 107
 108 void adaptiveFilterTask(timeUs_t currentTimeUs) {
 109
 110     //If we don't have current, min and max values for the filter, we can't run it yet
 111     if (!targetsSet) {
 112         return;
 113     }
 114
 115     static timeUs_t previousUpdateTimeUs = 0;
 116
 117     //Initialization procedure, filter setup etc.
 118     if (!adaptiveFilterInitialized) {
 119         adaptiveIntegratorTarget = 3.5f;
 120         previousUpdateTimeUs = currentTimeUs;
 121
 122         //Initialize the filter
 123         for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 124             pt1FilterInit(&stdFilter[axis], gyroConfig()->adaptiveFilterStdLpfHz, 1.0f / ADAPTIVE_FILTER_RATE_HZ);
 125         }
 126         adaptiveFilterInitialized = 1;
 127     }
 128
 129     //If not armed, leave this routine but reset integrator and set default LPF
 130     if (!ARMING_FLAG(ARMED)) {
 131         currentLpf = initialLpf;
 132         adaptiveFilterResetIntegrator();
 133         gyroUpdateDynamicLpf(currentLpf);
 134         return;
 135     }
 136
 137     //Do not run adaptive filter when throttle is low
 138     if (rcCommand[THROTTLE] < 1200) {
 139         return;
 140     }
 141
 142     //Prepare time delta to normalize time factor of the integrator
 143     const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
 144     previousUpdateTimeUs = currentTimeUs;
 145
 146     float combinedStd = 0.0f;
 147
 148     //Compute RMS for each axis
 149     for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 150
 151         //Copy axis samples to a temporary buffer
 152         float32_t tempBuffer[ADAPTIVE_FILTER_BUFFER_SIZE];
 153
 154         //Copute STD from buffer using arm_std_f32
 155         float32_t std;
 156         memcpy(tempBuffer, adaptiveFilterSamples[axis], sizeof(adaptiveFilterSamples[axis]));
 157         arm_std_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &std);
 158
 159         const float filteredStd = pt1FilterApply(&stdFilter[axis], std);
 160         const float error = filteredStd - adaptiveIntegratorTarget;
 161         const float adjustedError = error * axisAttenuationFactor[axis];
 162         const float timeAdjustedError = adjustedError * dT;
 163
 164         //Put into integrator
 165         adaptiveFilterIntegrator += timeAdjustedError;
 166
 167         combinedStd += std;
 168     }
 169
 170     if (adaptiveFilterIntegrator > gyroConfig()->adaptiveFilterIntegratorThresholdHigh) {
 171         //In this case there is too much noise, we need to lower the LPF frequency
 172         currentLpf = constrainf(currentLpf - 1.0f, minLpf, maxLpf);
 173         gyroUpdateDynamicLpf(currentLpf);
 174         adaptiveFilterResetIntegrator();
 175     } else if (adaptiveFilterIntegrator < gyroConfig()->adaptiveFilterIntegratorThresholdLow) {
 176         //In this case there is too little noise, we can to increase the LPF frequency
 177         currentLpf = constrainf(currentLpf + 1.0f, minLpf, maxLpf);
 178         gyroUpdateDynamicLpf(currentLpf);
 179         adaptiveFilterResetIntegrator();
 180     }
 181
 182     combinedStd /= XYZ_AXIS_COUNT;
 183
 184     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedStd * 1000.0f);
 185     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 1, adaptiveFilterIntegrator * 10.0f);
 186     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, currentLpf);
 187 }
 188
 189
 190 #endif /* USE_ADAPTIVE_FILTER */