1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 01:35:41 +03:00

address feedback

This commit is contained in:
Thorsten Laux 2019-07-30 07:31:00 +02:00
parent bca9f8a587
commit 293dc42710
6 changed files with 34 additions and 18 deletions

View file

@ -470,6 +470,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
{
static uint16_t rcThrottlePrevious = 0; // Store the last throttle direction for deadband transitions
static timeUs_t reversalTimeUs = 0; // time when motors last reversed in 3D mode
static float motorRangeMinIncrease = 0;
float currentThrottleInputRange = 0;
if (featureIsEnabled(FEATURE_3D)) {
@ -574,16 +575,15 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
pidResetIterm();
}
} else {
static float motorRangeMinIncrease = 0;
#ifdef USE_DYN_IDLE
if (currentPidProfile->idle_hz) {
static float oldMinRpm;
const float maxIncrease = isAirmodeActivated() ? currentPidProfile->idle_max_increase * 0.001f : 0.04f;
const float minRpm = rpmMinMotorSpeed();
const float minRpm = rpmMinMotorFrequency();
const float targetRpmChangeRate = (currentPidProfile->idle_hz - minRpm) * currentPidProfile->idle_adjustment_speed;
const float error = targetRpmChangeRate - (minRpm - oldMinRpm) * pidFrequency;
const float error = targetRpmChangeRate - (minRpm - oldMinRpm) * pidGetPidFrequency();
const float pidSum = constrainf(currentPidProfile->idle_p * 0.0001f * error, -currentPidProfile->idle_pid_limit, currentPidProfile->idle_pid_limit);
motorRangeMinIncrease = constrainf(motorRangeMinIncrease + pidSum * dT, 0.0f, maxIncrease);
motorRangeMinIncrease = constrainf(motorRangeMinIncrease + pidSum * pidGetDT(), 0.0f, maxIncrease);
oldMinRpm = minRpm;
DEBUG_SET(DEBUG_DYN_IDLE, 0, motorRangeMinIncrease * 1000);

View file

@ -75,8 +75,8 @@ static FAST_RAM_ZERO_INIT bool pidStabilisationEnabled;
static FAST_RAM_ZERO_INIT bool inCrashRecoveryMode = false;
FAST_RAM_ZERO_INIT float dT;
FAST_RAM_ZERO_INIT float pidFrequency;
static FAST_RAM_ZERO_INIT float dT;
static FAST_RAM_ZERO_INIT float pidFrequency;
static FAST_RAM_ZERO_INIT uint8_t antiGravityMode;
static FAST_RAM_ZERO_INIT float antiGravityThrottleHpf;
@ -129,7 +129,7 @@ static FAST_RAM_ZERO_INIT float airmodeThrottleOffsetLimit;
#define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 11);
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 12);
void resetPidProfile(pidProfile_t *pidProfile)
{
@ -1545,3 +1545,13 @@ float pidGetPreviousSetpoint(int axis)
{
return previousPidSetpoint[axis];
}
float pidGetDT()
{
return dT;
}
float pidGetPidFrequency()
{
return pidFrequency;
}

View file

@ -254,8 +254,6 @@ float calcHorizonLevelStrength(void);
void dynLpfDTermUpdate(float throttle);
void pidSetItermReset(bool enabled);
float pidGetPreviousSetpoint(int axis);
extern float dT;
extern float pidFrequency;
float pidGetDT();
float pidGetPidFrequency();

View file

@ -65,6 +65,7 @@ typedef struct rpmNotchFilter_s
FAST_RAM_ZERO_INIT static float erpmToHz;
FAST_RAM_ZERO_INIT static float filteredMotorErpm[MAX_SUPPORTED_MOTORS];
FAST_RAM_ZERO_INIT static float minMotorFrequency;
FAST_RAM_ZERO_INIT static uint8_t numberFilters;
FAST_RAM_ZERO_INIT static uint8_t numberRpmNotchFilters;
FAST_RAM_ZERO_INIT static uint8_t filterUpdatesPerIteration;
@ -212,6 +213,7 @@ FAST_CODE_NOINLINE void rpmFilterUpdate()
motor = 0;
}
motorFrequency[motor] = erpmToHz * filteredMotorErpm[motor];
minMotorFrequency = 0.0f;
}
currentFilter = &filters[filter];
}
@ -224,15 +226,17 @@ bool isRpmFilterEnabled(void)
return (motorConfig()->dev.useDshotTelemetry && (rpmFilterConfig()->gyro_rpm_notch_harmonics || rpmFilterConfig()->dterm_rpm_notch_harmonics));
}
float rpmMinMotorSpeed()
float rpmMinMotorFrequency()
{
float minSpeed = 10000.0f;
if (minMotorFrequency == 0.0f) {
minMotorFrequency = 10000.0f;
for (int i = getMotorCount(); i--;) {
if (motorFrequency[i] < minSpeed) {
minSpeed = motorFrequency[i];
if (motorFrequency[i] < minMotorFrequency) {
minMotorFrequency = motorFrequency[i];
}
}
return minSpeed;
}
return minMotorFrequency;
}

View file

@ -43,4 +43,4 @@ float rpmFilterGyro(int axis, float values);
float rpmFilterDterm(int axis, float values);
void rpmFilterUpdate();
bool isRpmFilterEnabled(void);
float rpmMinMotorSpeed();
float rpmMinMotorFrequency();

View file

@ -364,3 +364,7 @@ extern uint8_t __config_end;
#if defined(USE_EXST)
#define USE_FLASH_BOOT_LOADER
#endif
#if !defined(USE_RPM_FILTER)
#undef USE_DYN_IDLE
#endif