From d9ce797caefc848cf25957a0ab0bbdea78a39ab8 Mon Sep 17 00:00:00 2001 From: philsson Date: Sun, 9 Sep 2018 22:38:58 +0200 Subject: [PATCH] dT as zero crash prevention Should not happen, but just in case. --- src/control/PID.cpp | 4 ++-- src/control/lpf.cpp | 7 ++++++- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/control/PID.cpp b/src/control/PID.cpp index 78d1fc6..7f23708 100644 --- a/src/control/PID.cpp +++ b/src/control/PID.cpp @@ -26,7 +26,7 @@ controllerPD::controllerPD(float kP, float kD, float saturation) , m_kD(kD) , m_saturation(saturation) , m_lastError(0) -, m_pt1FilterApply4(10.0f) +, m_pt1FilterApply4(100.0f) { } @@ -36,7 +36,7 @@ float controllerPD::run(float dT, float input, float setPoint) float pTerm(m_kP*error); - float dTerm(m_kD*((error-m_lastError)/dT)); + float dTerm((dT != 0.0f) ? m_kD*((error-m_lastError)/dT) : 0.0f); // Store error for next iteration m_lastError = error; diff --git a/src/control/lpf.cpp b/src/control/lpf.cpp index 3cf836b..2b1362a 100644 --- a/src/control/lpf.cpp +++ b/src/control/lpf.cpp @@ -25,7 +25,12 @@ pt1FilterApply4::pt1FilterApply4(float freqCut) float pt1FilterApply4::filter(float input, float dT) { - m_filtered = m_filtered + dT / (m_RC + dT) * (input - m_filtered); + if (dT != 0.0f && m_RC + dT != 0.0f) + { + float gain(dT / (m_RC + dT)); + + m_filtered += gain * (input - m_filtered); + } return m_filtered; }