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

@@ -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)

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)))

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

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;

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) {

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

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<size_t>(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

firmware/controllers/sensors/software_knock.h

@@ -0,0 +1,7 @@
+#pragma once
+
+#include <cstdint>
+
+void initSoftwareKnock();
+void startKnockSampling(uint8_t cylinderIndex);
+void processLastKnockEvent();

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

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

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

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

firmware/util/math/biquad.cpp

@@ -11,9 +11,14 @@
 #include <math.h>
 
 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;
+}

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;
 };

java_console/models/src/main/java/com/rusefi/tracing/EnumNames.java

@@ -50,5 +50,6 @@ public class EnumNames {
 	"Hip9011IntHoldCallback",
 	"GlobalLock",
 	"GlobalUnlock",
+	"SoftwareKnockProcess",
 	};
 }