diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index c8364e2d1..18eed33b3 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -166,11 +166,8 @@ void setGyroSamplingSpeed(uint16_t looptime) {
 
         if (looptime < 1000) {
             masterConfig.force_motor_pwm_rate = 1;
-            if (masterConfig.use_multiShot || masterConfig.use_oneshot42) {
-                masterConfig.motor_pwm_rate = 4200;
-            } else {
-                masterConfig.motor_pwm_rate = 2700;
-            }
+            masterConfig.motor_pwm_rate = lrintf(1.0f / (gyroSampleRate * masterConfig.gyro_sync_denom * masterConfig.pid_process_denom * 0.000001f)) + 100;
+            if (!(masterConfig.use_multiShot || masterConfig.use_oneshot42)) masterConfig.motor_pwm_rate = constrain(masterConfig.motor_pwm_rate, 1000, 3800);
         } else {
             masterConfig.force_motor_pwm_rate = 0;
         }