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

Merge pull request #6734 from betaflight/iterm_relax_unittest

Browse source 
Add Iterm Relax unittest + refactor PID code (Separate iterm relax and absolute control)
This commit is contained in:
borisbstyle 2018-09-12 09:26:03 +02:00 • committed by GitHub
commit 45cf479042
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 185 additions and 67 deletions
Download patch file Download diff file Expand all files Collapse all files

149
src/main/flight/pid.c
View file

@ -56,8 +56,6 @@
#include "sensors/acceleration.h"
#define ITERM_RELAX_SETPOINT_THRESHOLD 30.0f
const char pidNames[] =
"ROLL;"
"PITCH;"
@ -842,6 +840,89 @@ void FAST_CODE applySmartFeedforward(int axis)
}
#endif // USE_SMART_FEEDFORWARD
#if defined(USE_ABSOLUTE_CONTROL)
static void applyAbsoluteControl(const int axis, const float gyroRate, const bool itermRelaxIsEnabled,
const float setpointLpf, const float setpointHpf,
float *currentPidSetpoint, float *itermErrorRate)
{
if (acGain > 0) {
float acErrorRate = 0;
if (itermRelaxIsEnabled) {
const float gmaxac = setpointLpf + 2 * setpointHpf;
const float gminac = setpointLpf - 2 * setpointHpf;
if (gyroRate >= gminac && gyroRate <= gmaxac) {
const float acErrorRate1 = gmaxac - gyroRate;
const float acErrorRate2 = gminac - gyroRate;
if (acErrorRate1 * axisError[axis] < 0) {
acErrorRate = acErrorRate1;
} else {
acErrorRate = acErrorRate2;
}
if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) {
acErrorRate = -axisError[axis] * pidFrequency;
}
} else {
acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
}
} else {
acErrorRate = *itermErrorRate;
}
if (isAirmodeActivated()) {
axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT,
-acErrorLimit, acErrorLimit);
const float acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit);
*currentPidSetpoint += acCorrection;
*itermErrorRate += acCorrection;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(axisError[axis] * 10));
}
}
}
}
#endif
#if defined(USE_ITERM_RELAX)
static void applyItermRelax(const int axis, const float ITerm,
const float gyroRate, float *itermErrorRate, float *currentPidSetpoint)
{
const float setpointLpf = pt1FilterApply(&windupLpf[axis], *currentPidSetpoint);
const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf);
bool itermRelaxIsEnabled = false;
if (itermRelax &&
(axis < FD_YAW || itermRelax == ITERM_RELAX_RPY ||
itermRelax == ITERM_RELAX_RPY_INC)) {
itermRelaxIsEnabled = true;
const float itermRelaxFactor = 1 - setpointHpf / ITERM_RELAX_SETPOINT_THRESHOLD;
const bool isDecreasingI =
((ITerm > 0) && (*itermErrorRate < 0)) || ((ITerm < 0) && (*itermErrorRate > 0));
if ((itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) {
// Do Nothing, use the precalculed itermErrorRate
} else if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < ITERM_RELAX_SETPOINT_THRESHOLD) {
*itermErrorRate *= itermRelaxFactor;
} else if (itermRelaxType == ITERM_RELAX_GYRO ) {
*itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf);
} else {
*itermErrorRate = 0.0f;
}
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointHpf));
DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f));
DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(*itermErrorRate));
}
} else {
#if defined(USE_ABSOLUTE_CONTROL)
applyAbsoluteControl(axis, gyroRate, setpointLpf, setpointHpf, itermRelaxIsEnabled, currentPidSetpoint, itermErrorRate);
#else
UNUSED(itermRelaxIsEnabled);
#endif
}
}
#endif
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
// Based on 2DOF reference design (matlab)
void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
@ -912,73 +993,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate,
&currentPidSetpoint, &errorRate);
#ifdef USE_ABSOLUTE_CONTROL
float acCorrection = 0;
float acErrorRate;
#endif
const float ITerm = pidData[axis].I;
float itermErrorRate = errorRate;
#if defined(USE_ITERM_RELAX)
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || itermRelax == ITERM_RELAX_RPY_INC)) {
const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
const float itermRelaxFactor = 1 - setpointHpf / ITERM_RELAX_SETPOINT_THRESHOLD;
const bool isDecreasingI = ((ITerm > 0) && (itermErrorRate < 0)) || ((ITerm < 0) && (itermErrorRate > 0));
if ((itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) {
// Do Nothing, use the precalculed itermErrorRate
} else if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < ITERM_RELAX_SETPOINT_THRESHOLD) {
itermErrorRate *= itermRelaxFactor;
} else if (itermRelaxType == ITERM_RELAX_GYRO ) {
itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf);
} else {
itermErrorRate = 0.0f;
}
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointHpf));
DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f));
DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(itermErrorRate));
}
#if defined(USE_ABSOLUTE_CONTROL)
const float gmaxac = setpointLpf + 2 * setpointHpf;
const float gminac = setpointLpf - 2 * setpointHpf;
if (gyroRate >= gminac && gyroRate <= gmaxac) {
float acErrorRate1 = gmaxac - gyroRate;
float acErrorRate2 = gminac - gyroRate;
if (acErrorRate1 * axisError[axis] < 0) {
acErrorRate = acErrorRate1;
} else {
acErrorRate = acErrorRate2;
}
if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) {
acErrorRate = -axisError[axis] / dT;
}
} else {
acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
}
#endif // USE_ABSOLUTE_CONTROL
} else
#endif // USE_ITERM_RELAX
{
#if defined(USE_ABSOLUTE_CONTROL)
acErrorRate = itermErrorRate;
#endif // USE_ABSOLUTE_CONTROL
}
#if defined(USE_ABSOLUTE_CONTROL)
if (acGain > 0 && isAirmodeActivated()) {
axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT, -acErrorLimit, acErrorLimit);
acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit);
currentPidSetpoint += acCorrection;
itermErrorRate += acCorrection;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(axisError[axis] * 10));
}
}
applyItermRelax(axis, ITerm, gyroRate, &itermErrorRate, &currentPidSetpoint);
#endif
// --------low-level gyro-based PID based on 2DOF PID controller. ----------

2
src/main/flight/pid.h
View file

@ -43,6 +43,8 @@
// This value gives the same "feel" as the previous Kd default of 26 (26 * DTERM_SCALE)
#define FEEDFORWARD_SCALE 0.013754f
#define ITERM_RELAX_SETPOINT_THRESHOLD 30.0f
typedef enum {
PID_ROLL,
PID_PITCH,

5
src/test/Makefile
View file

@ -300,6 +300,11 @@ pid_unittest_SRC := \
$(USER_DIR)/pg/pg.c \
$(USER_DIR)/fc/runtime_config.c
pid_unittest_DEFINES := \
USE_ITERM_RELAX \
USE_RC_SMOOTHING_FILTER \
USE_ABSOLUTE_CONTROL
rcdevice_unittest_DEFINES := \
USE_RCDEVICE

96
src/test/unit/pid_unittest.cc
View file

@ -24,7 +24,7 @@
#include "gtest/gtest.h"
#include "build/debug.h"
bool simulateMixerSaturated = false;
bool simulatedAirmodeEnabled = true;
float simulatedSetpointRate[3] = { 0,0,0 };
float simulatedRcDeflection[3] = { 0,0,0 };
float simulatedThrottlePIDAttenuation = 1.0f;
@ -67,6 +67,7 @@ extern "C" {
float getThrottlePIDAttenuation(void) { return simulatedThrottlePIDAttenuation; }
float getMotorMixRange(void) { return simulatedMotorMixRange; }
float getSetpointRate(int axis) { return simulatedSetpointRate[axis]; }
bool isAirmodeActivated() { return simulatedAirmodeEnabled; }
float getRcDeflectionAbs(int axis) { return ABS(simulatedRcDeflection[axis]); }
void systemBeep(bool) { }
bool gyroOverflowDetected(void) { return false; }
@ -121,6 +122,11 @@ void setDefaultTestSettings(void) {
pidProfile->throttle_boost = 0;
pidProfile->throttle_boost_cutoff = 15;
pidProfile->iterm_rotation = false;
pidProfile->smart_feedforward = false,
pidProfile->iterm_relax = ITERM_RELAX_OFF,
pidProfile->iterm_relax_cutoff = 11,
pidProfile->iterm_relax_type = ITERM_RELAX_SETPOINT,
pidProfile->abs_control_gain = 0,
gyro.targetLooptime = 4000;
}
@ -131,7 +137,6 @@ timeUs_t currentTestTime(void) {
void resetTest(void) {
loopIter = 0;
simulateMixerSaturated = false;
simulatedThrottlePIDAttenuation = 1.0f;
simulatedMotorMixRange = 0.0f;
@ -534,6 +539,93 @@ TEST(pidControllerTest, testFeedForward) {
}
TEST(pidControllerTest, testItermRelax) {
resetTest();
pidProfile->iterm_relax = ITERM_RELAX_RPY;
pidInit(pidProfile);
ENABLE_ARMING_FLAG(ARMED);
pidStabilisationState(PID_STABILISATION_ON);
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
// Loop 1 - Expecting zero since there is no error
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
simulatedSetpointRate[FD_ROLL] = 10;
simulatedSetpointRate[FD_PITCH] = -10;
simulatedSetpointRate[FD_YAW] = 10;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
ASSERT_NEAR(0.52f, pidData[FD_ROLL].I, calculateTolerance(0.52f));
ASSERT_NEAR(-0.65f, pidData[FD_PITCH].I, calculateTolerance(-0.65f));
ASSERT_NEAR(0.59f, pidData[FD_YAW].I, calculateTolerance(0.59f));
// Should stay same when ITERM_RELAX_SETPOINT_THRESHOLD reached
simulatedSetpointRate[FD_ROLL] = ITERM_RELAX_SETPOINT_THRESHOLD;
simulatedSetpointRate[FD_PITCH] = -ITERM_RELAX_SETPOINT_THRESHOLD;
simulatedSetpointRate[FD_YAW] = ITERM_RELAX_SETPOINT_THRESHOLD;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
ASSERT_NEAR(0.52f, pidData[FD_ROLL].I, calculateTolerance(0.52f));
ASSERT_NEAR(-0.65f, pidData[FD_PITCH].I, calculateTolerance(-0.65f));
ASSERT_NEAR(0.59f, pidData[FD_YAW].I, calculateTolerance(0.59f));
simulatedSetpointRate[FD_ROLL] = 20;
simulatedSetpointRate[FD_PITCH] = -20;
simulatedSetpointRate[FD_YAW] = 20;
pidProfile->iterm_relax_type = ITERM_RELAX_GYRO,
pidInit(pidProfile);
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
ASSERT_NEAR(0.52f, pidData[FD_ROLL].I, calculateTolerance(0.52f));
ASSERT_NEAR(-0.65f, pidData[FD_PITCH].I, calculateTolerance(-0.65f));
ASSERT_NEAR(0.59f, pidData[FD_YAW].I, calculateTolerance(0.59f));
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
ASSERT_NEAR(0.79f, pidData[FD_ROLL].I, calculateTolerance(0.79f));
ASSERT_NEAR(-0.98f, pidData[FD_PITCH].I, calculateTolerance(-0.98f));
ASSERT_NEAR(0.88f, pidData[FD_YAW].I, calculateTolerance(0.88));
}
// TODO - Add more tests
TEST(pidControllerTest, testAbsoluteControl) {
resetTest();
pidProfile->abs_control_gain = 10;
pidInit(pidProfile);
ENABLE_ARMING_FLAG(ARMED);
pidStabilisationState(PID_STABILISATION_ON);
// Loop 1 - Expecting zero since there is no error
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
// Add some rotation on ROLL to generate error
simulatedSetpointRate[FD_ROLL] = 10;
simulatedSetpointRate[FD_PITCH] = -10;
simulatedSetpointRate[FD_YAW] = 10;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
ASSERT_NEAR(12.8, pidData[FD_ROLL].P, calculateTolerance(12.8));
ASSERT_NEAR(-18.57, pidData[FD_PITCH].P, calculateTolerance(-18.57));
ASSERT_NEAR(22.4, pidData[FD_YAW].P, calculateTolerance(22.4));
ASSERT_NEAR(0.84, pidData[FD_ROLL].I, calculateTolerance(0.84));
ASSERT_NEAR(-0.92, pidData[FD_PITCH].I, calculateTolerance(-0.92));
ASSERT_NEAR(0.95, pidData[FD_YAW].I, calculateTolerance(-0.95));
}
TEST(pidControllerTest, testDtermFiltering) {
// TODO
}