Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 05:45:31 +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_lastErrorForDelta[3];
float unittest_pidLuxFloat_delta1[3]; float unittest_pidLuxFloat_delta1[3];
float unittest_pidLuxFloat_delta2[3]; float unittest_pidLuxFloat_delta2[3];
float unittest_pidLuxFloat_PTerm[3]; float unittest_pidLuxFloat_pterm[3];
float unittest_pidLuxFloat_ITerm[3]; float unittest_pidLuxFloat_iterm[3];
float unittest_pidLuxFloat_DTerm[3]; float unittest_pidLuxFloat_dterm[3];
#define SET_PID_LUX_FLOAT_LOCALS(axis) \ #define SET_PID_LUX_FLOAT_LOCALS(axis) \
{ \ { \
@ -66,15 +66,15 @@ float unittest_pidLuxFloat_DTerm[3];
unittest_pidLuxFloat_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \ unittest_pidLuxFloat_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \
unittest_pidLuxFloat_delta1[axis] = delta1[axis]; \ unittest_pidLuxFloat_delta1[axis] = delta1[axis]; \
unittest_pidLuxFloat_delta2[axis] = delta2[axis]; \ unittest_pidLuxFloat_delta2[axis] = delta2[axis]; \
unittest_pidLuxFloat_PTerm[axis] = PTerm; \ unittest_pidLuxFloat_pterm[axis] = pterm; \
unittest_pidLuxFloat_ITerm[axis] = ITerm; \ unittest_pidLuxFloat_iterm[axis] = iterm; \
unittest_pidLuxFloat_DTerm[axis] = DTerm; \ unittest_pidLuxFloat_dterm[axis] = dterm; \
} }
int32_t unittest_pidMultiWiiRewrite_lastErrorForDelta[3]; int32_t unittest_pidMultiWiiRewrite_lastErrorForDelta[3];
int32_t unittest_pidMultiWiiRewrite_PTerm[3]; int32_t unittest_pidMultiWiiRewrite_pterm[3];
int32_t unittest_pidMultiWiiRewrite_ITerm[3]; int32_t unittest_pidMultiWiiRewrite_iterm[3];
int32_t unittest_pidMultiWiiRewrite_DTerm[3]; int32_t unittest_pidMultiWiiRewrite_dterm[3];
#define SET_PID_MULTI_WII_REWRITE_LOCALS(axis) \ #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) \ #define GET_PID_MULTI_WII_REWRITE_LOCALS(axis) \
{ \ { \
unittest_pidMultiWiiRewrite_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \ unittest_pidMultiWiiRewrite_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \
unittest_pidMultiWiiRewrite_PTerm[axis] = PTerm; \ unittest_pidMultiWiiRewrite_pterm[axis] = pterm; \
unittest_pidMultiWiiRewrite_ITerm[axis] = ITerm; \ unittest_pidMultiWiiRewrite_iterm[axis] = iterm; \
unittest_pidMultiWiiRewrite_DTerm[axis] = DTerm; \ unittest_pidMultiWiiRewrite_dterm[axis] = dterm; \
} }
#else #else

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

@ -577,16 +577,16 @@ bool processRx(timeUs_t currentTimeUs)
if (isAirmodeActive() && ARMING_FLAG(ARMED)) { if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
if (throttlePercent >= rxConfig()->airModeActivateThreshold) { if (throttlePercent >= rxConfig()->airModeActivateThreshold) {
airmodeIsActivated = true; // Prevent Iterm from being reset airmodeIsActivated = true; // Prevent iterm from being reset
} }
} else { } else {
airmodeIsActivated = false; airmodeIsActivated = false;
} }
/* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. /* 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 */ 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) { if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) {
pidResetITerm(); pidResetIterm();
if (currentPidProfile->pidAtMinThrottle) if (currentPidProfile->pidAtMinThrottle)
pidStabilisationState(PID_STABILISATION_ON); pidStabilisationState(PID_STABILISATION_ON);
else else

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

@ -596,8 +596,8 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
currentThrottleInputRange = rcCommandThrottleRange3dHigh; currentThrottleInputRange = rcCommandThrottleRange3dHigh;
} }
if (currentTimeUs - reversalTimeUs < 250000) { if (currentTimeUs - reversalTimeUs < 250000) {
// keep ITerm zero for 250ms after motor reversal // keep iterm zero for 250ms after motor reversal
pidResetITerm(); pidResetIterm();
} }
} else { } else {
throttle = rcCommand[THROTTLE] - rxConfig()->mincheck + throttleAngleCorrection; 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 = {
[PID_ROLL] = { 46, 45, 25, 60 }, [PID_ROLL] = { 46, 45, 25, 60 },
[PID_PITCH] = { 50, 50, 27, 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_LEVEL] = { 50, 50, 75, 0 },
[PID_MAG] = { 40, 0, 0, 0 }, [PID_MAG] = { 40, 0, 0, 0 },
}, },
@ -130,12 +130,12 @@ void resetPidProfile(pidProfile_t *pidProfile)
.dterm_notch_hz = 0, .dterm_notch_hz = 0,
.dterm_notch_cutoff = 0, .dterm_notch_cutoff = 0,
.dterm_filter_type = FILTER_PT1, .dterm_filter_type = FILTER_PT1,
.itermWindupPointPercent = 100, .itermWindupPointPercent = 40,
.vbatPidCompensation = 0, .vbatPidCompensation = 0,
.pidAtMinThrottle = PID_STABILISATION_ON, .pidAtMinThrottle = PID_STABILISATION_ON,
.levelAngleLimit = 55, .levelAngleLimit = 55,
.feedForwardTransition = 0, .feedForwardTransition = 0,
.yawRateAccelLimit = 100, .yawRateAccelLimit = 0,
.rateAccelLimit = 0, .rateAccelLimit = 0,
.itermThrottleThreshold = 250, .itermThrottleThreshold = 250,
.itermAcceleratorGain = 5000, .itermAcceleratorGain = 5000,
@ -150,7 +150,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.horizon_tilt_effect = 75, .horizon_tilt_effect = 75,
.horizon_tilt_expert_mode = false, .horizon_tilt_expert_mode = false,
.crash_limit_yaw = 200, .crash_limit_yaw = 200,
.itermLimit = 150, .itermLimit = 300,
.throttle_boost = 5, .throttle_boost = 5,
.throttle_boost_cutoff = 15, .throttle_boost_cutoff = 15,
.iterm_rotation = true, .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 maxVelocity[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float feedForwardTransition; static FAST_RAM_ZERO_INIT float feedForwardTransition;
static FAST_RAM_ZERO_INIT float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio; 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 uint8_t horizonTiltExpertMode;
static FAST_RAM_ZERO_INIT timeDelta_t crashTimeLimitUs; static FAST_RAM_ZERO_INIT timeDelta_t crashTimeLimitUs;
static FAST_RAM_ZERO_INIT timeDelta_t crashTimeDelayUs; 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; static FAST_RAM_ZERO_INIT float acErrorLimit;
#endif #endif
void pidResetITerm(void) void pidResetIterm(void)
{ {
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
pidData[axis].I = 0.0f; pidData[axis].I = 0.0f;
@ -443,12 +443,10 @@ void pidInitConfig(const pidProfile_t *pidProfile)
horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f; horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f;
maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * dT; maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * dT;
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT;
ITermWindupPointInv = 0.0f; itermWindupPointInv = 1.0f;
if (pidProfile->itermWindupPointPercent < 100) { if (pidProfile->itermWindupPointPercent < 100) {
float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f;
ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint); itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint);
} else {
ITermWindupPointInv = 0.0f;
} }
itermAcceleratorGain = pidProfile->itermAcceleratorGain; itermAcceleratorGain = pidProfile->itermAcceleratorGain;
crashTimeLimitUs = pidProfile->crash_time * 1000; crashTimeLimitUs = pidProfile->crash_time * 1000;
@ -634,8 +632,8 @@ static void handleCrashRecovery(
*errorRate = *currentPidSetpoint - gyroRate; *errorRate = *currentPidSetpoint - gyroRate;
} }
} }
// reset ITerm, since accumulated error before crash is now meaningless // reset iterm, since accumulated error before crash is now meaningless
// and ITerm windup during crash recovery can be extreme, especially on yaw axis // and iterm windup during crash recovery can be extreme, especially on yaw axis
pidData[axis].I = 0.0f; pidData[axis].I = 0.0f;
if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs
|| (getMotorMixRange() < 1.0f || (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 (itermRotation
#if defined(USE_ABSOLUTE_CONTROL) #if defined(USE_ABSOLUTE_CONTROL)
@ -883,7 +881,7 @@ static void applyAbsoluteControl(const int axis, const float gyroRate, const boo
#endif #endif
#if defined(USE_ITERM_RELAX) #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 gyroRate, float *itermErrorRate, float *currentPidSetpoint)
{ {
const float setpointLpf = pt1FilterApply(&windupLpf[axis], *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 float itermRelaxFactor = 1 - setpointHpf / ITERM_RELAX_SETPOINT_THRESHOLD;
const bool isDecreasingI = 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) { if ((itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) {
// Do Nothing, use the precalculed itermErrorRate // Do Nothing, use the precalculed itermErrorRate
} else if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < ITERM_RELAX_SETPOINT_THRESHOLD) { } 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]; static float previousPidSetpoint[XYZ_AXIS_COUNT];
const float tpaFactor = getThrottlePIDAttenuation(); const float tpaFactor = getThrottlePIDAttenuation();
const float motorMixRange = getMotorMixRange();
#ifdef USE_YAW_SPIN_RECOVERY #ifdef USE_YAW_SPIN_RECOVERY
const bool yawSpinActive = gyroYawSpinDetected(); 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 // gradually scale back integration when above windup point
float dynCi = dT * itermAccelerator; float dynCi = dT * itermAccelerator;
if (ITermWindupPointInv > 0) { if (itermWindupPointInv > 1.0f) {
dynCi *= constrainf((1.0f - motorMixRange) * ITermWindupPointInv, 0.0f, 1.0f); dynCi *= constrainf((1.0f - getMotorMixRange()) * itermWindupPointInv, 0.0f, 1.0f);
} }
// Precalculate gyro deta for D-term here, this allows loop unrolling // 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]); gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]);
} }
rotateITermAndAxisError(); rotateItermAndAxisError();
// ----------PID controller---------- // ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { 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, pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate,
&currentPidSetpoint, &errorRate); &currentPidSetpoint, &errorRate);
const float ITerm = pidData[axis].I; const float iterm = pidData[axis].I;
float itermErrorRate = errorRate; float itermErrorRate = errorRate;
#if defined(USE_ITERM_RELAX) #if defined(USE_ITERM_RELAX)
applyItermRelax(axis, ITerm, gyroRate, &itermErrorRate, &currentPidSetpoint); applyItermRelax(axis, iterm, gyroRate, &itermErrorRate, &currentPidSetpoint);
#endif #endif
// --------low-level gyro-based PID based on 2DOF PID controller. ---------- // --------low-level gyro-based PID based on 2DOF PID controller. ----------
// 2-DOF PID controller with optional filter on derivative term. // 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). // 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; pidData[axis].P = pidCoefficient[axis].Kp * errorRate * tpaFactor;
if (axis == FD_YAW) { if (axis == FD_YAW) {
pidData[axis].P = ptermYawLowpassApplyFn((filter_t *) &ptermYawLowpass, pidData[axis].P); pidData[axis].P = ptermYawLowpassApplyFn((filter_t *) &ptermYawLowpass, pidData[axis].P);
} }
// -----calculate I component // -----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 // -----calculate D component
if (pidCoefficient[axis].Kd > 0) { 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]; pidf_t pid[PID_ITEM_COUNT];
uint8_t dterm_filter_type; // Filter selection for dterm 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 pidSumLimit;
uint16_t pidSumLimitYaw; uint16_t pidSumLimitYaw;
uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active. 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 float throttleBoost;
extern pt1Filter_t throttleLpf; extern pt1Filter_t throttleLpf;
void pidResetITerm(void); void pidResetIterm(void);
void pidStabilisationState(pidStabilisationState_e pidControllerState); void pidStabilisationState(pidStabilisationState_e pidControllerState);
void pidSetItermAccelerator(float newItermAccelerator); void pidSetItermAccelerator(float newItermAccelerator);
void pidInitFilters(const pidProfile_t *pidProfile); 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 failsafeStartMonitoring(void) {}
void failsafeUpdateState(void) {} void failsafeUpdateState(void) {}
bool failsafeIsActive(void) { return false; } bool failsafeIsActive(void) { return false; }
void pidResetITerm(void) {} void pidResetIterm(void) {}
void updateAdjustmentStates(void) {} void updateAdjustmentStates(void) {}
void processRcAdjustments(controlRateConfig_t *) {} void processRcAdjustments(controlRateConfig_t *) {}
void updateGpsWaypointsAndMode(void) {} void updateGpsWaypointsAndMode(void) {}

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

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