Skip to content

Commit

Permalink
Added robust differentiation to pid_luxfloat. Revised DTerm scaling.
Browse files Browse the repository at this point in the history
  • Loading branch information
@martinbudden
martinbudden committed Apr 26, 2016
1 parent 587fa94 commit 56503ea
Show file tree
Hide file tree
Showing 8 changed files with 186 additions and 72 deletions.
32 changes: 20 additions & 12 deletions src/main/config/config_unittest.h
View file
Expand Up @@ -46,24 +46,28 @@ bool unittest_outsideRealtimeGuardInterval;
#ifdef SRC_MAIN_FLIGHT_PID_LUXFLOAT_C_
#ifdef UNIT_TEST

float unittest_pidLuxFloatCore_lastRateForDelta[3];
float unittest_pidLuxFloatCore_deltaState[3][DTERM_AVERAGE_COUNT];
float unittest_pidLuxFloatCore_lastRate[3][PID_LAST_RATE_COUNT];
float unittest_pidLuxFloatCore_deltaState[3][PID_DELTA_MAX_SAMPLES];
float unittest_pidLuxFloatCore_PTerm[3];
float unittest_pidLuxFloatCore_ITerm[3];
float unittest_pidLuxFloatCore_DTerm[3];

#define SET_PID_LUX_FLOAT_CORE_LOCALS(axis) \
{ \
lastRateForDelta[axis] = unittest_pidLuxFloatCore_lastRateForDelta[axis]; \
for (int ii = 0; ii < DTERM_AVERAGE_COUNT; ++ii) { \
for (int ii = 0; ii < PID_LAST_RATE_COUNT; ++ii) { \
lastRate[axis][ii] = unittest_pidLuxFloatCore_lastRate[axis][ii]; \
} \
for (int ii = 0; ii < PID_DELTA_MAX_SAMPLES; ++ii) { \
deltaState[axis][ii] = unittest_pidLuxFloatCore_deltaState[axis][ii]; \
} \
}

#define GET_PID_LUX_FLOAT_CORE_LOCALS(axis) \
{ \
unittest_pidLuxFloatCore_lastRateForDelta[axis] = lastRateForDelta[axis]; \
for (int ii = 0; ii < DTERM_AVERAGE_COUNT; ++ii) { \
for (int ii = 0; ii < PID_LAST_RATE_COUNT; ++ii) { \
unittest_pidLuxFloatCore_lastRate[axis][ii] = lastRate[axis][ii]; \
} \
for (int ii = 0; ii < PID_DELTA_MAX_SAMPLES; ++ii) { \
unittest_pidLuxFloatCore_deltaState[axis][ii] = deltaState[axis][ii]; \
} \
unittest_pidLuxFloatCore_PTerm[axis] = PTerm; \
Expand All @@ -83,24 +87,28 @@ float unittest_pidLuxFloatCore_DTerm[3];
#ifdef SRC_MAIN_FLIGHT_PID_MWREWRITE_C_
#ifdef UNIT_TEST

int32_t unittest_pidMultiWiiRewriteCore_lastRateForDelta[3];
int32_t unittest_pidMultiWiiRewriteCore_deltaState[3][DTERM_AVERAGE_COUNT];
int32_t unittest_pidMultiWiiRewriteCore_lastRate[3][PID_LAST_RATE_COUNT];
int32_t unittest_pidMultiWiiRewriteCore_deltaState[3][PID_DELTA_MAX_SAMPLES];
int32_t unittest_pidMultiWiiRewriteCore_PTerm[3];
int32_t unittest_pidMultiWiiRewriteCore_ITerm[3];
int32_t unittest_pidMultiWiiRewriteCore_DTerm[3];

#define SET_PID_MULTI_WII_REWRITE_CORE_LOCALS(axis) \
{ \
lastRateForDelta[axis] = unittest_pidMultiWiiRewriteCore_lastRateForDelta[axis]; \
for (int ii = 0; ii < DTERM_AVERAGE_COUNT; ++ii) { \
for (int ii = 0; ii < PID_LAST_RATE_COUNT; ++ii) { \
lastRate[axis][ii] = unittest_pidMultiWiiRewriteCore_lastRate[axis][ii]; \
} \
for (int ii = 0; ii < PID_DELTA_MAX_SAMPLES; ++ii) { \
deltaState[axis][ii] = unittest_pidMultiWiiRewriteCore_deltaState[axis][ii]; \
} \
}

#define GET_PID_MULTI_WII_REWRITE_CORE_LOCALS(axis) \
{ \
unittest_pidMultiWiiRewriteCore_lastRateForDelta[axis] = lastRateForDelta[axis]; \
for (int ii = 0; ii < DTERM_AVERAGE_COUNT; ++ii) { \
for (int ii = 0; ii < PID_LAST_RATE_COUNT; ++ii) { \
unittest_pidMultiWiiRewriteCore_lastRate[axis][ii] = lastRate[axis][ii]; \
} \
for (int ii = 0; ii < PID_DELTA_MAX_SAMPLES; ++ii) { \
unittest_pidMultiWiiRewriteCore_deltaState[axis][ii] = deltaState[axis][ii]; \
} \
unittest_pidMultiWiiRewriteCore_PTerm[axis] = PTerm; \
Expand Down
11 changes: 7 additions & 4 deletions src/main/flight/pid.c
View file
Expand Up @@ -105,8 +105,10 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.I8[PIDVEL] = 45,
.D8[PIDVEL] = 1,

.dterm_noise_robust_differentiator = true,
.dterm_lpf_hz = 0,
.dterm_average_count = 0,
.yaw_p_limit = YAW_P_LIMIT_MAX,
.dterm_cut_hz = 0,
);

void pidResetITerm(void)
Expand All @@ -117,14 +119,15 @@ void pidResetITerm(void)
}
}

biquad_t deltaFilterState[3];
filterStatePt1_t deltaPt1FilterState[3];
biquad_t deltaBiquadFilterState[3];

void pidFilterIsSetCheck(const pidProfile_t *pidProfile)
{
static bool deltaStateIsSet = false;
if (!deltaStateIsSet && pidProfile->dterm_cut_hz) {
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (int axis = 0; axis < 3; axis++) {
BiQuadNewLpf(pidProfile->dterm_cut_hz, &deltaFilterState[axis], targetLooptime);
BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiquadFilterState[axis], targetLooptime);
}
deltaStateIsSet = true;
}
Expand Down
20 changes: 11 additions & 9 deletions src/main/flight/pid.h
View file
Expand Up @@ -25,7 +25,9 @@
#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter
#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter

#define DTERM_AVERAGE_COUNT 4
#define PID_DELTA_MAX_SAMPLES 8
#define PID_LAST_RATE_COUNT 6


typedef enum {
PIDROLL,
Expand All @@ -48,15 +50,15 @@ typedef enum {
PID_COUNT
} pidControllerType_e;

#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == PID_CONTROLLER_LUX_FLOAT)

typedef struct pidProfile_s {
uint8_t P8[PID_ITEM_COUNT];
uint8_t I8[PID_ITEM_COUNT];
uint8_t D8[PID_ITEM_COUNT];
uint8_t pidController;
uint16_t yaw_p_limit; // set P term limit (fixed value was 300)
uint16_t dterm_cut_hz; // dterm filtering
uint8_t P8[PID_ITEM_COUNT];
uint8_t I8[PID_ITEM_COUNT];
uint8_t D8[PID_ITEM_COUNT];
uint8_t pidController;
uint8_t dterm_noise_robust_differentiator;
uint16_t dterm_lpf_hz; // dterm low pass filter
uint8_t dterm_average_count;
uint16_t yaw_p_limit; // yaw P term limit (fixed value was 300)
} pidProfile_t;

PG_DECLARE_PROFILE(pidProfile_t, pidProfile);
Expand Down
65 changes: 51 additions & 14 deletions src/main/flight/pid_luxfloat.c
View file
Expand Up @@ -59,7 +59,8 @@ extern float dT;
extern uint8_t PIDweight[3];
extern float lastITermf[3], ITermLimitf[3];

extern biquad_t deltaFilterState[3];
extern biquad_t deltaBiquadFilterState[3];
extern filterStatePt1_t deltaPt1FilterState[3];

extern uint8_t motorCount;

Expand All @@ -70,13 +71,13 @@ extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
// constants to scale pidLuxFloat so output is same as pidMultiWiiRewrite
static const float luxPTermScale = 1.0f / 128;
static const float luxITermScale = 1000000.0f / 0x1000000;
static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 256;
static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 512;
static const float luxGyroScale = 16.4f / 4; // the 16.4 is needed because mwrewrite does not scale according to the gyro model gyro.scale

STATIC_UNIT_TESTED int16_t pidLuxFloatCore(int axis, const pidProfile_t *pidProfile, float gyroRate, float angleRate)
{
static float lastRateForDelta[3];
static float deltaState[3][DTERM_AVERAGE_COUNT];
static float lastRate[3][PID_LAST_RATE_COUNT];
static float deltaState[3][PID_DELTA_MAX_SAMPLES];

SET_PID_LUX_FLOAT_CORE_LOCALS(axis);

Expand Down Expand Up @@ -110,17 +111,51 @@ STATIC_UNIT_TESTED int16_t pidLuxFloatCore(int axis, const pidProfile_t *pidProf
// optimisation for when D8 is zero, often used by YAW axis
DTerm = 0;
} else {
// delta calculated from measurement
float delta = -(gyroRate - lastRateForDelta[axis]);
lastRateForDelta[axis] = gyroRate;
// Divide delta by dT to get differential (ie dr/dt)
delta *= (1.0f / dT);
if (pidProfile->dterm_cut_hz) {
float delta; // delta calculated from measurement
// Calculate derivative using noise-robust differentiator without time delay (one-sided or forward filters)
// by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
switch (pidProfile->dterm_noise_robust_differentiator) {
case 0:
// basic derivative
delta = -(gyroRate - lastRate[axis][0]);
break;
case 1:
// N=2: h[0] = 1/2, h[-1] = 0, h[-2] = -1/2
delta = -(gyroRate - lastRate[axis][1]) / 2;
break;
case 2:
// N=3: h[0] = 1/4, h[-1] = 1/4, h[-2] = -1/4, h[-3] = -1/4
delta = -(gyroRate + lastRate[axis][0] - lastRate[axis][1] - lastRate[axis][2]) / 4;
break;
case 3:
// N=4: h[0] = 5/8, h[-1] = 1/4, h[-2] = -1, h[-3] = -1/4, h[-4] = 3/8
delta = -(5*gyroRate + 2*lastRate[axis][0] - 8*lastRate[axis][1] - 2*lastRate[axis][2] + 3*lastRate[axis][3]) / 8;
break;
case 4:
// N=5: h[0] = 3/8, h[-1] = 1/2, h[-2] = -1/2, h[-3] = -3/4, h[-4] = 1/8, h[-5] = 1/4
delta = -(3*gyroRate + 4*lastRate[axis][0] - 4*lastRate[axis][1] - 6*lastRate[axis][2] + 1*lastRate[axis][3] + 2*lastRate[axis][4]) / 8;
break;
case 5:
// N=6: h[0] = 7/32, h[-1] = 1/2, h[-2] = -1/32, h[-3] = -3/4, h[-4] = -11/32, h[-5] = 1/4, h[-6] = 5/32
delta = -(7*gyroRate + 16*lastRate[axis][0] - 1*lastRate[axis][1] - 24*lastRate[axis][2] - 11*lastRate[axis][3] + 8*lastRate[axis][4]+ 5*lastRate[axis][5]) / 32;
break;
}
delta /= dT;
for (int i = PID_LAST_RATE_COUNT - 1; i > 0; i--) {
lastRate[axis][i] = lastRate[axis][i-1];
}
lastRate[axis][0] = gyroRate;
if (pidProfile->dterm_lpf_hz) {
// DTerm delta low pass filter
delta = applyBiQuadFilter(delta, &deltaFilterState[axis]);
} else {
// When DTerm low pass filter disabled apply moving average to reduce noise
delta = filterApplyAveragef(delta, DTERM_AVERAGE_COUNT, deltaState[axis]);
#ifdef USE_PID_BIQUAD_FILTER
delta = applyBiQuadFilter(delta, &deltaBiquadFilterState[axis]);
#else
delta = filterApplyPt1(delta, &deltaPt1FilterState[axis], pidProfile->dterm_lpf_hz, dT);
#endif
}
if (pidProfile->dterm_average_count) {
// Apply moving average
delta = filterApplyAveragef(delta, pidProfile->dterm_average_count, deltaState[axis]);
}
DTerm = luxDTermScale * delta * pidProfile->D8[axis] * PIDweight[axis] / 100;
DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D);
Expand All @@ -139,7 +174,9 @@ STATIC_UNIT_TESTED int16_t pidLuxFloatCore(int axis, const pidProfile_t *pidProf
void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
#ifdef USE_PID_BIQUAD_FILTER
pidFilterIsSetCheck(pidProfile);
#endif

float horizonLevelStrength;
if (FLIGHT_MODE(HORIZON_MODE)) {
Expand Down
12 changes: 6 additions & 6 deletions src/main/flight/pid_mw23.c
View file
Expand Up @@ -60,11 +60,11 @@ uint8_t dynP8[3], dynI8[3], dynD8[3];
extern uint8_t motorCount;

#ifdef BLACKBOX
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
extern int32_t axisPID_P[], axisPID_I[], axisPID_D[];
#endif
extern int32_t lastITerm[3], ITermLimit[3];
extern int32_t lastITerm[], ITermLimit[];

extern biquad_t deltaFilterState[3];
extern biquad_t deltaBiquadFilterState[3];


void pidResetITermAngle(void)
Expand Down Expand Up @@ -146,10 +146,10 @@ void pidMultiWii23(const pidProfile_t *pidProfile, const controlRateConfig_t *co
// Delta from measurement
delta = -(gyroError - lastErrorForDelta[axis]);
lastErrorForDelta[axis] = gyroError;
if (pidProfile->dterm_cut_hz) {
// Dterm delta low pass
if (pidProfile->dterm_lpf_hz) {
// Dterm low pass filter
DTerm = delta;
DTerm = lrintf(applyBiQuadFilter((float) DTerm, &deltaFilterState[axis])) * 3; // Keep same scaling as unfiltered DTerm
DTerm = lrintf(applyBiQuadFilter((float) DTerm, &deltaBiquadFilterState[axis])) * 3; // Keep same scaling as unfiltered DTerm
} else {
// When dterm filter disabled apply moving average to reduce noise
DTerm = delta1[axis] + delta2[axis] + delta;
Expand Down
48 changes: 35 additions & 13 deletions src/main/flight/pid_mwrewrite.c
View file
Expand Up @@ -54,10 +54,12 @@
#include "flight/mixer.h"


extern float dT;
extern uint8_t PIDweight[3];
extern int32_t lastITerm[3], ITermLimit[3];

extern biquad_t deltaFilterState[3];
extern biquad_t deltaBiquadFilterState[3];
extern filterStatePt1_t deltaPt1FilterState[3];

extern uint8_t motorCount;

Expand All @@ -68,8 +70,8 @@ extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];

STATIC_UNIT_TESTED int16_t pidMultiWiiRewriteCore(int axis, const pidProfile_t *pidProfile, int32_t gyroRate, int32_t angleRate)
{
static int32_t lastRateForDelta[3];
static int32_t deltaState[3][DTERM_AVERAGE_COUNT];
static int32_t lastRate[3][PID_LAST_RATE_COUNT];
static int32_t deltaState[3][PID_DELTA_MAX_SAMPLES];

SET_PID_MULTI_WII_REWRITE_CORE_LOCALS(axis);

Expand Down Expand Up @@ -109,18 +111,36 @@ STATIC_UNIT_TESTED int16_t pidMultiWiiRewriteCore(int axis, const pidProfile_t *
DTerm = 0;
} else {
// delta calculated from measurement
int32_t delta = -(gyroRate - lastRateForDelta[axis]);
lastRateForDelta[axis] = gyroRate;
// Divide delta by targetLooptime to get differential (ie dr/dt)
delta = (delta * ((uint16_t)0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6;
if (pidProfile->dterm_cut_hz) {
// DTerm delta low pass filter
delta = lrintf(applyBiQuadFilter((float)delta, &deltaFilterState[axis]));
int32_t delta;
if (pidProfile->dterm_noise_robust_differentiator) {
// Calculate derivative using 5-point noise-robust differentiator without time delay (one-sided or forward filters)
// by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
// h[0] = 5/8, h[-1] = 1/4, h[-2] = -1, h[-3] = -1/4, h[-4] = 3/8
delta = 5*gyroRate + 2*lastRate[axis][0] - 8*lastRate[axis][1] - 2*lastRate[axis][2] + 3*lastRate[axis][3];
delta /= (-8);
for (int i = PID_LAST_RATE_COUNT - 1; i > 0; i--) {
lastRate[axis][i] = lastRate[axis][i-1];
}
} else {
// When DTerm low pass filter disabled apply moving average to reduce noise
delta = filterApplyAverage(delta, DTERM_AVERAGE_COUNT, deltaState[axis]);
delta = -(gyroRate - lastRate[axis][0]);
}
DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 6;

lastRate[axis][0] = gyroRate;
// Divide delta by targetLooptime to get differential (ie dr/dt)
delta = (delta * ((uint16_t)0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 5;
if (pidProfile->dterm_lpf_hz) {
// DTerm low pass filter
#ifdef USE_PID_BIQUAD_FILTER
delta = lrintf(applyBiQuadFilter((float)delta, &deltaBiquadFilterState[axis]));
#else
delta = filterApplyPt1((float)delta, &deltaPt1FilterState[axis], pidProfile->dterm_lpf_hz, dT);
#endif
}
if (pidProfile->dterm_average_count) {
// Apply moving average
delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]);
}
DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
DTerm = constrain(DTerm, -PID_MAX_D, PID_MAX_D);
}

Expand All @@ -137,7 +157,9 @@ STATIC_UNIT_TESTED int16_t pidMultiWiiRewriteCore(int axis, const pidProfile_t *
void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
#ifdef USE_PID_BIQUAD_FILTER
pidFilterIsSetCheck(pidProfile);
#endif

int8_t horizonLevelStrength;
if (FLIGHT_MODE(HORIZON_MODE)) {
Expand Down
4 changes: 3 additions & 1 deletion src/main/io/serial_cli.c
View file
Expand Up @@ -689,7 +689,9 @@ const clivalue_t valueTable[] = {
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { PID_MIN, PID_MAX } , PG_PID_PROFILE, offsetof(pidProfile_t, D8[PIDVEL])},

{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } , PG_PID_PROFILE, offsetof(pidProfile_t, yaw_p_limit)},
{ "dterm_cut_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = {0, 500 } , PG_PID_PROFILE, offsetof(pidProfile_t, dterm_cut_hz)},
{ "dterm_lowpass_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 } , PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf_hz)},
{ "dterm_nrd", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 5 } , PG_PID_PROFILE, offsetof(pidProfile_t, dterm_noise_robust_differentiator)},
{ "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, PID_DELTA_MAX_SAMPLES } , PG_PID_PROFILE, offsetof(pidProfile_t, dterm_average_count)},

#ifdef GTUNE
{ "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 200 } , PG_GTUNE_CONFIG, offsetof(gtuneConfig_t, gtune_lolimP[FD_ROLL])},
Expand Down

0 comments on commit 56503ea

Please sign in to comment.