diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index fddd7fb63c..ba3616f12b 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1057,9 +1057,8 @@ const clivalue_t valueTable[] = { #endif #ifdef USE_DYN_IDLE - { "idle_hz", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_hz) }, + { "idle_min_rpm", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_min_rpm) }, { "idle_adjustment_speed", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 25, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_adjustment_speed) }, - { "idle_throttle", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_throttle) }, { "idle_p", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_p) }, { "idle_pid_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_pid_limit) }, { "idle_max_increase", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_max_increase) }, diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index ebf91e4d99..e92d51308f 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -37,6 +37,7 @@ #include "pg/motor.h" #include "pg/rx.h" +#include "drivers/dshot.h" #include "drivers/motor.h" #include "drivers/time.h" #include "drivers/io.h" @@ -288,6 +289,13 @@ FAST_RAM_ZERO_INIT float motorOutputHigh, motorOutputLow; static FAST_RAM_ZERO_INIT float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; static FAST_RAM_ZERO_INIT float rcCommandThrottleRange; +#ifdef USE_DYN_IDLE +static FAST_RAM_ZERO_INIT float idleMaxIncrease; +static FAST_RAM_ZERO_INIT float idleThrottleOffset; +static FAST_RAM_ZERO_INIT float idleMinMotorRps; +static FAST_RAM_ZERO_INIT float idleP; +#endif + uint8_t getMotorCount(void) { @@ -348,6 +356,12 @@ void mixerInit(mixerMode_e mixerMode) mixerTricopterInit(); } #endif +#ifdef USE_DYN_IDLE + idleMinMotorRps = currentPidProfile->idle_min_rpm * 0.01f * 60.0f; + idleMaxIncrease = currentPidProfile->idle_max_increase * 0.001f; + idleThrottleOffset = motorConfig()->digitalIdleOffsetValue * 0.0001f; + idleP = currentPidProfile->idle_p * 0.0001f; +#endif } #ifdef USE_LAUNCH_CONTROL @@ -471,6 +485,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; + static float oldMinRps; float currentThrottleInputRange = 0; if (featureIsEnabled(FEATURE_3D)) { @@ -575,26 +590,26 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) pidResetIterm(); } } else { + throttle = rcCommand[THROTTLE] - PWM_RANGE_MIN + throttleAngleCorrection; #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 = rpmMinMotorFrequency(); - const float targetRpmChangeRate = (currentPidProfile->idle_hz - minRpm) * currentPidProfile->idle_adjustment_speed; - 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); + if (idleMinMotorRps > 0.0f) { + motorOutputLow = DSHOT_MIN_THROTTLE; + const float maxIncrease = isAirmodeActivated() ? idleMaxIncrease : 0.04f; + const float minRps = rpmMinMotorFrequency(); + const float targetRpsChangeRate = (idleMinMotorRps - minRps) * currentPidProfile->idle_adjustment_speed; + const float error = targetRpsChangeRate - (minRps - oldMinRps) * pidGetPidFrequency(); + const float pidSum = constrainf(idleP * error, -currentPidProfile->idle_pid_limit, currentPidProfile->idle_pid_limit); motorRangeMinIncrease = constrainf(motorRangeMinIncrease + pidSum * pidGetDT(), 0.0f, maxIncrease); - oldMinRpm = minRpm; + oldMinRps = minRps; + throttle += idleThrottleOffset; DEBUG_SET(DEBUG_DYN_IDLE, 0, motorRangeMinIncrease * 1000); - DEBUG_SET(DEBUG_DYN_IDLE, 1, targetRpmChangeRate); + DEBUG_SET(DEBUG_DYN_IDLE, 1, targetRpsChangeRate); DEBUG_SET(DEBUG_DYN_IDLE, 2, error); - DEBUG_SET(DEBUG_DYN_IDLE, 3, minRpm); + DEBUG_SET(DEBUG_DYN_IDLE, 3, minRps); } #endif - - throttle = rcCommand[THROTTLE] - PWM_RANGE_MIN + throttleAngleCorrection; currentThrottleInputRange = rcCommandThrottleRange; motorRangeMin = motorOutputLow + motorRangeMinIncrease * (motorOutputHigh - motorOutputLow); motorRangeMax = motorOutputHigh; @@ -854,12 +869,6 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa throttle = pidCompensateThrustLinearization(throttle); #endif -#ifdef USE_DYN_IDLE - if (currentPidProfile->idle_hz) { - throttle += currentPidProfile->idle_throttle * 0.001f; - } -#endif - #if defined(USE_THROTTLE_BOOST) if (throttleBoost > 0.0f) { const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3db33cbfcb..817ce52b3a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -206,9 +206,8 @@ void resetPidProfile(pidProfile_t *pidProfile) .auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY, .transient_throttle_limit = 15, .profileName = { 0 }, - .idle_hz = 0, + .idle_min_rpm = 0, .idle_adjustment_speed = 50, - .idle_throttle = 60, .idle_p = 50, .idle_pid_limit = 200, .idle_max_increase = 150, diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 25cccf2a9f..3dbfdf575d 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -172,8 +172,7 @@ typedef struct pidProfile_s { uint8_t transient_throttle_limit; // Maximum DC component of throttle change to mix into throttle to prevent airmode mirroring noise char profileName[MAX_PROFILE_NAME_LENGTH + 1]; // Descriptive name for profile - uint8_t idle_hz; // minimum motor speed enforced by integrating p controller - uint8_t idle_throttle; // added to throttle, replaces dshot_idle_value + uint8_t idle_min_rpm; // minimum motor speed enforced by integrating p controller uint8_t idle_adjustment_speed; // how quickly the integrating p controller tries to correct uint8_t idle_p; // kP uint8_t idle_pid_limit; // max P