Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +03:00
Code Issues Wiki Activity

Merge pull request #9544 from etracer65/simplify_pid_motor_pwm_validation

Browse source 
Simplify and improve motor protocol PID loop limiting
This commit is contained in:
Michael Keller 2020-03-07 15:56:22 +13:00 • committed by GitHub
commit a67df68f2c
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 41 additions and 46 deletions
Download patch file Download diff file Expand all files Collapse all files

87
src/main/config/config.c
View file

@ -582,56 +582,51 @@ void validateAndFixGyroConfig(void)
} }
#endif #endif
float samplingTime; if (gyro.sampleRateHz > 0) {
switch (gyroMpuDetectionResult()->sensor) { float samplingTime = 1.0f / gyro.sampleRateHz;
case ICM_20649_SPI:
samplingTime = 1.0f / 9000.0f;
break;
case BMI_160_SPI:
samplingTime = 0.0003125f;
break;
default:
samplingTime = 0.000125f;
break;
}
// check for looptime restrictions based on motor protocol. Motor times have safety margin
// check for looptime restrictions based on motor protocol. Motor times have safety margin float motorUpdateRestriction;
float motorUpdateRestriction; switch (motorConfig()->dev.motorPwmProtocol) {
switch (motorConfig()->dev.motorPwmProtocol) { case PWM_TYPE_STANDARD:
case PWM_TYPE_STANDARD: motorUpdateRestriction = 1.0f / BRUSHLESS_MOTORS_PWM_RATE;
motorUpdateRestriction = 1.0f / BRUSHLESS_MOTORS_PWM_RATE; break;
break; case PWM_TYPE_ONESHOT125:
case PWM_TYPE_ONESHOT125: motorUpdateRestriction = 0.0005f;
motorUpdateRestriction = 0.0005f; break;
break; case PWM_TYPE_ONESHOT42:
case PWM_TYPE_ONESHOT42: motorUpdateRestriction = 0.0001f;
motorUpdateRestriction = 0.0001f; break;
break;
#ifdef USE_DSHOT #ifdef USE_DSHOT
case PWM_TYPE_DSHOT150: case PWM_TYPE_DSHOT150:
motorUpdateRestriction = 0.000250f; motorUpdateRestriction = 0.000250f;
break; break;
case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT300:
motorUpdateRestriction = 0.0001f; motorUpdateRestriction = 0.0001f;
break; break;
#endif #endif
default: default:
motorUpdateRestriction = 0.00003125f; motorUpdateRestriction = 0.00003125f;
break; break;
}
if (motorConfig()->dev.useUnsyncedPwm) {
// Prevent overriding the max rate of motors
if ((motorConfig()->dev.motorPwmProtocol <= PWM_TYPE_BRUSHED) && (motorConfig()->dev.motorPwmProtocol != PWM_TYPE_STANDARD)) {
const uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction);
motorConfigMutable()->dev.motorPwmRate = MIN(motorConfig()->dev.motorPwmRate, maxEscRate);
} }
} else {
const float pidLooptime = samplingTime * pidConfig()->pid_process_denom; if (motorConfig()->dev.useUnsyncedPwm) {
if (pidLooptime < motorUpdateRestriction) { // Prevent overriding the max rate of motors
const uint8_t minPidProcessDenom = constrain(motorUpdateRestriction / samplingTime, 1, MAX_PID_PROCESS_DENOM); if ((motorConfig()->dev.motorPwmProtocol <= PWM_TYPE_BRUSHED) && (motorConfig()->dev.motorPwmProtocol != PWM_TYPE_STANDARD)) {
pidConfigMutable()->pid_process_denom = MAX(pidConfigMutable()->pid_process_denom, minPidProcessDenom); const uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction);
motorConfigMutable()->dev.motorPwmRate = MIN(motorConfig()->dev.motorPwmRate, maxEscRate);
}
} else {
const float pidLooptime = samplingTime * pidConfig()->pid_process_denom;
if (pidLooptime < motorUpdateRestriction) {
uint8_t minPidProcessDenom = motorUpdateRestriction / samplingTime;
if (motorUpdateRestriction / samplingTime > minPidProcessDenom) {
// if any fractional part then round up
minPidProcessDenom++;
}
minPidProcessDenom = constrain(minPidProcessDenom, 1, MAX_PID_PROCESS_DENOM);
pidConfigMutable()->pid_process_denom = MAX(pidConfigMutable()->pid_process_denom, minPidProcessDenom);
}
} }
} }