Analog input filtering (#1680)

* improve biquad

* cleanup

* add filtering to subscriptions

* config sensors

* comment

* doesn't need to be that fast

Co-authored-by: Matthew Kennedy <makenne@microsoft.com>

firmware/hw_layer/adc/adc_inputs.cpp

@@ -399,7 +399,7 @@ int getSlowAdcCounter() {
 class SlowAdcController : public PeriodicController<256> {
 public:
 	SlowAdcController() 
-		: PeriodicController("ADC", NORMALPRIO + 5, 500)
+		: PeriodicController("ADC", NORMALPRIO + 5, SLOW_ADC_RATE)
 	{
 	}
 

firmware/hw_layer/adc/adc_inputs.h

@@ -13,6 +13,8 @@
 
 #if HAL_USE_ADC
 
+#define SLOW_ADC_RATE 500
+
 const char * getAdcMode(adc_channel_e hwChannel);
 void initAdcInputs();
 

firmware/hw_layer/adc/adc_subscription.cpp

@@ -3,6 +3,7 @@
 #include "adc_inputs.h"
 #include "engine.h"
 #include "perf_trace.h"
+#include "biquad.h"
 
 #include <iterator>
 
@@ -12,6 +13,7 @@ EXTERN_ENGINE;
 
 void AdcSubscription::SubscribeSensor(FunctionalSensor &sensor,
 									  adc_channel_e channel,
+									  float lowpassCutoff,
 									  float voltsPerAdcVolt /*= 0.0f*/)
 {
 }
@@ -22,6 +24,7 @@ struct AdcSubscriptionEntry {
 	FunctionalSensor *Sensor;
 	float VoltsPerAdcVolt;
 	adc_channel_e Channel;
+	Biquad Filter;
 };
 
 static size_t s_nextEntry = 0;
@@ -29,6 +32,7 @@ static AdcSubscriptionEntry s_entries[8];
 
 void AdcSubscription::SubscribeSensor(FunctionalSensor &sensor,
 									  adc_channel_e channel,
+									  float lowpassCutoff,
 									  float voltsPerAdcVolt /*= 0.0f*/) {
 	// Don't subscribe null channels
 	if (channel == EFI_ADC_NONE) {
@@ -50,6 +54,7 @@ void AdcSubscription::SubscribeSensor(FunctionalSensor &sensor,
 	entry.Sensor = &sensor;
 	entry.VoltsPerAdcVolt = voltsPerAdcVolt;
 	entry.Channel = channel;
+	entry.Filter.configureLowpass(SLOW_ADC_RATE, lowpassCutoff);
 
 	s_nextEntry++;
 }
@@ -63,7 +68,9 @@ void AdcSubscription::UpdateSubscribers(efitick_t nowNt) {
 		float mcuVolts = getVoltage("sensor", entry.Channel);
 		float sensorVolts = mcuVolts * entry.VoltsPerAdcVolt;
 
-		entry.Sensor->postRawValue(sensorVolts, nowNt);
+		float filtered = entry.Filter.filter(sensorVolts);
+
+		entry.Sensor->postRawValue(filtered, nowNt);
 	}
 }
 

firmware/hw_layer/adc/adc_subscription.h

@@ -9,6 +9,6 @@
 
 class AdcSubscription {
 public:
-	static void SubscribeSensor(FunctionalSensor &sensor, adc_channel_e channel, float voltsPerAdcVolt = 0.0f);
+	static void SubscribeSensor(FunctionalSensor &sensor, adc_channel_e channel, float lowpassCutoff, float voltsPerAdcVolt = 0.0f);
 	static void UpdateSubscribers(efitick_t nowNt);
 };

firmware/init/sensor/init_oil_pressure.cpp

@@ -37,7 +37,7 @@ void initOilPressure(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	oilpSensor.setFunction(oilpSensorFunc);
 
 	// Subscribe the sensor to the ADC
-	AdcSubscription::SubscribeSensor(oilpSensor, channel);
+	AdcSubscription::SubscribeSensor(oilpSensor, channel, 10);
 
 	if (!oilpSensor.Register()) {
 		warning(OBD_Oil_Pressure_Sensor_Malfunction, "Duplicate oilp sensor registration, ignoring");

firmware/init/sensor/init_thermistors.cpp

@@ -60,7 +60,7 @@ static void configureTempSensor(FunctionalSensor &sensor,
 
 	configTherm(sensor, p, config, isLinear);
 
-	AdcSubscription::SubscribeSensor(sensor, channel);
+	AdcSubscription::SubscribeSensor(sensor, channel, 2);
 
 	// Register & subscribe
 	if (!sensor.Register()) {

firmware/init/sensor/init_tps.cpp

@@ -51,7 +51,7 @@ static bool initTpsFunc(LinearFunc& func, FunctionalSensor& sensor, adc_channel_
 
 	sensor.setFunction(func);
 
-	AdcSubscription::SubscribeSensor(sensor, channel);
+	AdcSubscription::SubscribeSensor(sensor, channel, 200);
 
 	if (!sensor.Register()) {
 		firmwareError(CUSTOM_INVALID_TPS_SETTING, "Duplicate registration for sensor \"%s\"", sensor.getSensorName());

firmware/util/math/biquad.cpp

@@ -6,28 +6,55 @@
  */
 
 #include "biquad.h"
+#include "error_handling.h"
+
+#include <math.h>
 
 Biquad::Biquad() {
-	a0 = a1 = a2 = b1 = b2 = 0;
+	// Default to passthru
+	a0 = 1;
+	a1 = a2 = b1 = b2 = 0;
 	z1 = z2 = 0;
 }
 
-// todo: decouple from engine and just use bi_quard_s
-void Biquad::initValue(float input, bi_quard_s *settings) {
-	a0 = settings->a0;
-	a1 = settings->a1;
-	a2 = settings->a2;
-	b1 = settings->b1;
-	b2 = settings->b2;
-
-	z1 = input * (1 - a0);
-	z2 = input * (1 - a0 - a1 + b1);
+static float getK(float samplingFrequency, float cutoff) {
+	return tanf(3.14159f * cutoff / samplingFrequency);
 }
 
-float Biquad::getValue(float input) {
-    float result = input * a0 + z1;
-    z1 = input * a1 + z2 - b1 * result;
-    z2 = input * a2 - b2 * result;
-    return result;
+static float getNorm(float K, float Q) {
+	return 1 / (1 + K / Q + K * K);
 }
 
+void Biquad::configureBandpass(float samplingFrequency, float centerFrequency, float Q) {
+	efiAssertVoid(OBD_PCM_Processor_Fault, samplingFrequency >= 2 * centerFrequency, "Invalid biquad parameters");
+
+	float K = getK(samplingFrequency, centerFrequency);
+	float norm = getNorm(K, Q);
+
+	a0 = K / Q * norm;
+	a1 = 0;
+	a2 = -a0;
+	b1 = 2 * (K * K - 1) * norm;
+	b2 = (1 - K / Q + K * K) * norm;
+}
+
+void Biquad::configureLowpass(float samplingFrequency, float cutoffFrequency, float Q) {
+	efiAssertVoid(OBD_PCM_Processor_Fault, samplingFrequency >= 2 * cutoffFrequency, "Invalid biquad parameters");
+
+	float K = getK(samplingFrequency, cutoffFrequency);
+	float norm = getNorm(K, Q);
+
+	norm = 1 / (1 + K / Q + K * K);
+	a0 = K * K * norm;
+	a1 = 2 * a0;
+	a2 = a0;
+	b1 = 2 * (K * K - 1) * norm;
+	b2 = (1 - K / Q + K * K) * norm;
+}
+
+float Biquad::filter(float input) {
+	float result = input * a0 + z1;
+	z1 = input * a1 + z2 - b1 * result;
+	z2 = input * a2 - b2 * result;
+	return result;
+}

firmware/util/math/biquad.h

@@ -7,16 +7,18 @@
 
 #pragma once
 
-// todo: narrow this dependency further? only 'bi_quard_s' is needed, should it be extracted / moved to a smaller header?
-// todo: do we need to make code generation smarted and produce a larger number of smaller headers instead of one monster header?
-#include "engine_configuration.h"
-
 class Biquad {
 public:
-    Biquad();
-    void initValue(float input, bi_quard_s *settings);
-    float getValue(float input);
+	Biquad();
 
+    float filter(float input);
+
+	void configureBandpass(float samplingFrequency, float centerFrequency, float Q);
+
+	// Default Q = 1/sqrt(2) = 0.707 gives a maximally flat passband without any overshoot near the cutoff (ie, Butterworth)
+	void configureLowpass(float samplingFrequency, float cutoffFrequency, float Q = 0.707f);
+
+private:
     float a0, a1, a2, b1, b2;
     float z1, z2;
 };

firmware/util/util.mk

@@ -11,6 +11,7 @@ UTILSRC_CPP = \
 	$(UTIL_DIR)/containers/counter64.cpp \
 	$(UTIL_DIR)/containers/local_version_holder.cpp \
 	$(UTIL_DIR)/containers/table_helper.cpp \
+	$(UTIL_DIR)/math/biquad.cpp \
 	$(UTIL_DIR)/math/pid.cpp \
 	$(UTIL_DIR)/math/interpolation.cpp \
 	$(PROJECT_DIR)/util/datalogging.cpp \