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

Lead-Lag Compensator (#12730)

Browse source 
* Implement Lead-Lag-Compensator

* Refactoring / Clean up

* Remove trailing whitespaces
This commit is contained in:
Jan Post 2023-05-03 10:01:36 +02:00 • committed by GitHub
parent 405627dec2
commit 460e4d00fe
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 151 additions and 92 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -91,7 +91,7 @@ pt1Filter_t throttleLpf;
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 3);
#if !defined(DEFAULT_PID_PROCESS_DENOM)
#if defined(STM32F411xE)
#if defined(STM32F411xE)
#define DEFAULT_PID_PROCESS_DENOM 2
#else
#define DEFAULT_PID_PROCESS_DENOM 1
@ -298,7 +298,7 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
static float previousThrottle = 0.0f;
const float throttleInv = 1.0f - throttle;
float throttleDerivative = fabsf(throttle - previousThrottle) * pidRuntime.pidFrequency;
DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(throttleDerivative * 100));
DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(throttleDerivative * 100));
throttleDerivative *= throttleInv * throttleInv;
// generally focus on the low throttle period
if (throttle > previousThrottle) {
@ -310,7 +310,7 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
// lower cutoff suppresses peaks relative to troughs and prolongs the effects
// PT2 smoothing of throttle derivative.
// 6 is a typical value for the peak boost factor with default cutoff of 6Hz
DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(throttleDerivative * 100));
DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(throttleDerivative * 100));
pidRuntime.antiGravityThrottleD = throttleDerivative;
}
@ -911,7 +911,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
if (levelMode == LEVEL_MODE_RP) {
// if earth referencing is requested, attenuate yaw axis setpoint when pitched or rolled
// and send yawSetpoint to Angle code to modulate pitch and roll
// code cost is 107 cycles when earthRef enabled, 20 otherwise, nearly all in cos_approx
// code cost is 107 cycles when earthRef enabled, 20 otherwise, nearly all in cos_approx
if (pidRuntime.angleEarthRef) {
pidRuntime.angleYawSetpoint = currentPidSetpoint;
float maxAngleTargetAbs = pidRuntime.angleEarthRef * fmaxf( fabsf(pidRuntime.angleTarget[FD_ROLL]), fabsf(pidRuntime.angleTarget[FD_PITCH]) );