From a9a190ff3e372ee1a40b40e2d46364fab5216de5 Mon Sep 17 00:00:00 2001 From: "Ben V. Brown" Date: Sun, 15 Oct 2023 22:00:29 +1100 Subject: [PATCH] Cleaning up --- source/Core/BSP/Pinecilv2/configuration.h | 6 +++--- source/Core/Threads/PIDThread.cpp | 12 ++++++++---- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/source/Core/BSP/Pinecilv2/configuration.h b/source/Core/BSP/Pinecilv2/configuration.h index b6f447a3d1..d6587a24c7 100644 --- a/source/Core/BSP/Pinecilv2/configuration.h +++ b/source/Core/BSP/Pinecilv2/configuration.h @@ -165,9 +165,9 @@ #define NEEDS_VBUS_PROBE 0 // No vbus probe, its not connected in pcb #define CANT_DIRECT_READ_SETTINGS // We cant memcpy settings due to flash cache #define TIP_CONTROL_PID // We use PID rather than integrator -#define TIP_PID_KP 3000 -#define TIP_PID_KI 5 -#define TIP_PID_KD 300 +#define TIP_PID_KP 35 +#define TIP_PID_KI 8 +#define TIP_PID_KD 200 #endif /* Pinecilv2 */ diff --git a/source/Core/Threads/PIDThread.cpp b/source/Core/Threads/PIDThread.cpp index a5e2f16e7b..615a966df6 100644 --- a/source/Core/Threads/PIDThread.cpp +++ b/source/Core/Threads/PIDThread.cpp @@ -98,19 +98,23 @@ template struct PID { const T target_delta = set_point - new_reading; // Proportional term - const T kp_result = (Kp * target_delta) / 100; + const T kp_result = Kp * target_delta; - // Integral term - integration_running_sum += ((target_delta * interval_ms) / 3000); + // Integral term as we use mixed sampling rates, we cant assume a constant sample interval + // Thus we multiply this out by the interval time to ~= dv/dt + // Then the shift by 1000 is ms -> Seconds + + integration_running_sum += (target_delta * interval_ms * Ki) / 1000; // We constrain integration_running_sum to limit windup + // This is not overly required for most use cases but can prevent large overshoot in constrained implementations if (integration_running_sum > integral_limit_scale * max_output) { integration_running_sum = integral_limit_scale * max_output; } else if (integration_running_sum < -integral_limit_scale * max_output) { integration_running_sum = -integral_limit_scale * max_output; } // Calculate the integral term, we use a shift 100 to get precision in integral as we often need small amounts - T ki_result = (Ki * integration_running_sum) / 100; + T ki_result = integration_running_sum / 100; // Derivative term T derivative = (target_delta - previous_error_term);