Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +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",
"RPM_FILTER",
"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_RPM_FILTER,
DEBUG_D_MIN,
DEBUG_AC_CORRECTION,
DEBUG_AC_ERROR,
DEBUG_COUNT
} debugType_e;

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

@ -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