diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a1b0f65e95..061287cce7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -297,12 +297,12 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control rc = rcCommand[axis] << 1; - if (lowThrottlePidReduction) rc /= 4; + if (lowThrottlePidReduction) rc = rc >> 2; - gyroError = gyroADC[axis] / 4; + gyroError = gyroADC[axis] >> 2; error = rc - gyroError; - errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp 16 bits is ok here + errorGyroI[axis] = constrain(errorGyroI[axis] + ((error * (uint16_t)targetLooptime) >> 12) , -16000, +16000); // WindUp 16 bits is ok here if (ABS(gyroADC[axis]) > (640 * 4)) { errorGyroI[axis] = 0; @@ -372,7 +372,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control axisPID[axis] = PTerm + ITerm + DTerm; - if (lowThrottlePidReduction) axisPID[axis] /= 4; + if (lowThrottlePidReduction) axisPID[axis] = axisPID[axis] >> 2; #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {