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:
parent
0b0c63a73d
commit
2733dadf02
5 changed files with 30 additions and 10 deletions
|
@ -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;
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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));
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue