Retain dynamic idle settings, reduce dependencies
This commit is contained in:
parent
2e7515dd60
commit
7b0b8ee35d
|
@ -522,14 +522,6 @@ static void validateAndFixConfig(void)
|
|||
if ((!configuredMotorProtocolDshot || (motorConfig()->dev.useDshotBitbang == DSHOT_BITBANG_OFF && (motorConfig()->dev.useBurstDshot == DSHOT_DMAR_ON || nChannelTimerUsed)) || systemConfig()->schedulerOptimizeRate == SCHEDULER_OPTIMIZE_RATE_OFF) && motorConfig()->dev.useDshotTelemetry) {
|
||||
motorConfigMutable()->dev.useDshotTelemetry = false;
|
||||
}
|
||||
|
||||
#if defined(USE_DYN_IDLE)
|
||||
if (!isRpmFilterEnabled()) {
|
||||
for (unsigned i = 0; i < PID_PROFILE_COUNT; i++) {
|
||||
pidProfilesMutable(i)->dyn_idle_min_rpm = 0;
|
||||
}
|
||||
}
|
||||
#endif // USE_DYN_IDLE
|
||||
#endif // USE_DSHOT_TELEMETRY
|
||||
#endif // USE_DSHOT
|
||||
|
||||
|
|
|
@ -105,7 +105,6 @@ static FAST_DATA_ZERO_INIT float motorRangeMax;
|
|||
static FAST_DATA_ZERO_INIT float motorOutputRange;
|
||||
static FAST_DATA_ZERO_INIT int8_t motorOutputMixSign;
|
||||
|
||||
|
||||
static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
|
||||
{
|
||||
static uint16_t rcThrottlePrevious = 0; // Store the last throttle direction for deadband transitions
|
||||
|
@ -220,7 +219,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
|
|||
|
||||
#ifdef USE_DYN_IDLE
|
||||
if (mixerRuntime.dynIdleMinRps > 0.0f) {
|
||||
const float maxIncrease = isAirmodeActivated() ? mixerRuntime.dynIdleMaxIncrease : 0.04f;
|
||||
const float maxIncrease = isAirmodeActivated() ? mixerRuntime.dynIdleMaxIncrease : 0.05f;
|
||||
float minRps = rpmMinMotorFrequency();
|
||||
DEBUG_SET(DEBUG_DYN_IDLE, 3, (minRps * 10));
|
||||
float rpsError = mixerRuntime.dynIdleMinRps - minRps;
|
||||
|
@ -233,11 +232,10 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
|
|||
mixerRuntime.dynIdleI += rpsError * mixerRuntime.dynIdleIGain;
|
||||
mixerRuntime.dynIdleI = constrainf(mixerRuntime.dynIdleI, 0.0f, maxIncrease);
|
||||
motorRangeMinIncrease = constrainf((dynIdleP + mixerRuntime.dynIdleI + dynIdleD), 0.0f, maxIncrease);
|
||||
|
||||
DEBUG_SET(DEBUG_DYN_IDLE, 0, (MAX(-1000.0f, dynIdleP * 10000)));
|
||||
DEBUG_SET(DEBUG_DYN_IDLE, 1, (mixerRuntime.dynIdleI * 10000));
|
||||
DEBUG_SET(DEBUG_DYN_IDLE, 2, (dynIdleD * 10000));
|
||||
} else {
|
||||
} else {
|
||||
motorRangeMinIncrease = 0;
|
||||
}
|
||||
#endif
|
||||
|
@ -537,9 +535,9 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
|
|||
mixerThrottle = throttle;
|
||||
|
||||
#ifdef USE_DYN_IDLE
|
||||
// Apply digital idle throttle offset when stick is at zero after all other adjustments are complete
|
||||
// Set min throttle offset of 1% when stick is at zero and dynamic idle is active
|
||||
if (mixerRuntime.dynIdleMinRps > 0.0f) {
|
||||
throttle = MAX(throttle, mixerRuntime.idleThrottleOffset);
|
||||
throttle = MAX(throttle, 0.01f);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
@ -288,11 +288,7 @@ void initEscEndpoints(void)
|
|||
if (currentPidProfile->motor_output_limit < 100) {
|
||||
motorOutputLimit = currentPidProfile->motor_output_limit / 100.0f;
|
||||
}
|
||||
|
||||
motorInitEndpoints(motorConfig(), motorOutputLimit, &mixerRuntime.motorOutputLow, &mixerRuntime.motorOutputHigh, &mixerRuntime.disarmMotorOutput, &mixerRuntime.deadbandMotor3dHigh, &mixerRuntime.deadbandMotor3dLow);
|
||||
if (!mixerRuntime.feature3dEnabled && currentPidProfile->dyn_idle_min_rpm) {
|
||||
mixerRuntime.motorOutputLow = DSHOT_MIN_THROTTLE;
|
||||
}
|
||||
}
|
||||
|
||||
// Initialize pidProfile related mixer settings
|
||||
|
@ -300,12 +296,23 @@ void initEscEndpoints(void)
|
|||
void mixerInitProfile(void)
|
||||
{
|
||||
#ifdef USE_DYN_IDLE
|
||||
mixerRuntime.dynIdleMinRps = currentPidProfile->dyn_idle_min_rpm * 100.0f / 60.0f;
|
||||
if (motorConfigMutable()->dev.useDshotTelemetry) {
|
||||
mixerRuntime.dynIdleMinRps = currentPidProfile->dyn_idle_min_rpm * 100.0f / 60.0f;
|
||||
} else {
|
||||
mixerRuntime.dynIdleMinRps = 0.0f;
|
||||
}
|
||||
mixerRuntime.dynIdlePGain = currentPidProfile->dyn_idle_p_gain * 0.00015f;
|
||||
mixerRuntime.dynIdleIGain = currentPidProfile->dyn_idle_i_gain * 0.01f * pidGetDT();
|
||||
mixerRuntime.dynIdleDGain = currentPidProfile->dyn_idle_d_gain * 0.0000003f * pidGetPidFrequency();
|
||||
mixerRuntime.dynIdleMaxIncrease = currentPidProfile->dyn_idle_max_increase * 0.001f;
|
||||
mixerRuntime.minRpsDelayK = 800 * pidGetDT() / 20.0f; //approx 20ms D delay, arbitrarily suits many motors
|
||||
if (!mixerRuntime.feature3dEnabled ) {
|
||||
if (mixerRuntime.dynIdleMinRps) {
|
||||
mixerRuntime.motorOutputLow = DSHOT_MIN_THROTTLE;
|
||||
} else {
|
||||
mixerRuntime.motorOutputLow = DSHOT_MIN_THROTTLE + mixerRuntime.idleThrottleOffset * DSHOT_RANGE;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
|
||||
|
|
|
@ -166,10 +166,6 @@ float rpmFilterGyro(const int axis, float value)
|
|||
|
||||
FAST_CODE_NOINLINE void rpmFilterUpdate(void)
|
||||
{
|
||||
if (gyroFilter == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (int motor = 0; motor < getMotorCount(); motor++) {
|
||||
filteredMotorErpm[motor] = pt1FilterApply(&rpmFilters[motor], getDshotTelemetry(motor));
|
||||
if (motor < 4) {
|
||||
|
@ -178,6 +174,11 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void)
|
|||
motorFrequency[motor] = erpmToHz * filteredMotorErpm[motor];
|
||||
}
|
||||
|
||||
if (gyroFilter == NULL) {
|
||||
minMotorFrequency = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
for (int i = 0; i < filterUpdatesPerIteration; i++) {
|
||||
|
||||
float frequency = constrainf(
|
||||
|
@ -230,7 +231,7 @@ bool isRpmFilterEnabled(void)
|
|||
float rpmMinMotorFrequency(void)
|
||||
{
|
||||
if (minMotorFrequency == 0.0f) {
|
||||
minMotorFrequency = 10000.0f;
|
||||
minMotorFrequency = 10000.0f; // max RPM reported in Hz = 600,000RPM
|
||||
for (int i = getMotorCount(); i--;) {
|
||||
if (motorFrequency[i] < minMotorFrequency) {
|
||||
minMotorFrequency = motorFrequency[i];
|
||||
|
@ -240,5 +241,4 @@ float rpmMinMotorFrequency(void)
|
|||
return minMotorFrequency;
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
|
|
|
@ -2846,6 +2846,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
#endif
|
||||
}
|
||||
pidInitConfig(currentPidProfile);
|
||||
mixerInitProfile();
|
||||
|
||||
break;
|
||||
case MSP_SET_SENSOR_CONFIG:
|
||||
|
|
Loading…
Reference in New Issue