mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 16:55:29 +03:00
Gyro Soft LPF Rework // Filter rework
remove 3 point average lux optimize
This commit is contained in:
borisbstyle
parent
f40982cd2d
commit
995f40c2f4
9 changed files with 128 additions and 59 deletions
|
|
@ -20,7 +20,9 @@
|
|||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/filter.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
|
||||
// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
|
||||
|
|
@ -35,3 +37,59 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float
|
|||
|
||||
return filter->state;
|
||||
}
|
||||
|
||||
/**
|
||||
* Typical quadcopter motor noise frequency (at 50% throttle):
|
||||
* 450-sized, 920kv, 9.4x4.3 props, 3S : 4622rpm = 77Hz
|
||||
* 250-sized, 2300kv, 5x4.5 props, 4S : 14139rpm = 235Hz
|
||||
*/
|
||||
static int8_t gyroFIRCoeff_1000[3][9] = { { 0, 0, 12, 23, 40, 51, 52, 40, 38 }, // looptime=1000; group delay 2.5ms; -0.5db = 32Hz ; -1db = 45Hz; -5db = 97Hz; -10db = 132Hz
|
||||
{ 18, 30, 42, 46, 40, 34, 22, 8, 8}, // looptime=1000; group delay 3ms; -0.5db = 18Hz ; -1db = 33Hz; -5db = 81Hz; -10db = 113Hz
|
||||
{ 18, 12, 28, 40, 44, 40, 32, 22, 20} }; // looptime=1000; group delay 4ms; -0.5db = 23Hz ; -1db = 35Hz; -5db = 75Hz; -10db = 103Hz
|
||||
static int8_t gyroFIRCoeff_2000[3][9] = { { 0, 0, 0, 6, 24, 58, 82, 64, 20 }, // looptime=2000, group delay 4ms; -0.5db = 21Hz ; -1db = 31Hz; -5db = 71Hz; -10db = 99Hz
|
||||
{ 0, 0, 14, 22, 46, 60, 56, 40, 24}, // looptime=2000, group delay 5ms; -0.5db = 20Hz ; -1db = 26Hz; -5db = 52Hz; -10db = 71Hz
|
||||
{ 14, 12, 26, 38, 44, 42, 34, 24, 20} }; // looptime=2000, group delay 7ms; -0.5db = 11Hz ; -1db = 18Hz; -5db = 38Hz; -10db = 52Hz
|
||||
static int8_t gyroFIRCoeff_3000[3][9] = { { 0, 0, 0, 0, 4, 36, 88, 88, 44 }, // looptime=3000, group delay 4.5ms; -0.5db = 18Hz ; -1db = 26Hz; -5db = 57Hz; -10db = 78Hz
|
||||
{ 0, 0, 0, 14, 32, 64, 72, 54, 28}, // looptime=3000, group delay 6.5ms; -0.5db = 16Hz ; -1db = 21Hz; -5db = 42Hz; -10db = 57Hz
|
||||
{ 0, 6, 10, 28, 44, 54, 54, 38, 22} }; // looptime=3000, group delay 9ms; -0.5db = 10Hz ; -1db = 13Hz; -5db = 32Hz; -10db = 45Hz
|
||||
|
||||
int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint16_t targetLooptime)
|
||||
{
|
||||
if (filter_level == 0) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
int firIndex = constrain(filter_level, 1, 3) - 1;
|
||||
|
||||
// For looptimes faster than 1499 (and looptime=0) use filter for 1kHz looptime
|
||||
if (targetLooptime < 1500) {
|
||||
return gyroFIRCoeff_1000[firIndex];
|
||||
}
|
||||
// 1500 ... 2499
|
||||
else if (targetLooptime < 2500) {
|
||||
return gyroFIRCoeff_2000[firIndex];
|
||||
}
|
||||
// > 2500
|
||||
else {
|
||||
return gyroFIRCoeff_3000[firIndex];
|
||||
}
|
||||
}
|
||||
|
||||
// 9 Tap FIR filter as described here:
|
||||
// Thanks to Qcopter & BorisB & DigitalEntity
|
||||
void filterApply9TapFIR(int16_t data[3], int16_t state[3][9], int8_t coeff[9])
|
||||
{
|
||||
int32_t FIRsum;
|
||||
int axis, i;
|
||||
|
||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
FIRsum = 0;
|
||||
for (i = 0; i <= 7; i++) {
|
||||
state[axis][i] = state[axis][i + 1];
|
||||
FIRsum += state[axis][i] * (int16_t)coeff[i];
|
||||
}
|
||||
state[axis][8] = data[axis];
|
||||
FIRsum += state[axis][8] * coeff[8];
|
||||
data[axis] = FIRsum / 256;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -22,3 +22,5 @@ typedef struct filterStatePt1_s {
|
|||
} filterStatePt1_t;
|
||||
|
||||
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
|
||||
int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint16_t targetLooptime);
|
||||
void filterApply9TapFIR(int16_t data[3], int16_t state[3][9], int8_t coeff[9]);
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@
|
|||
#include "common/color.h"
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/filter.h"
|
||||
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
|
|
@ -177,8 +178,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
|
|||
pidProfile->D8[PIDVEL] = 1;
|
||||
|
||||
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
|
||||
pidProfile->gyro_cut_hz = 0;
|
||||
pidProfile->pterm_cut_hz = 0;
|
||||
pidProfile->gyro_soft_lpf = 0; // no filtering by default
|
||||
pidProfile->yaw_pterm_cut_hz = 0;
|
||||
pidProfile->dterm_cut_hz = 0;
|
||||
|
||||
pidProfile->P_f[ROLL] = 1.4f; // new PID with preliminary defaults test carefully
|
||||
|
|
@ -717,7 +718,7 @@ void activateConfig(void)
|
|||
¤tProfile->pidProfile
|
||||
);
|
||||
|
||||
useGyroConfig(&masterConfig.gyroConfig);
|
||||
useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf, masterConfig.looptime));
|
||||
|
||||
#ifdef TELEMETRY
|
||||
telemetryUseConfig(&masterConfig.telemetryConfig);
|
||||
|
|
|
|||
|
|
@ -94,7 +94,7 @@ void pidResetErrorGyro(void)
|
|||
|
||||
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
|
||||
|
||||
static filterStatePt1_t PTermState[3], DTermState[3];
|
||||
static filterStatePt1_t yawPTermState, DTermState[3];
|
||||
|
||||
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
|
||||
|
|
@ -172,9 +172,9 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
// -----calculate P component
|
||||
PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
|
||||
|
||||
// Pterm low pass
|
||||
if (pidProfile->pterm_cut_hz) {
|
||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
|
||||
// Yaw Pterm low pass
|
||||
if (axis == YAW && pidProfile->yaw_pterm_cut_hz) {
|
||||
PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
// -----calculate I component.
|
||||
|
|
@ -191,17 +191,24 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
|
||||
// would be scaled by different dt each time. Division by dT fixes that.
|
||||
delta *= (1.0f / dT);
|
||||
// add moving average here to reduce noise
|
||||
deltaSum = delta1[axis] + delta2[axis] + delta;
|
||||
delta2[axis] = delta1[axis];
|
||||
delta1[axis] = delta;
|
||||
|
||||
// In case of soft gyro lpf combined with dterm_cut_hz we don't need the old 3 point averaging
|
||||
if (pidProfile->gyro_soft_lpf && pidProfile->dterm_cut_hz) {
|
||||
deltaSum = delta;
|
||||
} else {
|
||||
// add moving average here to reduce noise
|
||||
deltaSum = delta1[axis] + delta2[axis] + delta;
|
||||
delta2[axis] = delta1[axis];
|
||||
delta1[axis] = delta;
|
||||
deltaSum /= 3.0f;
|
||||
}
|
||||
|
||||
// Dterm low pass
|
||||
if (pidProfile->dterm_cut_hz) {
|
||||
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
|
||||
DTerm = constrainf(deltaSum * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
|
||||
|
||||
// -----calculate total PID output
|
||||
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
|
||||
|
|
@ -277,16 +284,22 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
|
|||
|
||||
PTerm -= ((int32_t)(gyroADC[axis] / 4) * dynP8[axis]) >> 6; // 32 bits is needed for calculation
|
||||
|
||||
// Pterm low pass
|
||||
if (pidProfile->pterm_cut_hz) {
|
||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
|
||||
// Yaw Pterm low pass
|
||||
if (axis == YAW && pidProfile->yaw_pterm_cut_hz) {
|
||||
PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
delta = (gyroADC[axis] - lastGyro[axis]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
||||
lastGyro[axis] = gyroADC[axis];
|
||||
DTerm = delta1[axis] + delta2[axis] + delta;
|
||||
delta2[axis] = delta1[axis];
|
||||
delta1[axis] = delta;
|
||||
|
||||
// In case of soft gyro lpf combined with dterm_cut_hz we don't need the old 3 point averaging
|
||||
if (pidProfile->gyro_soft_lpf && pidProfile->dterm_cut_hz) {
|
||||
DTerm = delta * 3; // Scaling to match the old pid values
|
||||
} else {
|
||||
DTerm = delta1[axis] + delta2[axis] + delta;
|
||||
delta2[axis] = delta1[axis];
|
||||
delta1[axis] = delta;
|
||||
}
|
||||
|
||||
// Dterm low pass
|
||||
if (pidProfile->dterm_cut_hz) {
|
||||
|
|
@ -419,9 +432,9 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
|
|||
// -----calculate P component
|
||||
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
|
||||
|
||||
// Pterm low pass
|
||||
if (pidProfile->pterm_cut_hz) {
|
||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
|
||||
// Yaw Pterm low pass
|
||||
if (axis == YAW && pidProfile->yaw_pterm_cut_hz) {
|
||||
PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
// -----calculate I component
|
||||
|
|
@ -443,10 +456,16 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
|
|||
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
|
||||
// would be scaled by different dt each time. Division by dT fixes that.
|
||||
delta = (delta * ((uint16_t) 0xFFFF / (cycleTime >> 4))) >> 6;
|
||||
// add moving average here to reduce noise
|
||||
deltaSum = delta1[axis] + delta2[axis] + delta;
|
||||
delta2[axis] = delta1[axis];
|
||||
delta1[axis] = delta;
|
||||
|
||||
// In case of soft gyro lpf combined with dterm_cut_hz we don't need the old 3 point averaging
|
||||
if (pidProfile->gyro_soft_lpf && pidProfile->dterm_cut_hz) {
|
||||
deltaSum = delta * 3; // Scaling to approximately match the old D values
|
||||
} else {
|
||||
// add moving average here to reduce noise
|
||||
deltaSum = delta1[axis] + delta2[axis] + delta;
|
||||
delta2[axis] = delta1[axis];
|
||||
delta1[axis] = delta;
|
||||
}
|
||||
|
||||
// Dterm delta low pass
|
||||
if (pidProfile->dterm_cut_hz) {
|
||||
|
|
@ -486,4 +505,3 @@ void pidSetController(pidControllerType_e type)
|
|||
pid_controller = pidMultiWii23;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -60,8 +60,8 @@ typedef struct pidProfile_s {
|
|||
|
||||
uint16_t yaw_p_limit; // set P term limit (fixed value was 300)
|
||||
uint8_t dterm_cut_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
|
||||
uint8_t pterm_cut_hz; // Used for fitlering Pterm noise on noisy frames
|
||||
uint8_t gyro_cut_hz; // Used for soft gyro filtering
|
||||
uint8_t yaw_pterm_cut_hz; // Used for filering Pterm noise on noisy frames
|
||||
uint8_t gyro_soft_lpf;
|
||||
|
||||
#ifdef GTUNE
|
||||
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune
|
||||
|
|
|
|||
|
|
@ -354,6 +354,10 @@ static const char * const lookupTableSerialRX[] = {
|
|||
"XB-B-RJ01"
|
||||
};
|
||||
|
||||
static const char * const lookupTableGyroFilter[] = {
|
||||
"OFF", "LOW", "MEDIUM", "HIGH"
|
||||
};
|
||||
|
||||
|
||||
typedef struct lookupTableEntry_s {
|
||||
const char * const *values;
|
||||
|
|
@ -375,6 +379,7 @@ typedef enum {
|
|||
TABLE_GIMBAL_MODE,
|
||||
TABLE_PID_CONTROLLER,
|
||||
TABLE_SERIAL_RX,
|
||||
TABLE_GYRO_FILTER,
|
||||
} lookupTableIndex_e;
|
||||
|
||||
static const lookupTableEntry_t lookupTables[] = {
|
||||
|
|
@ -391,7 +396,8 @@ static const lookupTableEntry_t lookupTables[] = {
|
|||
{ lookupTableCurrentSensor, sizeof(lookupTableCurrentSensor) / sizeof(char *) },
|
||||
{ lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) },
|
||||
{ lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) },
|
||||
{ lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) }
|
||||
{ lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) },
|
||||
{ lookupTableGyroFilter, sizeof(lookupTableGyroFilter) / sizeof(char *) }
|
||||
};
|
||||
|
||||
#define VALUE_TYPE_OFFSET 0
|
||||
|
|
@ -641,9 +647,9 @@ const clivalue_t valueTable[] = {
|
|||
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } },
|
||||
|
||||
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } },
|
||||
{ "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = {0, 200 } },
|
||||
{ "pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pterm_cut_hz, .config.minmax = {0, 200 } },
|
||||
{ "gyro_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_cut_hz, .config.minmax = {0, 200 } },
|
||||
{ "gyro_soft_lpf", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_GYRO_FILTER } },
|
||||
{ "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = {0, 200 } },
|
||||
{ "yaw_pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz, .config.minmax = {0, 200 } },
|
||||
|
||||
#ifdef GTUNE
|
||||
{ "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], .config.minmax = { 10, 200 } },
|
||||
|
|
|
|||
|
|
@ -702,27 +702,6 @@ void filterRc(void){
|
|||
}
|
||||
}
|
||||
|
||||
// Gyro Low Pass
|
||||
void filterGyro(void) {
|
||||
int axis;
|
||||
static filterStatePt1_t gyroADCState[XYZ_AXIS_COUNT];
|
||||
|
||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
if (masterConfig.looptime > 0) {
|
||||
// Static dT calculation based on configured looptime
|
||||
if (!gyroADCState[axis].constdT) {
|
||||
gyroADCState[axis].constdT = (float)masterConfig.looptime * 0.000001f;
|
||||
}
|
||||
|
||||
gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz, gyroADCState[axis].constdT);
|
||||
}
|
||||
|
||||
else {
|
||||
gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz, dT);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void loop(void)
|
||||
{
|
||||
static uint32_t loopTime;
|
||||
|
|
@ -781,10 +760,6 @@ void loop(void)
|
|||
|
||||
dT = (float)cycleTime * 0.000001f;
|
||||
|
||||
if (currentProfile->pidProfile.gyro_cut_hz) {
|
||||
filterGyro();
|
||||
}
|
||||
|
||||
annexCode();
|
||||
|
||||
if (masterConfig.rxConfig.rcSmoothing) {
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@
|
|||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/filter.h"
|
||||
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
|
|
@ -37,13 +38,16 @@ int16_t gyroADC[XYZ_AXIS_COUNT];
|
|||
int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
|
||||
|
||||
static gyroConfig_t *gyroConfig;
|
||||
static int8_t * gyroFIRTable = 0L;
|
||||
static int16_t gyroFIRState[3][9];
|
||||
|
||||
gyro_t gyro; // gyro access functions
|
||||
sensor_align_e gyroAlign = 0;
|
||||
|
||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse)
|
||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse)
|
||||
{
|
||||
gyroConfig = gyroConfigToUse;
|
||||
gyroFIRTable = filterTableToUse;
|
||||
}
|
||||
|
||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
|
||||
|
|
@ -120,6 +124,11 @@ void gyroUpdate(void)
|
|||
if (!gyro.read(gyroADC)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (gyroFIRTable) {
|
||||
filterApply9TapFIR(gyroADC, gyroFIRState, gyroFIRTable);
|
||||
}
|
||||
|
||||
alignSensors(gyroADC, gyroADC, gyroAlign);
|
||||
|
||||
if (!isGyroCalibrationComplete()) {
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@ typedef struct gyroConfig_s {
|
|||
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
|
||||
} gyroConfig_t;
|
||||
|
||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse);
|
||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse);
|
||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
||||
void gyroUpdate(void);
|
||||
bool isGyroCalibrationComplete(void);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue