From 5e089075903689346248f64c24052a663bb8aa8e Mon Sep 17 00:00:00 2001 From: Matthew Kennedy Date: Tue, 6 Jul 2021 18:44:59 -0700 Subject: [PATCH] knock sensing peak detect (#2910) * knock peak detect * comments * comment * initialize * test * fix warnings while we're here * those are functions, not values * ram --- .../controllers/actuators/boost_control.h | 2 +- .../actuators/gppwm/gppwm_channel.h | 2 +- firmware/controllers/engine_controller.cpp | 2 +- .../controllers/sensors/software_knock.cpp | 31 ++++++++++++++----- firmware/util/peak_detect.h | 25 +++++++++++++++ unit_tests/tests/test_util.cpp | 22 +++++++++++++ 6 files changed, 74 insertions(+), 10 deletions(-) create mode 100644 firmware/util/peak_detect.h diff --git a/firmware/controllers/actuators/boost_control.h b/firmware/controllers/actuators/boost_control.h index 72281d2411..add39d2577 100644 --- a/firmware/controllers/actuators/boost_control.h +++ b/firmware/controllers/actuators/boost_control.h @@ -11,7 +11,7 @@ #include "closed_loop_controller.h" #include "pid.h" -class IPwm; +struct IPwm; class BoostController : public ClosedLoopController { public: diff --git a/firmware/controllers/actuators/gppwm/gppwm_channel.h b/firmware/controllers/actuators/gppwm/gppwm_channel.h index db34a689b2..6510e26051 100644 --- a/firmware/controllers/actuators/gppwm/gppwm_channel.h +++ b/firmware/controllers/actuators/gppwm/gppwm_channel.h @@ -6,7 +6,7 @@ struct gppwm_channel; class OutputPin; -class IPwm; +struct IPwm; class ValueProvider3D; class GppwmChannel { diff --git a/firmware/controllers/engine_controller.cpp b/firmware/controllers/engine_controller.cpp index 69f3e04e01..77e6605ce0 100644 --- a/firmware/controllers/engine_controller.cpp +++ b/firmware/controllers/engine_controller.cpp @@ -698,7 +698,7 @@ void initEngineContoller(DECLARE_ENGINE_PARAMETER_SUFFIX) { * UNUSED_SIZE constants. */ #ifndef RAM_UNUSED_SIZE -#define RAM_UNUSED_SIZE 1500 +#define RAM_UNUSED_SIZE 1400 #endif #ifndef CCM_UNUSED_SIZE #define CCM_UNUSED_SIZE 300 diff --git a/firmware/controllers/sensors/software_knock.cpp b/firmware/controllers/sensors/software_knock.cpp index 54a1ab5e06..a604451821 100644 --- a/firmware/controllers/sensors/software_knock.cpp +++ b/firmware/controllers/sensors/software_knock.cpp @@ -1,4 +1,3 @@ - #include "global.h" #include "engine.h" #include "biquad.h" @@ -7,6 +6,7 @@ #include "knock_logic.h" #include "software_knock.h" #include "thread_priority.h" +#include "peak_detect.h" #if EFI_SOFTWARE_KNOCK @@ -15,9 +15,10 @@ EXTERN_ENGINE; #include "knock_config.h" #include "ch.hpp" -NO_CACHE adcsample_t sampleBuffer[2000]; -int8_t currentCylinderIndex = 0; -Biquad knockFilter; +static NO_CACHE adcsample_t sampleBuffer[2000]; +static int8_t currentCylinderIndex = 0; +static efitick_t lastKnockSampleTime = 0; +static Biquad knockFilter; static volatile bool knockIsSampling = false; static volatile bool knockNeedsProcess = false; @@ -141,6 +142,7 @@ void startKnockSampling(uint8_t cylinderIndex) { currentCylinderIndex = cylinderIndex; adcStartConversionI(&KNOCK_ADC, conversionGroup, sampleBuffer, sampleCount); + lastKnockSampleTime = getTimeNowNt(); } class KnockThread : public ThreadController<256> { @@ -164,6 +166,10 @@ void initSoftwareKnock() { } } +using PD = PeakDetect; +static PD peakDetectors[12]; +static PD allCylinderPeakDetector; + void processLastKnockEvent() { if (!knockNeedsProcess) { return; @@ -190,16 +196,27 @@ void processLastKnockEvent() { sumSq += filtered * filtered; } + // take a local copy + auto lastKnockTime = lastKnockSampleTime; + + // We're done with inspecting the buffer, another sample can be taken + knockNeedsProcess = false; + // mean of squares (not yet root) float meanSquares = sumSq / localCount; // RMS float db = 10 * log10(meanSquares); - tsOutputChannels.knockLevels[currentCylinderIndex] = roundf(clampF(-100, db, 100)); - tsOutputChannels.knockLevel = db; + // clamp to reasonable range + db = clampF(-100, db, 100); + + // Pass through peak detector + float cylPeak = peakDetectors[currentCylinderIndex].detect(db, lastKnockTime); + + tsOutputChannels.knockLevels[currentCylinderIndex] = roundf(cylPeak); + tsOutputChannels.knockLevel = allCylinderPeakDetector.detect(db, lastKnockTime); - knockNeedsProcess = false; } void KnockThread::ThreadTask() { diff --git a/firmware/util/peak_detect.h b/firmware/util/peak_detect.h new file mode 100644 index 0000000000..f5496dd774 --- /dev/null +++ b/firmware/util/peak_detect.h @@ -0,0 +1,25 @@ +#pragma once + +#include "rusefi_types.h" + +/** + * Stores the recent peak value, preventing loss of intermittent peaks in a signal. + */ +template +class PeakDetect { +public: + TValue detect(TValue currentValue, efitick_t nowNt) { + if ((nowNt > m_lastPeakTime + TTimeoutPeriod) || // if timed out + (currentValue > m_peak)) { // or current is higher than the previous peak + // store new peak and time + m_peak = currentValue; + m_lastPeakTime = nowNt; + } + + return m_peak; + } + +private: + TValue m_peak = std::numeric_limits::min(); + efitick_t m_lastPeakTime = std::numeric_limits::min(); +}; diff --git a/unit_tests/tests/test_util.cpp b/unit_tests/tests/test_util.cpp index 6203eab8fb..54cccfbd1f 100644 --- a/unit_tests/tests/test_util.cpp +++ b/unit_tests/tests/test_util.cpp @@ -24,6 +24,7 @@ #include "io_pins.h" #include "efi_gpio.h" #include "efilib.h" +#include "peak_detect.h" #include "gtest/gtest.h" @@ -490,3 +491,24 @@ TEST(util, datalogging) { // printf("Got [%s]\r\n", LOGGING_BUFFER); // ASSERT_STREQ("rusEfiVersion,776655@321ID DEFAULT_FRANKENSO 239,", LOGGING_BUFFER); } + +TEST(util, PeakDetect) { + constexpr int startTime = 50; + constexpr int timeout = 100; + PeakDetect dut; + + // Set a peak + EXPECT_EQ(dut.detect(1000, startTime), 1000); + + // Smaller value at the same time is ignored + EXPECT_EQ(dut.detect(500, startTime), 1000); + + // Larger value at the same time raises the peak + EXPECT_EQ(dut.detect(1500, startTime), 1500); + + // Small value at almost the timeout is ignored + EXPECT_EQ(dut.detect(500, startTime + timeout - 1), 1500); + + // Small value past the timeout is used + EXPECT_EQ(dut.detect(500, startTime + timeout + 1), 500); +}