From c3532b4635cf6f4db5a7449c5333e873fbcd2843 Mon Sep 17 00:00:00 2001
From: Matthew Kennedy <matthewkennedy@outlook.com>
Date: Wed, 27 Apr 2022 19:29:51 -0700
Subject: [PATCH] fix vss/iss filter parameter validation (#4114)

* fix filter parameter validation

* use values that don't get clipped
---
 firmware/controllers/sensors/frequency_sensor.cpp      | 7 ++++---
 firmware/init/sensor/init_input_shaft_speed_sensor.cpp | 4 ++--
 firmware/init/sensor/init_vehicle_speed_sensor.cpp     | 4 ++--
 firmware/util/math/biquad.cpp                          | 4 ++--
 unit_tests/tests/sensor/test_frequency_sensor.cpp      | 2 +-
 5 files changed, 11 insertions(+), 10 deletions(-)

diff --git a/firmware/controllers/sensors/frequency_sensor.cpp b/firmware/controllers/sensors/frequency_sensor.cpp
index 7b6e177899..b570da7466 100644
--- a/firmware/controllers/sensors/frequency_sensor.cpp
+++ b/firmware/controllers/sensors/frequency_sensor.cpp
@@ -18,9 +18,10 @@ void FrequencySensor::initIfValid(brain_pin_e pin, SensorConverter &converter, f
 		return;
 	}
 
-	// Filter parameter less than 0.5 impossible, must always average over at least two events
-	if (filterParameter < 0.5f) {
-		filterParameter = 0.5f;
+	// Filter parameter greater than or equal to 0.5 impossible as it causes filter instability, clamp
+	// far under that value.
+	if (filterParameter > 0.35f) {
+		filterParameter = 0.35f;
 	}
 
 	m_filter.configureLowpass(1, filterParameter);
diff --git a/firmware/init/sensor/init_input_shaft_speed_sensor.cpp b/firmware/init/sensor/init_input_shaft_speed_sensor.cpp
index 4136d02367..55044157c6 100644
--- a/firmware/init/sensor/init_input_shaft_speed_sensor.cpp
+++ b/firmware/init/sensor/init_input_shaft_speed_sensor.cpp
@@ -10,8 +10,8 @@ static InputShaftSpeedConverter inputSpeedConverter;
 void initInputShaftSpeedSensor() {
 	int parameter = engineConfiguration->issFilterReciprocal;
 
-	if (parameter <= 0 || parameter > 200) {
-		parameter = 2;
+	if (parameter < 3 || parameter > 200) {
+		parameter = 3;
 	}
 
 	float filterParameter = 1.0f / parameter;
diff --git a/firmware/init/sensor/init_vehicle_speed_sensor.cpp b/firmware/init/sensor/init_vehicle_speed_sensor.cpp
index 7d0cfdbd51..04ea6601aa 100644
--- a/firmware/init/sensor/init_vehicle_speed_sensor.cpp
+++ b/firmware/init/sensor/init_vehicle_speed_sensor.cpp
@@ -11,8 +11,8 @@ static VehicleSpeedConverter vehicleSpeedConverter;
 void initVehicleSpeedSensor() {
 	int parameter = engineConfiguration->vssFilterReciprocal;
 
-	if (parameter <= 0 || parameter > 200) {
-		parameter = 2;
+	if (parameter < 3 || parameter > 200) {
+		parameter = 3;
 	}
 
 	float filterParameter = 1.0f / parameter;
diff --git a/firmware/util/math/biquad.cpp b/firmware/util/math/biquad.cpp
index 4dff895059..e2433218c5 100644
--- a/firmware/util/math/biquad.cpp
+++ b/firmware/util/math/biquad.cpp
@@ -30,7 +30,7 @@ static float getNorm(float K, float Q) {
 }
 
 void Biquad::configureBandpass(float samplingFrequency, float centerFrequency, float Q) {
-	efiAssertVoid(OBD_PCM_Processor_Fault, samplingFrequency >= 2 * centerFrequency, "Invalid biquad parameters");
+	efiAssertVoid(OBD_PCM_Processor_Fault, samplingFrequency >= 2.5f * centerFrequency, "Invalid biquad parameters");
 
 	float K = getK(samplingFrequency, centerFrequency);
 	float norm = getNorm(K, Q);
@@ -43,7 +43,7 @@ void Biquad::configureBandpass(float samplingFrequency, float centerFrequency, f
 }
 
 void Biquad::configureLowpass(float samplingFrequency, float cutoffFrequency, float Q) {
-	efiAssertVoid(OBD_PCM_Processor_Fault, samplingFrequency >= 2 * cutoffFrequency, "Invalid biquad parameters");
+	efiAssertVoid(OBD_PCM_Processor_Fault, samplingFrequency >= 2.5f * cutoffFrequency, "Invalid biquad parameters");
 
 	float K = getK(samplingFrequency, cutoffFrequency);
 	float norm = getNorm(K, Q);
diff --git a/unit_tests/tests/sensor/test_frequency_sensor.cpp b/unit_tests/tests/sensor/test_frequency_sensor.cpp
index 47405cf569..27889fa816 100644
--- a/unit_tests/tests/sensor/test_frequency_sensor.cpp
+++ b/unit_tests/tests/sensor/test_frequency_sensor.cpp
@@ -22,7 +22,7 @@ public:
 	void SetUp() override {
 		// If somehow prodcode will be unwrapped for test it MAYBE! will fire with error.
 		// At least we must init FlexSensor somehow
-		dut.initIfValid(GPIOA_0, identityFunc, 0.5f);
+		dut.initIfValid(GPIOA_0, identityFunc, 0.1f);
 	}
 
 	/*