Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00
Code Issues Wiki Activity

Basic approach to adaptive filter.

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2024-04-04 21:01:39 +02:00
parent 561f7b8be2
commit ac242b566a
3 changed files with 56 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

55
src/main/flight/adaptive_filter.c
View file

@ -35,6 +35,7 @@
#include "common/filter.h"
#include "build/debug.h"
#include "fc/config.h"
#include "sensors/gyro.h"
STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE];
STATIC_FASTRAM uint8_t adaptiveFilterSampleIndex = 0;
@ -52,6 +53,12 @@ STATIC_FASTRAM float axisAttenuationFactor[XYZ_AXIS_COUNT];
STATIC_FASTRAM uint8_t adaptiveFilterInitialized = 0;
STATIC_FASTRAM uint8_t hpfFilterInitialized = 0;
//Defines if current, min and max values for the filter were set and filter is ready to work
STATIC_FASTRAM uint8_t targetsSet = 0;
STATIC_FASTRAM float currentLpf;
STATIC_FASTRAM float minLpf;
STATIC_FASTRAM float maxLpf;
STATIC_FASTRAM float adaptiveFilterIntegrator;
STATIC_FASTRAM float adaptiveIntegratorTarget;
@ -82,7 +89,24 @@ void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rat
axisAttenuationFactor[index] = constrainf(axisAttenuationFactor[index], 0.0f, 1.0f);
}
void adaptiveFilterResetIntegrator(void) {
adaptiveFilterIntegrator = 0.0f;
}
void adaptiveFilterSetDefaultFrequency(int lpf, int min, int max) {
currentLpf = lpf;
minLpf = min;
maxLpf = max;
targetsSet = 1;
}
void adaptiveFilterTask(timeUs_t currentTimeUs) {
//If we don't have current, min and max values for the filter, we can't run it yet
if (!targetsSet) {
return;
}
static timeUs_t previousUpdateTimeUs = 0;
//Initialization procedure, filter setup etc.
@ -127,18 +151,37 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
combinedStd += std;
if (axis == 0) {
DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, filteredStd * 1000.0f);
DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, error * 1000.0f);
DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 4, adjustedError * 1000.0f);
DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 5, timeAdjustedError * 1000.0f);
}
// if (axis == 0) {
// DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, filteredStd * 1000.0f);
// DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, error * 1000.0f);
// DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 4, adjustedError * 1000.0f);
// DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 5, timeAdjustedError * 1000.0f);
// }
}
//TODO filter gets updated only when ARMED
if (adaptiveFilterIntegrator > ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD) {
//In this case there is too much noise, we need to lower the LPF frequency
currentLpf = constrainf(currentLpf - 1.0f, minLpf, maxLpf);
gyroUpdateDynamicLpf(currentLpf);
adaptiveFilterResetIntegrator();
} else if (adaptiveFilterIntegrator < -ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD) {
//In this case there is too little noise, we can to increase the LPF frequency
currentLpf = constrainf(currentLpf + 1.0f, minLpf, maxLpf);
gyroUpdateDynamicLpf(currentLpf);
adaptiveFilterResetIntegrator();
}
// if (ARMING_FLAG(ARMED)) {
// //
// }
combinedStd /= XYZ_AXIS_COUNT;
DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedStd * 1000.0f);
DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 1, adaptiveFilterIntegrator);
DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, currentLpf);
}

3
src/main/flight/adaptive_filter.h
View file

@ -29,7 +29,10 @@
#define ADAPTIVE_FILTER_RATE_HZ 100
#define ADAPTIVE_FILTER_LPF_HZ 1
#define ADAPTIVE_FILTER_HPF_HZ 15
#define ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD 20.0f
void adaptiveFilterPush(const flight_dynamics_index_t index, const float value);
void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rate, const uint8_t configRate);
void adaptiveFilterResetIntegrator(void);
void adaptiveFilterSetDefaultFrequency(int lpf, int min, int max);
void adaptiveFilterTask(timeUs_t currentTimeUs);

4
src/main/sensors/gyro.c
View file

@ -264,6 +264,10 @@ static void gyroInitFilters(void)
//Second gyro LPF runnig and PID frequency - this filter is dynamic when gyro_use_dyn_lpf = ON
initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_type, gyroConfig()->gyro_main_lpf_hz, getLooptime());
#ifdef USE_ADAPTIVE_FILTER
adaptiveFilterSetDefaultFrequency(gyroConfig()->gyro_main_lpf_hz, 50, 150);
#endif
#ifdef USE_GYRO_KALMAN
if (gyroConfig()->kalmanEnabled) {
gyroKalmanInitialize(gyroConfig()->kalman_q);