From 0d8d085cfec7b8b3528fb7859816cfa906e12d3b Mon Sep 17 00:00:00 2001
From: borisbstyle <dreambb1982@gmail.com>
Date: Tue, 3 Jan 2017 16:28:58 +0100
Subject: [PATCH] Guard against too fast looptimes unsupported by motor pwm
 type

---
 src/main/fc/config.c     | 38 ++++++++++++++++++++++++++++++++++++++
 src/main/io/serial_cli.c |  2 +-
 2 files changed, 39 insertions(+), 1 deletion(-)

diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index 992e78d4e..290d0ed9e 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -18,6 +18,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <string.h>
+#include <math.h>
 
 #include "platform.h"
 
@@ -1040,9 +1041,46 @@ void validateAndFixGyroConfig(void)
         gyroConfig()->gyro_soft_notch_hz_2 = 0;
     }
 
+    float samplingTime = 0.000125f;
+
     if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
         pidConfig()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
         gyroConfig()->gyro_sync_denom = 1;
+        samplingTime = 0.001f;
+    }
+
+    // check for looptime restrictions based on motor protocol. Motor times have safety margin
+    const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom;
+    float motorUpdateRestriction;
+    switch(motorConfig()->motorPwmProtocol) {
+        case (PWM_TYPE_STANDARD):
+            motorUpdateRestriction = 0.002f;
+            break;
+        case (PWM_TYPE_ONESHOT125):
+            motorUpdateRestriction = 0.0005f;
+            break;
+        case (PWM_TYPE_ONESHOT42):
+            motorUpdateRestriction = 0.0001f;
+            break;
+        case (PWM_TYPE_DSHOT150):
+            motorUpdateRestriction = 0.000125f;
+            break;
+        case (PWM_TYPE_DSHOT300):
+            motorUpdateRestriction = 0.0000625f;
+            break;
+        default:
+            motorUpdateRestriction = 0.00003125f;
+    }
+
+    if(pidLooptime < motorUpdateRestriction)
+        pidConfig()->pid_process_denom = motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom);
+
+    // Prevent overriding the max rate of motors
+    if(motorConfig()->useUnsyncedPwm) {
+        uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction);
+
+        if(motorConfig()->motorPwmRate > maxEscRate)
+            motorConfig()->motorPwmRate = maxEscRate;
     }
 }
 
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 2cdf39f99..1f327389d 100755
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -691,7 +691,7 @@ const clivalue_t valueTable[] = {
     { "accum_threshold",            VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } },
     { "yaw_accum_threshold",        VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } },
     { "yaw_lowpass",                VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },
-    { "pid_process_denom",          VAR_UINT8  | MASTER_VALUE,  &pidConfig()->pid_process_denom, .config.minmax = { 1,  8 } },
+    { "pid_process_denom",          VAR_UINT8  | MASTER_VALUE,  &pidConfig()->pid_process_denom, .config.minmax = { 1,  16 } },
 
     { "p_pitch",                    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], .config.minmax = { 0,  200 } },
     { "i_pitch",                    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], .config.minmax = { 0,  200 } },