From 1a55085bb6f9da32eaac6eaec80921fc15d639ff Mon Sep 17 00:00:00 2001 From: Matthew Kennedy Date: Fri, 28 Aug 2020 18:13:50 -0700 Subject: [PATCH] Software knock detection (#1730) * s * science * set pin mode * turn stuff off so it fits * filtering maybe * filtering actually works * generate filter parameters internally * shorter window * guard behind enable flag * use checked in filter * add biquad reset * tracing * const * exec order * do it from a thread * smaller buffer, comment * configure with header * only for proteus * oops * unused * not needed * guards * pin config * don't need that include * precook filter steady state * define sample rate * config enable switch --- firmware/config/boards/proteus/board.mk | 6 +- firmware/config/boards/proteus/knock_config.h | 16 ++ firmware/config/boards/proteus/prepend.txt | 1 + firmware/console/status_loop.cpp | 4 +- .../controllers/engine_cycle/spark_logic.cpp | 5 + firmware/controllers/sensors/sensors.mk | 1 + .../controllers/sensors/software_knock.cpp | 146 ++++++++++++++++++ firmware/controllers/sensors/software_knock.h | 7 + firmware/development/perf_trace.h | 1 + firmware/hw_layer/hardware.cpp | 5 + firmware/integration/rusefi_config.txt | 3 +- firmware/tunerstudio/rusefi.input | 5 + firmware/util/math/biquad.cpp | 17 +- firmware/util/math/biquad.h | 8 +- .../java/com/rusefi/tracing/EnumNames.java | 1 + 15 files changed, 216 insertions(+), 10 deletions(-) create mode 100644 firmware/config/boards/proteus/knock_config.h create mode 100644 firmware/controllers/sensors/software_knock.cpp create mode 100644 firmware/controllers/sensors/software_knock.h diff --git a/firmware/config/boards/proteus/board.mk b/firmware/config/boards/proteus/board.mk index 5da66e3e98..4510b15426 100644 --- a/firmware/config/boards/proteus/board.mk +++ b/firmware/config/boards/proteus/board.mk @@ -2,19 +2,19 @@ BOARDSRC = $(CHIBIOS)/os/hal/boards/ST_NUCLEO144_F767ZI/board.c BOARDSRC_CPP = $(PROJECT_DIR)/config/boards/proteus/board_configuration.cpp \ $(PROJECT_DIR)/config/boards/proteus/adc_hack.cpp +BOARDINC = $(PROJECT_DIR)/config/boards/proteus # Target processor details ifeq ($(PROJECT_CPU),ARCH_STM32F4) MCU_DEFS = -DSTM32F407xx BOARDSRC = $(CHIBIOS)/os/hal/boards/ST_STM32F4_DISCOVERY/board.c - BOARDINC = $(BOARDS_DIR)/microrusefi BOARDINC += $(PROJECT_DIR)/config/stm32f4ems # For board.h BOARDINC += $(BOARDS_DIR)/st_stm32f4 LDSCRIPT= $(BOARDS_DIR)/prometheus/STM32F405xG.ld else MCU_DEFS = -DSTM32F767xx BOARDSRC = $(CHIBIOS)/os/hal/boards/ST_NUCLEO144_F767ZI/board.c - BOARDINC = $(BOARDS_DIR)/nucleo_f767 # For board.h + BOARDINC += $(BOARDS_DIR)/nucleo_f767 # For board.h BOARDINC += $(PROJECT_DIR)/config/stm32f7ems # efifeatures/halconf/chconf.h LDSCRIPT= $(BOARDS_DIR)/nucleo_f767/STM32F76xxI.ld CONFDIR=config/stm32f4ems @@ -22,7 +22,7 @@ else endif # Override DEFAULT_ENGINE_TYPE -DDEFS += $(MCU_DEFS) -DEFI_USE_OSC=TRUE -DLED_CRITICAL_ERROR_BRAIN_PIN=GPIOE_3 -DFIRMWARE_ID=\"proteus\" -DDEFAULT_ENGINE_TYPE=PROTEUS -DSTM32_ADC_USE_ADC3=TRUE -DEFI_INCLUDE_ENGINE_PRESETS=FALSE -DEFI_ICU_INPUTS=FALSE -DHAL_TRIGGER_USE_PAL=TRUE -DEFI_VEHICLE_SPEED=FALSE -DEFI_LOGIC_ANALYZER=FALSE +DDEFS += $(MCU_DEFS) -DEFI_USE_OSC=TRUE -DLED_CRITICAL_ERROR_BRAIN_PIN=GPIOE_3 -DFIRMWARE_ID=\"proteus\" -DDEFAULT_ENGINE_TYPE=PROTEUS -DSTM32_ADC_USE_ADC3=TRUE -DEFI_INCLUDE_ENGINE_PRESETS=FALSE -DEFI_ICU_INPUTS=FALSE -DHAL_TRIGGER_USE_PAL=TRUE -DEFI_VEHICLE_SPEED=FALSE -DEFI_LOGIC_ANALYZER=FALSE -DEFI_SOFTWARE_KNOCK=TRUE # Proteus <=v0.2 needs ADC hack - vbatt is on ADC3 ifeq ($(PROTEUS_LEGACY),TRUE) diff --git a/firmware/config/boards/proteus/knock_config.h b/firmware/config/boards/proteus/knock_config.h new file mode 100644 index 0000000000..ac65da1777 --- /dev/null +++ b/firmware/config/boards/proteus/knock_config.h @@ -0,0 +1,16 @@ +#pragma once + +// Knock is on ADC3 +#define KNOCK_ADC ADCD3 + +// knock 1 - pin PF4 +#define KNOCK_ADC_CH1 ADC_CHANNEL_IN14 +#define KNOCK_PIN_CH1 GPIOF_4 + +// knock 2 - pin PF5 +#define KNOCK_ADC_CH2 ADC_CHANNEL_IN15 +#define KNOCK_PIN_CH2 GPIOF_5 + +// Sample rate & time - depends on the exact MCU +#define KNOCK_SAMPLE_TIME ADC_SAMPLE_84 +#define KNOCK_SAMPLE_RATE (STM32_PCLK2 / (4 * (84 + 12))) diff --git a/firmware/config/boards/proteus/prepend.txt b/firmware/config/boards/proteus/prepend.txt index 5a6f65b6f7..656a30d43f 100644 --- a/firmware/config/boards/proteus/prepend.txt +++ b/firmware/config/boards/proteus/prepend.txt @@ -11,6 +11,7 @@ #define ts_show_can_pins false #define ts_show_tunerstudio_port false #define ts_show_can2 false +#define ts_show_software_knock true #define show_Frankenso_presets false #define show_microRusEFI_presets false diff --git a/firmware/console/status_loop.cpp b/firmware/console/status_loop.cpp index 07b1c9fde7..d2ede40e8d 100644 --- a/firmware/console/status_loop.cpp +++ b/firmware/console/status_loop.cpp @@ -617,8 +617,8 @@ void updateTunerStudioState(TunerStudioOutputChannels *tsOutputChannels DECLARE_ tsOutputChannels->manifoldAirPressure = mapValue; } - tsOutputChannels->knockCount = engine->knockCount; - tsOutputChannels->knockLevel = engine->knockVolts; + //tsOutputChannels->knockCount = engine->knockCount; + //tsOutputChannels->knockLevel = engine->knockVolts; #if HW_CHECK_MODE tsOutputChannels->hasCriticalError = 1; diff --git a/firmware/controllers/engine_cycle/spark_logic.cpp b/firmware/controllers/engine_cycle/spark_logic.cpp index adbc811848..ba532450f8 100644 --- a/firmware/controllers/engine_cycle/spark_logic.cpp +++ b/firmware/controllers/engine_cycle/spark_logic.cpp @@ -5,6 +5,7 @@ * @author Andrey Belomutskiy, (c) 2012-2020 */ +#include "software_knock.h" #include "spark_logic.h" #include "os_access.h" #include "engine_math.h" @@ -203,6 +204,10 @@ if (engineConfiguration->debugMode == DBG_DWELL_METRIC) { // If all events have been scheduled, prepare for next time. prepareCylinderIgnitionSchedule(dwellAngleDuration, sparkDwell, event PASS_ENGINE_PARAMETER_SUFFIX); } + +#if EFI_SOFTWARE_KNOCK + startKnockSampling(event->cylinderIndex); +#endif } static void startDwellByTurningSparkPinHigh(IgnitionEvent *event, IgnitionOutputPin *output) { diff --git a/firmware/controllers/sensors/sensors.mk b/firmware/controllers/sensors/sensors.mk index 41bd04b616..c8256733c3 100644 --- a/firmware/controllers/sensors/sensors.mk +++ b/firmware/controllers/sensors/sensors.mk @@ -13,6 +13,7 @@ CONTROLLERS_SENSORS_SRC_CPP = $(PROJECT_DIR)/controllers/sensors/thermistors.cp $(PROJECT_DIR)/controllers/sensors/sensor_info_printing.cpp \ $(PROJECT_DIR)/controllers/sensors/functional_sensor.cpp \ $(PROJECT_DIR)/controllers/sensors/redundant_sensor.cpp \ + $(PROJECT_DIR)/controllers/sensors/software_knock.cpp \ $(PROJECT_DIR)/controllers/sensors/converters/linear_func.cpp \ $(PROJECT_DIR)/controllers/sensors/converters/resistance_func.cpp \ $(PROJECT_DIR)/controllers/sensors/converters/thermistor_func.cpp diff --git a/firmware/controllers/sensors/software_knock.cpp b/firmware/controllers/sensors/software_knock.cpp new file mode 100644 index 0000000000..bf179e7197 --- /dev/null +++ b/firmware/controllers/sensors/software_knock.cpp @@ -0,0 +1,146 @@ + +#include "global.h" +#include "engine.h" +#include "biquad.h" +#include "perf_trace.h" +#include "thread_controller.h" +#include "software_knock.h" + +#if EFI_SOFTWARE_KNOCK + +EXTERN_ENGINE; + +#include "knock_config.h" + +adcsample_t sampleBuffer[2000]; +Biquad knockFilter; + +static volatile bool knockIsSampling = false; +static volatile bool knockNeedsProcess = false; +static volatile size_t sampleCount = 0; + +binary_semaphore_t knockSem; + +static void completionCallback(ADCDriver* adcp, adcsample_t*, size_t) { + palClearPad(GPIOD, 2); + + if (adcp->state == ADC_COMPLETE) { + knockNeedsProcess = true; + + // Notify the processing thread that it's time to process this sample + chSysLockFromISR(); + chBSemSignalI(&knockSem); + chSysUnlockFromISR(); + } +} + +static void errorCallback(ADCDriver*, adcerror_t err) { +} + +static const ADCConversionGroup adcConvGroup = { FALSE, 1, &completionCallback, &errorCallback, + 0, + ADC_CR2_SWSTART, + ADC_SMPR1_SMP_AN14(KNOCK_SAMPLE_TIME), // sample times for channels 10...18 + 0, + + 0, // htr + 0, // ltr + + 0, // sqr1 + 0, // sqr2 + ADC_SQR3_SQ1_N(KNOCK_ADC_CH1) +}; + +void startKnockSampling(uint8_t cylinderIndex) { + if (!CONFIG(enableSoftwareKnock)) { + return; + } + + // Cancel if ADC isn't ready + if (!((KNOCK_ADC.state == ADC_READY) || + (KNOCK_ADC.state == ADC_COMPLETE) || + (KNOCK_ADC.state == ADC_ERROR))) { + return; + } + + // If there's pending processing, skip this event + if (knockNeedsProcess) { + return; + } + + // Sample for 45 degrees + float samplingSeconds = ENGINE(rpmCalculator).oneDegreeUs * 45 * 1e-6; + constexpr int sampleRate = KNOCK_SAMPLE_RATE; + sampleCount = 0xFFFFFFFE & static_cast(clampF(100, samplingSeconds * sampleRate, efi::size(sampleBuffer))); + + adcStartConversionI(&KNOCK_ADC, &adcConvGroup, sampleBuffer, sampleCount); +} + +class KnockThread : public ThreadController<256> { +public: + KnockThread() : ThreadController("knock", NORMALPRIO - 10) {} + void ThreadTask() override; +}; + +KnockThread kt; + +void initSoftwareKnock() { + chBSemObjectInit(&knockSem, TRUE); + + if (CONFIG(enableSoftwareKnock)) { + knockFilter.configureBandpass(KNOCK_SAMPLE_RATE, 1000 * CONFIG(knockBandCustom), 3); + adcStart(&KNOCK_ADC, nullptr); + + efiSetPadMode("knock ch1", KNOCK_PIN_CH1, PAL_MODE_INPUT_ANALOG); + efiSetPadMode("knock ch2", KNOCK_PIN_CH2, PAL_MODE_INPUT_ANALOG); + + kt.Start(); + } +} + +void processLastKnockEvent() { + if (!knockNeedsProcess) { + return; + } + + float sumSq = 0; + + constexpr float ratio = 3.3f / 4095.0f; + + size_t localCount = sampleCount; + + // Prepare the steady state at vcc/2 so that there isn't a step + // when samples begin + knockFilter.cookSteadyState(3.3f / 2); + + // Compute the sum of squares + for (size_t i = 0; i < localCount; i++) + { + float volts = ratio * sampleBuffer[i]; + + float filtered = knockFilter.filter(volts); + + sumSq += filtered * filtered; + } + + // mean of squares (not yet root) + float meanSquares = sumSq / localCount; + + // RMS + float db = 10 * log10(meanSquares); + + tsOutputChannels.knockLevel = db; + + knockNeedsProcess = false; +} + +void KnockThread::ThreadTask() { + while(1) { + chBSemWait(&knockSem); + + ScopePerf perf(PE::SoftwareKnockProcess); + processLastKnockEvent(); + } +} + +#endif // EFI_SOFTWARE_KNOCK diff --git a/firmware/controllers/sensors/software_knock.h b/firmware/controllers/sensors/software_knock.h new file mode 100644 index 0000000000..1fdd0e4fce --- /dev/null +++ b/firmware/controllers/sensors/software_knock.h @@ -0,0 +1,7 @@ +#pragma once + +#include + +void initSoftwareKnock(); +void startKnockSampling(uint8_t cylinderIndex); +void processLastKnockEvent(); diff --git a/firmware/development/perf_trace.h b/firmware/development/perf_trace.h index ccfb601bcb..1ae6a51359 100644 --- a/firmware/development/perf_trace.h +++ b/firmware/development/perf_trace.h @@ -62,6 +62,7 @@ enum class PE : uint8_t { Hip9011IntHoldCallback, GlobalLock, GlobalUnlock, + SoftwareKnockProcess, // enum_end_tag // The tag above is consumed by PerfTraceTool.java // please note that the tool requires a comma at the end of last value diff --git a/firmware/hw_layer/hardware.cpp b/firmware/hw_layer/hardware.cpp index 3b540782b3..4f7e6cd279 100644 --- a/firmware/hw_layer/hardware.cpp +++ b/firmware/hw_layer/hardware.cpp @@ -50,6 +50,7 @@ #include "aux_pid.h" #include "perf_trace.h" #include "boost_control.h" +#include "software_knock.h" #if EFI_MC33816 #include "mc33816.h" #endif /* EFI_MC33816 */ @@ -483,6 +484,10 @@ void initHardware(Logging *l) { waitForSlowAdc(1); #endif /* HAL_USE_ADC */ +#if EFI_SOFTWARE_KNOCK + initSoftwareKnock(); +#endif /* EFI_SOFTWARE_KNOCK */ + initRtc(); #if HAL_USE_SPI diff --git a/firmware/integration/rusefi_config.txt b/firmware/integration/rusefi_config.txt index 606d702894..de5d22e993 100644 --- a/firmware/integration/rusefi_config.txt +++ b/firmware/integration/rusefi_config.txt @@ -870,7 +870,7 @@ custom maf_sensor_type_e 4 bits, S32, @OFFSET@, [0:1], @@maf_sensor_type_e_enum@ bit showHumanReadableWarning bit stftIgnoreErrorMagnitude;+If enabled, adjust at a constant rate instead of a rate proportional to the current lambda error. This mode may be easier to tune, and more tolerant of sensor noise. Use of this mode is required if you have a narrowband O2 sensor.; bit dcMotorIdleValve;+Used on some German vehicles around late 90s: cable-operated throttle and DC motor idle air valve.\nSet the primary TPS to the cable-operated throttle's sensor\nSet the secondary TPS to the mini ETB's position sensor(s). - bit unusedBit_251_12 + bit enableSoftwareKnock bit unusedBit_251_13 bit unusedBit_251_14 bit unusedBit_251_15 @@ -1736,6 +1736,7 @@ end_struct #define ts_show_trigger_comparator false #define ts_show_auxserial_pins true #define ts_show_can2 true +#define ts_show_software_knock false #define show_test_presets true #define show_Frankenso_presets true diff --git a/firmware/tunerstudio/rusefi.input b/firmware/tunerstudio/rusefi.input index 6c5d4be05a..798322b98b 100644 --- a/firmware/tunerstudio/rusefi.input +++ b/firmware/tunerstudio/rusefi.input @@ -1370,6 +1370,7 @@ menuDialog = main subMenu = std_separator subMenu = hipFunction, "HIP9011 settings (knock sensor) (alpha version)" @@if_ts_show_hip9011 + subMenu = softwareKnock, "Software Knock" @@if_ts_show_software_knock subMenu = std_separator subMenu = etbDialog, "Electronic throttle body (beta version)" @@if_ts_show_etb @@ -2322,6 +2323,10 @@ cmd_set_engine_type_default = "@@TS_IO_TEST_COMMAND_char@@\x00\x31\x00\x00" field = "Pin mode", malfunctionIndicatorPinMode field = "Warning Period", warningPeriod + dialog = softwareKnock, "Software Knock" + field = "Enable", enableSoftwareKnock + field = "Band Freq override", knockBandCustom, {enableSoftwareKnock} + ; Engine->hip9011 Settings dialog = hipFunction, "HIP9011 Settings (knock decoder)" field = "Enabled", isHip9011Enabled diff --git a/firmware/util/math/biquad.cpp b/firmware/util/math/biquad.cpp index 8cfa342ff5..61091757ad 100644 --- a/firmware/util/math/biquad.cpp +++ b/firmware/util/math/biquad.cpp @@ -11,9 +11,14 @@ #include Biquad::Biquad() { - // Default to passthru +// Default to passthru a0 = 1; a1 = a2 = b1 = b2 = 0; + + reset(); +} + +void Biquad::reset() { z1 = z2 = 0; } @@ -58,3 +63,13 @@ float Biquad::filter(float input) { z2 = input * a2 - b2 * result; return result; } + +void Biquad::cookSteadyState(float steadyStateInput) { + float Y = steadyStateInput * (a0 + a1 + a2) / (1 + b1 + b2); + + float steady_z2 = steadyStateInput * a2 - Y * b2; + float steady_z1 = steady_z2 + steadyStateInput * a1 - Y * b1; + + this->z1 = steady_z1; + this->z2 = steady_z2; +} diff --git a/firmware/util/math/biquad.h b/firmware/util/math/biquad.h index 7ef51beba9..0f1c8c7dd5 100644 --- a/firmware/util/math/biquad.h +++ b/firmware/util/math/biquad.h @@ -11,7 +11,9 @@ class Biquad { public: Biquad(); - float filter(float input); + float filter(float input); + void reset(); + void cookSteadyState(float steadyStateInput); void configureBandpass(float samplingFrequency, float centerFrequency, float Q); @@ -19,6 +21,6 @@ public: void configureLowpass(float samplingFrequency, float cutoffFrequency, float Q = 0.707f); private: - float a0, a1, a2, b1, b2; - float z1, z2; + float a0, a1, a2, b1, b2; + float z1, z2; }; diff --git a/java_console/models/src/main/java/com/rusefi/tracing/EnumNames.java b/java_console/models/src/main/java/com/rusefi/tracing/EnumNames.java index 5654a66b7f..5129b02df8 100644 --- a/java_console/models/src/main/java/com/rusefi/tracing/EnumNames.java +++ b/java_console/models/src/main/java/com/rusefi/tracing/EnumNames.java @@ -50,5 +50,6 @@ public class EnumNames { "Hip9011IntHoldCallback", "GlobalLock", "GlobalUnlock", + "SoftwareKnockProcess", }; }