Fix gov.P and gov.D mixup

rotorflight · Aug 13, 2024 · 82bdf75 · 82bdf75
1 parent 5011519
commit 82bdf75
Showing 1 changed file with 5 additions and 5 deletions.
diff --git a/src/main/flight/governor.c b/src/main/flight/governor.c
@@ -838,7 +838,7 @@ static float govPIDControl(void)
     // PID limits
     gov.P = constrainf(gov.P, -GOV_P_TERM_LIMIT, GOV_P_TERM_LIMIT);
     gov.I = constrainf(gov.I,                 0, GOV_I_TERM_LIMIT);
-    gov.D = constrainf(gov.P, -GOV_D_TERM_LIMIT, GOV_D_TERM_LIMIT);
+    gov.D = constrainf(gov.D, -GOV_D_TERM_LIMIT, GOV_D_TERM_LIMIT);
 
     // Governor PID sum
     gov.pidSum = gov.P + gov.I + gov.D + gov.C;
@@ -865,7 +865,7 @@ static void govMode1Init(void)
 {
     // PID limits
     gov.P = constrainf(gov.P, -GOV_P_TERM_LIMIT, GOV_P_TERM_LIMIT);
-    gov.D = constrainf(gov.P, -GOV_D_TERM_LIMIT, GOV_D_TERM_LIMIT);
+    gov.D = constrainf(gov.D, -GOV_D_TERM_LIMIT, GOV_D_TERM_LIMIT);
     gov.F = constrainf(gov.F,                 0, GOV_F_TERM_LIMIT);
 
     // Use gov.I to reach the target
@@ -882,7 +882,7 @@ static float govMode1Control(void)
     // PID limits
     gov.P = constrainf(gov.P, -GOV_P_TERM_LIMIT, GOV_P_TERM_LIMIT);
     gov.I = constrainf(gov.I,                 0, GOV_I_TERM_LIMIT);
-    gov.D = constrainf(gov.P, -GOV_D_TERM_LIMIT, GOV_D_TERM_LIMIT);
+    gov.D = constrainf(gov.D, -GOV_D_TERM_LIMIT, GOV_D_TERM_LIMIT);
     gov.F = constrainf(gov.F,                 0, GOV_F_TERM_LIMIT);
 
     // Governor PIDF sum
@@ -916,7 +916,7 @@ static void govMode2Init(void)
 
     // PID limits
     gov.P = constrainf(gov.P, -GOV_P_TERM_LIMIT, GOV_P_TERM_LIMIT);
-    gov.D = constrainf(gov.P, -GOV_D_TERM_LIMIT, GOV_D_TERM_LIMIT);
+    gov.D = constrainf(gov.D, -GOV_D_TERM_LIMIT, GOV_D_TERM_LIMIT);
     gov.F = constrainf(gov.F,                 0, GOV_F_TERM_LIMIT);
 
     // Use gov.I to reach the target
@@ -936,7 +936,7 @@ static float govMode2Control(void)
     // PID limits
     gov.P = constrainf(gov.P, -GOV_P_TERM_LIMIT, GOV_P_TERM_LIMIT);
     gov.I = constrainf(gov.I,                 0, GOV_I_TERM_LIMIT);
-    gov.D = constrainf(gov.P, -GOV_D_TERM_LIMIT, GOV_D_TERM_LIMIT);
+    gov.D = constrainf(gov.D, -GOV_D_TERM_LIMIT, GOV_D_TERM_LIMIT);
     gov.F = constrainf(gov.F,                 0, GOV_F_TERM_LIMIT);
 
     // Governor PIDF sum