rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Improve soft filtering function for reuse 

Reorder serial.c *_cut_hz parameters

Remove unnecessary dT calculation in luxfloat

Restructured filter

filter.h fix

Luxfloat remove internal dT

Void function for gyro fillter
This commit is contained in:
borisbstyle 2015-06-30 11:05:13 +02:00
parent 637fd64f36
commit fdcfe71b73
9 changed files with 86 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@ -221,6 +221,7 @@ COMMON_SRC = build_config.c \
common/printf.c \
common/typeconversion.c \
common/encoding.c \
common/filter.c \
main.c \
mw.c \
flight/altitudehold.c \
@ -229,7 +230,6 @@ COMMON_SRC = build_config.c \
flight/imu.c \
flight/mixer.c \
flight/lowpass.c \
flight/filter.c \
drivers/bus_i2c_soft.c \
drivers/serial.c \
drivers/sound_beeper.c \

27
src/main/common/filter.c Normal file
View File

@ -0,0 +1,27 @@
/*
* filter.c
*
* Created on: 24 jun. 2015
* Author: borisb
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include "common/filter.h"
// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dT) {
// Pre calculate and store RC
if (!filter->RC) {
filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
}
filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
return filter->state;
}

14
src/main/common/filter.h Normal file
View File

@ -0,0 +1,14 @@
/*
* filter.h
*
* Created on: 24 jun. 2015
* Author: borisb
*/
typedef struct filterStatePt1_s {
float state;
float RC;
float constdT;
} filterStatePt1_t;
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);

4
src/main/config/config.c
View File

@ -171,9 +171,9 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->D8[PIDVEL] = 1;
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->dterm_cut_hz = 0;
pidProfile->pterm_cut_hz = 0;
pidProfile->gyro_cut_hz = 0;
pidProfile->pterm_cut_hz = 0;
pidProfile->dterm_cut_hz = 0;
pidProfile->P_f[ROLL] = 2.5f; // new PID with preliminary defaults test carefully
pidProfile->I_f[ROLL] = 0.6f;

27
src/main/flight/filter.c
View File

@ -1,27 +0,0 @@
/*
* filter.c
*
* Created on: 24 jun. 2015
* Author: borisb
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include "common/axis.h"
#include "flight/filter.h"
extern uint16_t cycleTime;
// PT1 Low Pass filter
float filterApplyPt1(float input, filterStatePt1_t* state, uint8_t f_cut) {
float dT = (float)cycleTime * 0.000001f;
float RC= 1.0f / ( 2.0f * (float)M_PI * f_cut );
*state = *state + dT / (RC + dT) * (input - *state);
return *state;
}

11
src/main/flight/filter.h
View File

@ -1,11 +0,0 @@
/*
* filter.h
*
* Created on: 24 jun. 2015
* Author: borisb
*/
typedef float filterStatePt1_t;
float filterApplyPt1(float input, filterStatePt1_t* state, uint8_t f_cut);

30
src/main/flight/pid.c
View File

@ -25,6 +25,7 @@
#include "common/axis.h"
#include "common/maths.h"
#include "common/filter.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
@ -42,12 +43,12 @@
#include "flight/imu.h"
#include "flight/navigation.h"
#include "flight/autotune.h"
#include "flight/filter.h"
#include "config/runtime_config.h"
extern uint16_t cycleTime;
extern uint8_t motorCount;
extern float dT;
int16_t heading;
int16_t axisPID[3];
@ -112,12 +113,9 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
static float lastGyroRate[3];
static float delta1[3], delta2[3];
float delta, deltaSum;
float dT;
int axis;
float horizonLevelStrength = 1;
dT = (float)cycleTime * 0.000001f;
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
@ -190,7 +188,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
}
// -----calculate I component. Note that PIDweight is divided by 10, because it is simplified formule from the previous multiply by 10
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * PIDweight[axis] / 10, -250.0f, 250.0f);
@ -213,7 +211,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], 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);
@ -299,7 +297,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
}
delta = (gyroADC[axis] - lastGyro[axis]) / 4;
@ -310,7 +308,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
DTerm = (deltaSum * dynD8[axis]) / 32;
@ -389,7 +387,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->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
@ -400,7 +398,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
DTerm = filterApplyPt1(DTerm, &DTermState[axis], pidProfile->dterm_cut_hz);
DTerm = filterApplyPt1(DTerm, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation
@ -513,7 +511,7 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
}
delta = (gyroADC[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroADC[axis];
@ -523,7 +521,7 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
DTerm = (deltaSum * dynD8[axis]) / 32;
@ -634,7 +632,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
}
delta = (gyroADCQuant - lastGyro[axis]) / ACCDeltaTimeINS;
@ -686,7 +684,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
}
axisPID[FD_YAW] = PTerm + ITerm;
@ -781,7 +779,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
}
// -----calculate I component
@ -810,7 +808,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// Dterm delta low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;

4
src/main/io/serial_cli.c
View File

@ -505,9 +505,9 @@ const clivalue_t valueTable[] = {
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 },
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX },
{ "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, 0, 200 },
{ "pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pterm_cut_hz, 0, 200 },
{ "gyro_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_cut_hz, 0, 200 },
{ "pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pterm_cut_hz, 0, 200 },
{ "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, 0, 200 },
#ifdef BLACKBOX
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 },

35
src/main/mw.c
View File

@ -25,6 +25,7 @@
#include "common/maths.h"
#include "common/axis.h"
#include "common/color.h"
#include "common/filter.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
@ -72,8 +73,6 @@
#include "flight/failsafe.h"
#include "flight/autotune.h"
#include "flight/navigation.h"
#include "flight/filter.h"
#include "config/runtime_config.h"
#include "config/config.h"
@ -96,6 +95,7 @@ enum {
uint32_t currentTime = 0;
uint32_t previousTime = 0;
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
float dT;
int16_t magHold;
int16_t headFreeModeHold;
@ -681,6 +681,27 @@ void processRx(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;
@ -736,14 +757,10 @@ void loop(void)
cycleTime = (int32_t)(currentTime - previousTime);
previousTime = currentTime;
// Gyro Low Pass
if (currentProfile->pidProfile.gyro_cut_hz) {
int axis;
static filterStatePt1_t gyroADCState[XYZ_AXIS_COUNT];
dT = (float)cycleTime * 0.000001f;
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz);
}
if (currentProfile->pidProfile.gyro_cut_hz) {
filterGyro();
}
annexCode();