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

antigravity gain decoupled from Iterm

Browse source
This commit is contained in:
Nicola De Pasquale 2020-01-18 16:24:21 +01:00
parent a2aeb0cef4
commit 54f46c9327
3 changed files with 13 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/fc/rc.c
View file

@ -274,7 +274,7 @@ static void checkForThrottleErrorResetState(uint16_t rxRefreshRate)
if (ABS(rcCommandSpeed) > throttleVelocityThreshold) { if (ABS(rcCommandSpeed) > throttleVelocityThreshold) {
pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain)); pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain));
} else { } else {
pidSetItermAccelerator(1.0f); pidSetItermAccelerator(0.0f);
} }
} }
} }

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

@ -82,7 +82,7 @@ static FAST_RAM_ZERO_INIT float pidFrequency;
static FAST_RAM_ZERO_INIT uint8_t antiGravityMode; static FAST_RAM_ZERO_INIT uint8_t antiGravityMode;
static FAST_RAM_ZERO_INIT float antiGravityThrottleHpf; static FAST_RAM_ZERO_INIT float antiGravityThrottleHpf;
static FAST_RAM_ZERO_INIT uint16_t itermAcceleratorGain; static FAST_RAM_ZERO_INIT uint16_t itermAcceleratorGain;
static FAST_RAM float antiGravityOsdCutoff = 1.0f; static FAST_RAM float antiGravityOsdCutoff = 0.0f;
static FAST_RAM_ZERO_INIT bool antiGravityEnabled; static FAST_RAM_ZERO_INIT bool antiGravityEnabled;
static FAST_RAM_ZERO_INIT bool zeroThrottleItermReset; static FAST_RAM_ZERO_INIT bool zeroThrottleItermReset;
@ -242,7 +242,7 @@ static void pidSetTargetLooptime(uint32_t pidLooptime)
#endif #endif
} }
static FAST_RAM float itermAccelerator = 1.0f; static FAST_RAM float itermAccelerator = 0.0f;
void pidSetItermAccelerator(float newItermAccelerator) void pidSetItermAccelerator(float newItermAccelerator)
{ {
@ -656,7 +656,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
// For the new AG it's a continuous floating value so we want to trigger the OSD // For the new AG it's a continuous floating value so we want to trigger the OSD
// display when it exceeds 25% of its possible range. This gives a useful indication // display when it exceeds 25% of its possible range. This gives a useful indication
// of AG activity without excessive display. // of AG activity without excessive display.
antiGravityOsdCutoff = 1.0f; antiGravityOsdCutoff = 0.0f;
if (antiGravityMode == ANTI_GRAVITY_SMOOTH) { if (antiGravityMode == ANTI_GRAVITY_SMOOTH) {
antiGravityOsdCutoff += ((itermAcceleratorGain - 1000) / 1000.0f) * 0.25f; antiGravityOsdCutoff += ((itermAcceleratorGain - 1000) / 1000.0f) * 0.25f;
} }
@ -1304,13 +1304,15 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// Dynamic i component, // Dynamic i component,
if ((antiGravityMode == ANTI_GRAVITY_SMOOTH) && antiGravityEnabled) { if ((antiGravityMode == ANTI_GRAVITY_SMOOTH) && antiGravityEnabled) {
itermAccelerator = 1 + fabsf(antiGravityThrottleHpf) * 0.01f * (itermAcceleratorGain - 1000); itermAccelerator = fabsf(antiGravityThrottleHpf) * 0.01f * (itermAcceleratorGain - 1000);
DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(antiGravityThrottleHpf * 1000)); DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(antiGravityThrottleHpf * 1000));
} }
DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(itermAccelerator * 1000)); DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(itermAccelerator * 1000));
float agGain = dT * itermAccelerator * AG_KI;
// gradually scale back integration when above windup point // gradually scale back integration when above windup point
float dynCi = dT * itermAccelerator; float dynCi = dT;
if (itermWindupPointInv > 1.0f) { if (itermWindupPointInv > 1.0f) {
dynCi *= constrainf((1.0f - getMotorMixRange()) * itermWindupPointInv, 0.0f, 1.0f); dynCi *= constrainf((1.0f - getMotorMixRange()) * itermWindupPointInv, 0.0f, 1.0f);
} }
@ -1420,7 +1422,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
#else #else
const float Ki = pidCoefficient[axis].Ki; const float Ki = pidCoefficient[axis].Ki;
#endif #endif
pidData[axis].I = constrainf(previousIterm + Ki * itermErrorRate * dynCi, -itermLimit, itermLimit); pidData[axis].I = constrainf(previousIterm + Ki * itermErrorRate * dynCi + agGain * itermErrorRate, -itermLimit, itermLimit);
// -----calculate pidSetpointDelta // -----calculate pidSetpointDelta
float pidSetpointDelta = 0; float pidSetpointDelta = 0;
@ -1587,7 +1589,7 @@ void pidSetAntiGravityState(bool newState)
{ {
if (newState != antiGravityEnabled) { if (newState != antiGravityEnabled) {
// reset the accelerator on state changes // reset the accelerator on state changes
itermAccelerator = 1.0f; itermAccelerator = 0.0f;
} }
antiGravityEnabled = newState; antiGravityEnabled = newState;
} }

3
src/main/flight/pid.h
View file

@ -48,6 +48,9 @@
#define ITERM_RELAX_SETPOINT_THRESHOLD 40.0f #define ITERM_RELAX_SETPOINT_THRESHOLD 40.0f
#define ITERM_RELAX_CUTOFF_DEFAULT 20 #define ITERM_RELAX_CUTOFF_DEFAULT 20
// Anti gravity I constant
#define AG_KI 21.586988f;
typedef enum { typedef enum {
PID_ROLL, PID_ROLL,
PID_PITCH, PID_PITCH,