From 6d2c1a3c01015cabfb413d80ebc217cb436f7e39 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Sun, 10 Jun 2018 08:40:04 +1000 Subject: [PATCH] initial debug changes --- src/main/flight/pid.c | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9c280b3b2..a26f31822 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -815,17 +815,23 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) { const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf); - if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < 60) { - itermErrorRate = (1 - setpointHpf / 60.0f) * (currentPidSetpoint - gyroRate); - } if (axis == FD_ROLL) { DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointLpf)); DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(setpointHpf)); - DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(currentPidSetpoint)); - DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(axisError[axis] * 10)); + } + if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < 60) { + itermErrorRate = (1 - setpointHpf / 60.0f) * (currentPidSetpoint - gyroRate); + if (axis == FD_ROLL) { + DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(currentPidSetpoint)); + DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf((1 - setpointHpf / 60.0f) * 100)); + } } if (itermRelaxType == ITERM_RELAX_GYRO ) { itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf); + if (axis == FD_ROLL) { + DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(gyroRate + setpointHpf)); + DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(gyroRate - setpointHpf)); + } } #if defined(USE_ABSOLUTE_CONTROL) @@ -845,6 +851,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT } else { acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate; } + if (axis == FD_ROLL) { + DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(acErrorRate)); + DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(axisError[axis] * 10)); + } + #endif // USE_ABSOLUTE_CONTROL } else #endif // USE_ITERM_RELAX -- 2.11.4.GIT