diff --git a/firmware/hw_layer/adc/adc_inputs.cpp b/firmware/hw_layer/adc/adc_inputs.cpp index 612f51efa9..8eeb408a72 100644 --- a/firmware/hw_layer/adc/adc_inputs.cpp +++ b/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) { } diff --git a/firmware/hw_layer/adc/adc_inputs.h b/firmware/hw_layer/adc/adc_inputs.h index 6bf231b9a4..048bb3003e 100644 --- a/firmware/hw_layer/adc/adc_inputs.h +++ b/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(); diff --git a/firmware/hw_layer/adc/adc_subscription.cpp b/firmware/hw_layer/adc/adc_subscription.cpp index 37a24eb62f..8f59ae2578 100644 --- a/firmware/hw_layer/adc/adc_subscription.cpp +++ b/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 @@ -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); } } diff --git a/firmware/hw_layer/adc/adc_subscription.h b/firmware/hw_layer/adc/adc_subscription.h index f318b10333..f8f85728f8 100644 --- a/firmware/hw_layer/adc/adc_subscription.h +++ b/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); }; diff --git a/firmware/init/sensor/init_oil_pressure.cpp b/firmware/init/sensor/init_oil_pressure.cpp index 0cf3e34923..15b5e73d34 100644 --- a/firmware/init/sensor/init_oil_pressure.cpp +++ b/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"); diff --git a/firmware/init/sensor/init_thermistors.cpp b/firmware/init/sensor/init_thermistors.cpp index 65eed4f2f0..7cd736222d 100644 --- a/firmware/init/sensor/init_thermistors.cpp +++ b/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()) { diff --git a/firmware/init/sensor/init_tps.cpp b/firmware/init/sensor/init_tps.cpp index 46c8609c3b..477636abcc 100644 --- a/firmware/init/sensor/init_tps.cpp +++ b/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()); diff --git a/firmware/util/math/biquad.cpp b/firmware/util/math/biquad.cpp index 44adf85fed..8cfa342ff5 100644 --- a/firmware/util/math/biquad.cpp +++ b/firmware/util/math/biquad.cpp @@ -6,28 +6,55 @@ */ #include "biquad.h" +#include "error_handling.h" + +#include 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; +} diff --git a/firmware/util/math/biquad.h b/firmware/util/math/biquad.h index d22d2a80dd..7ef51beba9 100644 --- a/firmware/util/math/biquad.h +++ b/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; }; diff --git a/firmware/util/util.mk b/firmware/util/util.mk index c227e632d9..534439946b 100644 --- a/firmware/util/util.mk +++ b/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 \