diff --git a/src/main/build/debug.c b/src/main/build/debug.c index ca953f2e06..7b3d65d4ad 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -83,4 +83,6 @@ const char * const debugModeNames[DEBUG_COUNT] = { "DSHOT_RPM_TELEMETRY", "RPM_FILTER", "D_MIN", + "AC_CORRECTION", + "AC_ERROR", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index ffc67acb9e..57c1385b8f 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -99,6 +99,8 @@ typedef enum { DEBUG_DSHOT_RPM_TELEMETRY, DEBUG_RPM_FILTER, DEBUG_D_MIN, + DEBUG_AC_CORRECTION, + DEBUG_AC_ERROR, DEBUG_COUNT } debugType_e; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 2b5f1b0cb6..879ec97dba 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1007,7 +1007,7 @@ STATIC_UNIT_TESTED void rotateItermAndAxisError() { if (itermRotation #if defined(USE_ABSOLUTE_CONTROL) - || acGain > 0 + || acGain > 0 || debugMode == DEBUG_AC_ERROR #endif ) { const float gyroToAngle = dT * RAD; @@ -1016,7 +1016,7 @@ STATIC_UNIT_TESTED void rotateItermAndAxisError() rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle; } #if defined(USE_ABSOLUTE_CONTROL) - if (acGain > 0) { + if (acGain > 0 || debugMode == DEBUG_AC_ERROR) { rotateVector(axisError, rotationRads); } #endif @@ -1076,7 +1076,7 @@ void FAST_CODE applySmartFeedforward(int axis) #if defined(USE_ABSOLUTE_CONTROL) STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate) { - if (acGain > 0) { + if (acGain > 0 || debugMode == DEBUG_AC_ERROR) { const float setpointLpf = pt1FilterApply(&acLpf[axis], *currentPidSetpoint); const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf); float acErrorRate = 0; @@ -1103,10 +1103,12 @@ STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRat const float acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit); *currentPidSetpoint += acCorrection; *itermErrorRate += acCorrection; + DEBUG_SET(DEBUG_AC_CORRECTION, axis, lrintf(acCorrection * 10)); if (axis == FD_ROLL) { DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(acCorrection * 10)); } } + DEBUG_SET(DEBUG_AC_ERROR, axis, lrintf(axisError[axis] * 10)); } } #endif