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

Fix: RPM Limit - reset I-term at arm (#13214)

Browse source 
* RPM Limiter - reset I-term at arm

* Update src/main/flight/mixer.c

Co-authored-by: Jan Post <Rm2k-Freak@web.de>

---------

Co-authored-by: Jan Post <Rm2k-Freak@web.de>
This commit is contained in:
Ivan Efimov 2023-12-10 17:40:34 -06:00 • committed by GitHub
parent 4f5bb1abc0
commit 6ac08cdd58
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 10 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/fc/core.c
View file

@ -550,6 +550,9 @@ void tryArm(void)
#ifdef USE_OSD
osdSuppressStats(false);
#endif
#ifdef USE_RPM_LIMIT
mixerResetRpmLimiter();
#endif
ENABLE_ARMING_FLAG(ARMED);

9
src/main/flight/mixer.c
View file

@ -348,7 +348,6 @@ static void applyFlipOverAfterCrashModeToMotors(void)
static void applyRpmLimiter(mixerRuntime_t *mixer)
{
static float prevError = 0.0f;
static float i = 0.0f;
const float unsmoothedAverageRpm = getDshotRpmAverage();
const float averageRpm = pt1FilterApply(&mixer->rpmLimiterAverageRpmFilter, unsmoothedAverageRpm);
const float error = averageRpm - mixer->rpmLimiterRpmLimit;
@ -356,9 +355,9 @@ static void applyRpmLimiter(mixerRuntime_t *mixer)
// PID
const float p = error * mixer->rpmLimiterPGain;
const float d = (error - prevError) * mixer->rpmLimiterDGain; // rpmLimiterDGain already adjusted for looprate (see mixer_init.c)
i += error * mixer->rpmLimiterIGain; // rpmLimiterIGain already adjusted for looprate (see mixer_init.c)
i = MAX(0.0f, i);
float pidOutput = p + i + d;
mixer->rpmLimiterI += error * mixer->rpmLimiterIGain; // rpmLimiterIGain already adjusted for looprate (see mixer_init.c)
mixer->rpmLimiterI = MAX(0.0f, mixer->rpmLimiterI);
float pidOutput = p + mixer->rpmLimiterI + d;
// Throttle limit learning
if (error > 0.0f && rcCommand[THROTTLE] < rxConfig()->maxcheck) {
@ -382,7 +381,7 @@ static void applyRpmLimiter(mixerRuntime_t *mixer)
DEBUG_SET(DEBUG_RPM_LIMIT, 3, lrintf(throttle * 100.0f));
DEBUG_SET(DEBUG_RPM_LIMIT, 4, lrintf(error));
DEBUG_SET(DEBUG_RPM_LIMIT, 5, lrintf(p * 100.0f));
DEBUG_SET(DEBUG_RPM_LIMIT, 6, lrintf(i * 100.0f));
DEBUG_SET(DEBUG_RPM_LIMIT, 6, lrintf(mixer->rpmLimiterI * 100.0f));
DEBUG_SET(DEBUG_RPM_LIMIT, 7, lrintf(d * 100.0f));
}
#endif // USE_RPM_LIMIT

2
src/main/flight/mixer_init.c
View file

@ -361,6 +361,7 @@ void mixerInitProfile(void)
mixerRuntime.rpmLimiterPGain = mixerConfig()->rpm_limit_p * 15e-6f;
mixerRuntime.rpmLimiterIGain = mixerConfig()->rpm_limit_i * 1e-3f * pidGetDT();
mixerRuntime.rpmLimiterDGain = mixerConfig()->rpm_limit_d * 3e-7f * pidGetPidFrequency();
mixerRuntime.rpmLimiterI = 0.0;
pt1FilterInit(&mixerRuntime.rpmLimiterAverageRpmFilter, pt1FilterGain(6.0f, pidGetDT()));
pt1FilterInit(&mixerRuntime.rpmLimiterThrottleScaleOffsetFilter, pt1FilterGain(2.0f, pidGetDT()));
mixerResetRpmLimiter();
@ -373,6 +374,7 @@ void mixerInitProfile(void)
#ifdef USE_RPM_LIMIT
void mixerResetRpmLimiter(void)
{
mixerRuntime.rpmLimiterI = 0.0;
mixerRuntime.rpmLimiterThrottleScale = constrainf(mixerRuntime.rpmLimiterRpmLimit / motorEstimateMaxRpm(), 0.0f, 1.0f);
mixerRuntime.rpmLimiterInitialThrottleScale = mixerRuntime.rpmLimiterThrottleScale;
}

1
src/main/flight/mixer_init.h
View file

@ -60,6 +60,7 @@ typedef struct mixerRuntime_s {
float rpmLimiterPGain;
float rpmLimiterIGain;
float rpmLimiterDGain;
float rpmLimiterI;
pt1Filter_t rpmLimiterAverageRpmFilter;
pt1Filter_t rpmLimiterThrottleScaleOffsetFilter;
#endif