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

Merge pull request #6742 from ctzsnooze/update-defaults-for-yaw

Browse source 
defaults to improve yaw behaviour, ITermWindupPointInv already define…
This commit is contained in:
borisbstyle 2018-09-18 08:32:19 +02:00 • committed by GitHub
commit 9b51fb3216
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 43 additions and 46 deletions
Download patch file Download diff file Expand all files Collapse all files

24
src/main/config/config_unittest.h
View file

@ -50,9 +50,9 @@ bool unittest_outsideRealtimeGuardInterval;
float unittest_pidLuxFloat_lastErrorForDelta[3];
float unittest_pidLuxFloat_delta1[3];
float unittest_pidLuxFloat_delta2[3];
float unittest_pidLuxFloat_PTerm[3];
float unittest_pidLuxFloat_ITerm[3];
float unittest_pidLuxFloat_DTerm[3];
float unittest_pidLuxFloat_pterm[3];
float unittest_pidLuxFloat_iterm[3];
float unittest_pidLuxFloat_dterm[3];
#define SET_PID_LUX_FLOAT_LOCALS(axis) \
{ \
@ -66,15 +66,15 @@ float unittest_pidLuxFloat_DTerm[3];
unittest_pidLuxFloat_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \
unittest_pidLuxFloat_delta1[axis] = delta1[axis]; \
unittest_pidLuxFloat_delta2[axis] = delta2[axis]; \
unittest_pidLuxFloat_PTerm[axis] = PTerm; \
unittest_pidLuxFloat_ITerm[axis] = ITerm; \
unittest_pidLuxFloat_DTerm[axis] = DTerm; \
unittest_pidLuxFloat_pterm[axis] = pterm; \
unittest_pidLuxFloat_iterm[axis] = iterm; \
unittest_pidLuxFloat_dterm[axis] = dterm; \
}
int32_t unittest_pidMultiWiiRewrite_lastErrorForDelta[3];
int32_t unittest_pidMultiWiiRewrite_PTerm[3];
int32_t unittest_pidMultiWiiRewrite_ITerm[3];
int32_t unittest_pidMultiWiiRewrite_DTerm[3];
int32_t unittest_pidMultiWiiRewrite_pterm[3];
int32_t unittest_pidMultiWiiRewrite_iterm[3];
int32_t unittest_pidMultiWiiRewrite_dterm[3];
#define SET_PID_MULTI_WII_REWRITE_LOCALS(axis) \
{ \
@ -84,9 +84,9 @@ int32_t unittest_pidMultiWiiRewrite_DTerm[3];
#define GET_PID_MULTI_WII_REWRITE_LOCALS(axis) \
{ \
unittest_pidMultiWiiRewrite_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \
unittest_pidMultiWiiRewrite_PTerm[axis] = PTerm; \
unittest_pidMultiWiiRewrite_ITerm[axis] = ITerm; \
unittest_pidMultiWiiRewrite_DTerm[axis] = DTerm; \
unittest_pidMultiWiiRewrite_pterm[axis] = pterm; \
unittest_pidMultiWiiRewrite_iterm[axis] = iterm; \
unittest_pidMultiWiiRewrite_dterm[axis] = dterm; \
}
#else

8
src/main/fc/core.c
View file

@ -577,16 +577,16 @@ bool processRx(timeUs_t currentTimeUs)
if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
if (throttlePercent >= rxConfig()->airModeActivateThreshold) {
airmodeIsActivated = true; // Prevent Iterm from being reset
airmodeIsActivated = true; // Prevent iterm from being reset
}
} else {
airmodeIsActivated = false;
}
/* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
/* In airmode iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
This is needed to prevent iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) {
pidResetITerm();
pidResetIterm();
if (currentPidProfile->pidAtMinThrottle)
pidStabilisationState(PID_STABILISATION_ON);
else

4
src/main/flight/mixer.c
View file

@ -596,8 +596,8 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
currentThrottleInputRange = rcCommandThrottleRange3dHigh;
}
if (currentTimeUs - reversalTimeUs < 250000) {
// keep ITerm zero for 250ms after motor reversal
pidResetITerm();
// keep iterm zero for 250ms after motor reversal
pidResetIterm();
}
} else {
throttle = rcCommand[THROTTLE] - rxConfig()->mincheck + throttleAngleCorrection;

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

@ -117,7 +117,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.pid = {
[PID_ROLL] = { 46, 45, 25, 60 },
[PID_PITCH] = { 50, 50, 27, 60 },
[PID_YAW] = { 65, 45, 0 , 60 },
[PID_YAW] = { 45, 100, 0, 100 },
[PID_LEVEL] = { 50, 50, 75, 0 },
[PID_MAG] = { 40, 0, 0, 0 },
},
@ -130,12 +130,12 @@ void resetPidProfile(pidProfile_t *pidProfile)
.dterm_notch_hz = 0,
.dterm_notch_cutoff = 0,
.dterm_filter_type = FILTER_PT1,
.itermWindupPointPercent = 100,
.itermWindupPointPercent = 40,
.vbatPidCompensation = 0,
.pidAtMinThrottle = PID_STABILISATION_ON,
.levelAngleLimit = 55,
.feedForwardTransition = 0,
.yawRateAccelLimit = 100,
.yawRateAccelLimit = 0,
.rateAccelLimit = 0,
.itermThrottleThreshold = 250,
.itermAcceleratorGain = 5000,
@ -150,7 +150,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.horizon_tilt_effect = 75,
.horizon_tilt_expert_mode = false,
.crash_limit_yaw = 200,
.itermLimit = 150,
.itermLimit = 300,
.throttle_boost = 5,
.throttle_boost_cutoff = 15,
.iterm_rotation = true,
@ -368,7 +368,7 @@ static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float maxVelocity[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float feedForwardTransition;
static FAST_RAM_ZERO_INIT float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio;
static FAST_RAM_ZERO_INIT float ITermWindupPointInv;
static FAST_RAM_ZERO_INIT float itermWindupPointInv;
static FAST_RAM_ZERO_INIT uint8_t horizonTiltExpertMode;
static FAST_RAM_ZERO_INIT timeDelta_t crashTimeLimitUs;
static FAST_RAM_ZERO_INIT timeDelta_t crashTimeDelayUs;
@ -395,7 +395,7 @@ static FAST_RAM_ZERO_INIT float acLimit;
static FAST_RAM_ZERO_INIT float acErrorLimit;
#endif
void pidResetITerm(void)
void pidResetIterm(void)
{
for (int axis = 0; axis < 3; axis++) {
pidData[axis].I = 0.0f;
@ -443,12 +443,10 @@ void pidInitConfig(const pidProfile_t *pidProfile)
horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f;
maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * dT;
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT;
ITermWindupPointInv = 0.0f;
itermWindupPointInv = 1.0f;
if (pidProfile->itermWindupPointPercent < 100) {
float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
} else {
ITermWindupPointInv = 0.0f;
const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f;
itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint);
}
itermAcceleratorGain = pidProfile->itermAcceleratorGain;
crashTimeLimitUs = pidProfile->crash_time * 1000;
@ -634,8 +632,8 @@ static void handleCrashRecovery(
*errorRate = *currentPidSetpoint - gyroRate;
}
}
// reset ITerm, since accumulated error before crash is now meaningless
// and ITerm windup during crash recovery can be extreme, especially on yaw axis
// reset iterm, since accumulated error before crash is now meaningless
// and iterm windup during crash recovery can be extreme, especially on yaw axis
pidData[axis].I = 0.0f;
if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs
|| (getMotorMixRange() < 1.0f
@ -697,7 +695,7 @@ static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT]
}
}
static void rotateITermAndAxisError()
static void rotateItermAndAxisError()
{
if (itermRotation
#if defined(USE_ABSOLUTE_CONTROL)
@ -883,7 +881,7 @@ static void applyAbsoluteControl(const int axis, const float gyroRate, const boo
#endif
#if defined(USE_ITERM_RELAX)
static void applyItermRelax(const int axis, const float ITerm,
static void applyItermRelax(const int axis, const float iterm,
const float gyroRate, float *itermErrorRate, float *currentPidSetpoint)
{
const float setpointLpf = pt1FilterApply(&windupLpf[axis], *currentPidSetpoint);
@ -897,7 +895,7 @@ static void applyItermRelax(const int axis, const float ITerm,
const float itermRelaxFactor = 1 - setpointHpf / ITERM_RELAX_SETPOINT_THRESHOLD;
const bool isDecreasingI =
((ITerm > 0) && (*itermErrorRate < 0)) || ((ITerm < 0) && (*itermErrorRate > 0));
((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) {
@ -931,7 +929,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
static float previousPidSetpoint[XYZ_AXIS_COUNT];
const float tpaFactor = getThrottlePIDAttenuation();
const float motorMixRange = getMotorMixRange();
#ifdef USE_YAW_SPIN_RECOVERY
const bool yawSpinActive = gyroYawSpinDetected();
@ -946,8 +943,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
// gradually scale back integration when above windup point
float dynCi = dT * itermAccelerator;
if (ITermWindupPointInv > 0) {
dynCi *= constrainf((1.0f - motorMixRange) * ITermWindupPointInv, 0.0f, 1.0f);
if (itermWindupPointInv > 1.0f) {
dynCi *= constrainf((1.0f - getMotorMixRange()) * itermWindupPointInv, 0.0f, 1.0f);
}
// Precalculate gyro deta for D-term here, this allows loop unrolling
@ -958,7 +955,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]);
}
rotateITermAndAxisError();
rotateItermAndAxisError();
// ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
@ -993,25 +990,25 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate,
&currentPidSetpoint, &errorRate);
const float ITerm = pidData[axis].I;
const float iterm = pidData[axis].I;
float itermErrorRate = errorRate;
#if defined(USE_ITERM_RELAX)
applyItermRelax(axis, ITerm, gyroRate, &itermErrorRate, &currentPidSetpoint);
applyItermRelax(axis, iterm, gyroRate, &itermErrorRate, &currentPidSetpoint);
#endif
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
// 2-DOF PID controller with optional filter on derivative term.
// b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error).
// -----calculate P component and add Dynamic Part based on stick input
// -----calculate P component
pidData[axis].P = pidCoefficient[axis].Kp * errorRate * tpaFactor;
if (axis == FD_YAW) {
pidData[axis].P = ptermYawLowpassApplyFn((filter_t *) &ptermYawLowpass, pidData[axis].P);
}
// -----calculate I component
pidData[axis].I = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
pidData[axis].I = constrainf(iterm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
// -----calculate D component
if (pidCoefficient[axis].Kd > 0) {

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

@ -105,7 +105,7 @@ typedef struct pidProfile_s {
pidf_t pid[PID_ITEM_COUNT];
uint8_t dterm_filter_type; // Filter selection for dterm
uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation
uint8_t itermWindupPointPercent; // iterm windup threshold, percent motor saturation
uint16_t pidSumLimit;
uint16_t pidSumLimitYaw;
uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active.
@ -181,7 +181,7 @@ extern uint32_t targetPidLooptime;
extern float throttleBoost;
extern pt1Filter_t throttleLpf;
void pidResetITerm(void);
void pidResetIterm(void);
void pidStabilisationState(pidStabilisationState_e pidControllerState);
void pidSetItermAccelerator(float newItermAccelerator);
void pidInitFilters(const pidProfile_t *pidProfile);

2
src/test/unit/arming_prevention_unittest.cc
View file

@ -823,7 +823,7 @@ extern "C" {
void failsafeStartMonitoring(void) {}
void failsafeUpdateState(void) {}
bool failsafeIsActive(void) { return false; }
void pidResetITerm(void) {}
void pidResetIterm(void) {}
void updateAdjustmentStates(void) {}
void processRcAdjustments(controlRateConfig_t *) {}
void updateGpsWaypointsAndMode(void) {}

2
src/test/unit/vtx_unittest.cc
View file

@ -145,7 +145,7 @@ extern "C" {
void failsafeStartMonitoring(void) {}
void failsafeUpdateState(void) {}
bool failsafeIsActive(void) { return false; }
void pidResetITerm(void) {}
void pidResetIterm(void) {}
void updateAdjustmentStates(void) {}
void processRcAdjustments(controlRateConfig_t *) {}
void updateGpsWaypointsAndMode(void) {}