diff --git a/Makefile b/Makefile index 658476c117..964f0d0c59 100755 --- a/Makefile +++ b/Makefile @@ -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 \ diff --git a/src/main/common/filter.c b/src/main/common/filter.c new file mode 100644 index 0000000000..38459f915f --- /dev/null +++ b/src/main/common/filter.c @@ -0,0 +1,27 @@ +/* + * filter.c + * + * Created on: 24 jun. 2015 + * Author: borisb + */ + +#include +#include +#include +#include + +#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; +} diff --git a/src/main/common/filter.h b/src/main/common/filter.h new file mode 100644 index 0000000000..7ea2d34990 --- /dev/null +++ b/src/main/common/filter.h @@ -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); diff --git a/src/main/config/config.c b/src/main/config/config.c index 6764ad4abd..2c04ef33bd 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -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; diff --git a/src/main/flight/filter.c b/src/main/flight/filter.c deleted file mode 100644 index 3a35f182de..0000000000 --- a/src/main/flight/filter.c +++ /dev/null @@ -1,27 +0,0 @@ -/* - * filter.c - * - * Created on: 24 jun. 2015 - * Author: borisb - */ - -#include -#include -#include -#include - -#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; -} diff --git a/src/main/flight/filter.h b/src/main/flight/filter.h deleted file mode 100644 index 5782ba9bf2..0000000000 --- a/src/main/flight/filter.h +++ /dev/null @@ -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); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 21b73a6494..63e7e0555c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 01f5fdb82d..9c81e206b5 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -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 }, diff --git a/src/main/mw.c b/src/main/mw.c index 5a376f8f23..61ef317183 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -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();