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

add abs control debug modes

Browse source
This commit is contained in:
Thorsten Laux 2019-03-27 08:01:12 +01:00
parent 173e958daf
commit fb6558a142
3 changed files with 9 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/build/debug.c
View file

@ -83,4 +83,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"DSHOT_RPM_TELEMETRY", "DSHOT_RPM_TELEMETRY",
"RPM_FILTER", "RPM_FILTER",
"D_MIN", "D_MIN",
"AC_CORRECTION",
"AC_ERROR",
}; };

2
src/main/build/debug.h
View file

@ -99,6 +99,8 @@ typedef enum {
DEBUG_DSHOT_RPM_TELEMETRY, DEBUG_DSHOT_RPM_TELEMETRY,
DEBUG_RPM_FILTER, DEBUG_RPM_FILTER,
DEBUG_D_MIN, DEBUG_D_MIN,
DEBUG_AC_CORRECTION,
DEBUG_AC_ERROR,
DEBUG_COUNT DEBUG_COUNT
} debugType_e; } debugType_e;

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

@ -1007,7 +1007,7 @@ STATIC_UNIT_TESTED void rotateItermAndAxisError()
{ {
if (itermRotation if (itermRotation
#if defined(USE_ABSOLUTE_CONTROL) #if defined(USE_ABSOLUTE_CONTROL)
|| acGain > 0 || acGain > 0 || debugMode == DEBUG_AC_ERROR
#endif #endif
) { ) {
const float gyroToAngle = dT * RAD; const float gyroToAngle = dT * RAD;
@ -1016,7 +1016,7 @@ STATIC_UNIT_TESTED void rotateItermAndAxisError()
rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle; rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle;
} }
#if defined(USE_ABSOLUTE_CONTROL) #if defined(USE_ABSOLUTE_CONTROL)
if (acGain > 0) { if (acGain > 0 || debugMode == DEBUG_AC_ERROR) {
rotateVector(axisError, rotationRads); rotateVector(axisError, rotationRads);
} }
#endif #endif
@ -1076,7 +1076,7 @@ void FAST_CODE applySmartFeedforward(int axis)
#if defined(USE_ABSOLUTE_CONTROL) #if defined(USE_ABSOLUTE_CONTROL)
STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate) 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 setpointLpf = pt1FilterApply(&acLpf[axis], *currentPidSetpoint);
const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf); const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf);
float acErrorRate = 0; 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); const float acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit);
*currentPidSetpoint += acCorrection; *currentPidSetpoint += acCorrection;
*itermErrorRate += acCorrection; *itermErrorRate += acCorrection;
DEBUG_SET(DEBUG_AC_CORRECTION, axis, lrintf(acCorrection * 10));
if (axis == FD_ROLL) { if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(acCorrection * 10)); DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(acCorrection * 10));
} }
} }
DEBUG_SET(DEBUG_AC_ERROR, axis, lrintf(axisError[axis] * 10));
} }
} }
#endif #endif