1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00

Rpm limiter initial throttle cap prediction fix (#13039)

* initial commit

* initial commit

* Update src/main/flight/mixer_init.c

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

* rpm limiter vbat compensation

* remove forced define

* change to multiply

* rpm limiter vbat compensation method 2

* fixes

* remove def

* test

* test

* test

* cleanup

* fix for maxcheck condition not satisfied if set to 2000

* Refactoring / cleanup

* Rename averageRpmFilter

* missing paren

* Move RPM estimation to motor.h

* Add macro for deadband

---------

Co-authored-by: Jan Post <Rm2k-Freak@web.de>
This commit is contained in:
Tanner Beard 2023-12-06 13:51:52 -08:00 committed by GitHub
parent 0b0c63a73d
commit 2733dadf02
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
5 changed files with 30 additions and 10 deletions

View file

@ -33,13 +33,15 @@
#include "config/feature.h" #include "config/feature.h"
#include "drivers/dshot.h" // for DSHOT_ constants in initEscEndpoints; may be gone in the future #include "drivers/dshot.h" // for DSHOT_ constants in initEscEndpoints; may be gone in the future
#include "drivers/pwm_output.h" // for PWM_TYPE_* and others
#include "drivers/time.h"
#include "drivers/dshot_bitbang.h" #include "drivers/dshot_bitbang.h"
#include "drivers/dshot_dpwm.h" #include "drivers/dshot_dpwm.h"
#include "drivers/pwm_output.h" // for PWM_TYPE_* and others
#include "drivers/time.h"
#include "fc/rc_controls.h" // for flight3DConfig_t #include "fc/rc_controls.h" // for flight3DConfig_t
#include "sensors/battery.h"
#include "motor.h" #include "motor.h"
static FAST_DATA_ZERO_INIT motorDevice_t *motorDevice; static FAST_DATA_ZERO_INIT motorDevice_t *motorDevice;
@ -343,6 +345,16 @@ void motorEnable(void)
} }
} }
float motorEstimateMaxRpm(void)
{
// Empirical testing found this relationship between estimated max RPM without props attached
// (unloaded) and measured max RPM with props attached (loaded), independent from prop size
float unloadedMaxRpm = 0.01f * getBatteryVoltage() * motorConfig()->kv;
float loadDerating = -5.44e-6f * unloadedMaxRpm + 0.944f;
return unloadedMaxRpm * loadDerating;
}
bool motorIsEnabled(void) bool motorIsEnabled(void)
{ {
return motorDevice->enabled; return motorDevice->enabled;

View file

@ -93,6 +93,7 @@ bool isMotorProtocolEnabled(void);
void motorDisable(void); void motorDisable(void);
void motorEnable(void); void motorEnable(void);
float motorEstimateMaxRpm(void);
bool motorIsEnabled(void); bool motorIsEnabled(void);
bool motorIsMotorEnabled(uint8_t index); bool motorIsMotorEnabled(uint8_t index);
timeMs_t motorGetMotorEnableTimeMs(void); timeMs_t motorGetMotorEnableTimeMs(void);

View file

@ -345,12 +345,13 @@ static void applyFlipOverAfterCrashModeToMotors(void)
} }
#ifdef USE_RPM_LIMIT #ifdef USE_RPM_LIMIT
#define STICK_HIGH_DEADBAND 5 // deadband to make sure throttle cap can raise, even with maxcheck set around 2000
static void applyRpmLimiter(mixerRuntime_t *mixer) static void applyRpmLimiter(mixerRuntime_t *mixer)
{ {
static float prevError = 0.0f; static float prevError = 0.0f;
static float i = 0.0f; static float i = 0.0f;
const float unsmoothedAverageRpm = getDshotRpmAverage(); const float unsmoothedAverageRpm = getDshotRpmAverage();
const float averageRpm = pt1FilterApply(&mixer->averageRpmFilter, unsmoothedAverageRpm); const float averageRpm = pt1FilterApply(&mixer->rpmLimiterAverageRpmFilter, unsmoothedAverageRpm);
const float error = averageRpm - mixer->rpmLimiterRpmLimit; const float error = averageRpm - mixer->rpmLimiterRpmLimit;
// PID // PID
@ -363,11 +364,13 @@ static void applyRpmLimiter(mixerRuntime_t *mixer)
// Throttle limit learning // Throttle limit learning
if (error > 0.0f && rcCommand[THROTTLE] < rxConfig()->maxcheck) { if (error > 0.0f && rcCommand[THROTTLE] < rxConfig()->maxcheck) {
mixer->rpmLimiterThrottleScale *= 1.0f - 4.8f * pidGetDT(); mixer->rpmLimiterThrottleScale *= 1.0f - 4.8f * pidGetDT();
} else if (pidOutput < -400 * pidGetDT() && rcCommand[THROTTLE] >= rxConfig()->maxcheck && !areMotorsSaturated()) { // Throttle accel corresponds with motor accel } else if (pidOutput < -400.0f * pidGetDT() && lrintf(rcCommand[THROTTLE]) >= rxConfig()->maxcheck - STICK_HIGH_DEADBAND && !areMotorsSaturated()) { // Throttle accel corresponds with motor accel
mixer->rpmLimiterThrottleScale *= 1.0f + 3.2f * pidGetDT(); mixer->rpmLimiterThrottleScale *= 1.0f + 3.2f * pidGetDT();
} }
mixer->rpmLimiterThrottleScale = constrainf(mixer->rpmLimiterThrottleScale, 0.01f, 1.0f); mixer->rpmLimiterThrottleScale = constrainf(mixer->rpmLimiterThrottleScale, 0.01f, 1.0f);
throttle *= mixer->rpmLimiterThrottleScale;
float rpmLimiterThrottleScaleOffset = pt1FilterApply(&mixer->rpmLimiterThrottleScaleOffsetFilter, constrainf(mixer->rpmLimiterRpmLimit / motorEstimateMaxRpm(), 0.0f, 1.0f) - mixer->rpmLimiterInitialThrottleScale);
throttle *= constrainf(mixer->rpmLimiterThrottleScale + rpmLimiterThrottleScaleOffset, 0.0f, 1.0f);
// Output // Output
pidOutput = MAX(0.0f, pidOutput); pidOutput = MAX(0.0f, pidOutput);
@ -375,7 +378,7 @@ static void applyRpmLimiter(mixerRuntime_t *mixer)
prevError = error; prevError = error;
DEBUG_SET(DEBUG_RPM_LIMIT, 0, lrintf(averageRpm)); DEBUG_SET(DEBUG_RPM_LIMIT, 0, lrintf(averageRpm));
DEBUG_SET(DEBUG_RPM_LIMIT, 1, lrintf(unsmoothedAverageRpm)); DEBUG_SET(DEBUG_RPM_LIMIT, 1, lrintf(rpmLimiterThrottleScaleOffset * 100.0f));
DEBUG_SET(DEBUG_RPM_LIMIT, 2, lrintf(mixer->rpmLimiterThrottleScale * 100.0f)); DEBUG_SET(DEBUG_RPM_LIMIT, 2, lrintf(mixer->rpmLimiterThrottleScale * 100.0f));
DEBUG_SET(DEBUG_RPM_LIMIT, 3, lrintf(throttle * 100.0f)); DEBUG_SET(DEBUG_RPM_LIMIT, 3, lrintf(throttle * 100.0f));
DEBUG_SET(DEBUG_RPM_LIMIT, 4, lrintf(error)); DEBUG_SET(DEBUG_RPM_LIMIT, 4, lrintf(error));

View file

@ -361,7 +361,8 @@ void mixerInitProfile(void)
mixerRuntime.rpmLimiterPGain = mixerConfig()->rpm_limit_p * 15e-6f; mixerRuntime.rpmLimiterPGain = mixerConfig()->rpm_limit_p * 15e-6f;
mixerRuntime.rpmLimiterIGain = mixerConfig()->rpm_limit_i * 1e-3f * pidGetDT(); mixerRuntime.rpmLimiterIGain = mixerConfig()->rpm_limit_i * 1e-3f * pidGetDT();
mixerRuntime.rpmLimiterDGain = mixerConfig()->rpm_limit_d * 3e-7f * pidGetPidFrequency(); mixerRuntime.rpmLimiterDGain = mixerConfig()->rpm_limit_d * 3e-7f * pidGetPidFrequency();
pt1FilterInit(&mixerRuntime.averageRpmFilter, pt1FilterGain(6.0f, pidGetDT())); pt1FilterInit(&mixerRuntime.rpmLimiterAverageRpmFilter, pt1FilterGain(6.0f, pidGetDT()));
pt1FilterInit(&mixerRuntime.rpmLimiterThrottleScaleOffsetFilter, pt1FilterGain(2.0f, pidGetDT()));
mixerResetRpmLimiter(); mixerResetRpmLimiter();
#endif #endif
} }
@ -369,9 +370,10 @@ void mixerInitProfile(void)
#ifdef USE_RPM_LIMIT #ifdef USE_RPM_LIMIT
void mixerResetRpmLimiter(void) void mixerResetRpmLimiter(void)
{ {
const float maxExpectedRpm = MAX(1.0f, motorConfig()->kv * getBatteryVoltage() * 0.01f); mixerRuntime.rpmLimiterThrottleScale = constrainf(mixerRuntime.rpmLimiterRpmLimit / motorEstimateMaxRpm(), 0.0f, 1.0f);
mixerRuntime.rpmLimiterThrottleScale = constrainf(mixerRuntime.rpmLimiterRpmLimit / maxExpectedRpm, 0.0f, 1.0f); mixerRuntime.rpmLimiterInitialThrottleScale = mixerRuntime.rpmLimiterThrottleScale;
} }
#endif // USE_RPM_LIMIT #endif // USE_RPM_LIMIT
#ifdef USE_LAUNCH_CONTROL #ifdef USE_LAUNCH_CONTROL

View file

@ -56,10 +56,12 @@ typedef struct mixerRuntime_s {
#if defined(USE_RPM_LIMIT) #if defined(USE_RPM_LIMIT)
float rpmLimiterRpmLimit; float rpmLimiterRpmLimit;
float rpmLimiterThrottleScale; float rpmLimiterThrottleScale;
float rpmLimiterInitialThrottleScale;
float rpmLimiterPGain; float rpmLimiterPGain;
float rpmLimiterIGain; float rpmLimiterIGain;
float rpmLimiterDGain; float rpmLimiterDGain;
pt1Filter_t averageRpmFilter; pt1Filter_t rpmLimiterAverageRpmFilter;
pt1Filter_t rpmLimiterThrottleScaleOffsetFilter;
#endif #endif
} mixerRuntime_t; } mixerRuntime_t;