From a6f7d9e238caf660ac8e554108bd797ce0157cab Mon Sep 17 00:00:00 2001 From: andreika-git Date: Tue, 23 Jan 2018 15:24:13 +0200 Subject: [PATCH] Impl. narrow EGO averaging (alpha) (#545) * Impl. narrow EGO averaging (alpha) * Use cyclic_buffer as a generic container for CIC * Implicit #include of cyclic_buffer.h for safety --- firmware/controllers/engine_controller.cpp | 2 + firmware/controllers/sensors/ego.cpp | 81 +++++++++++++++++++++- firmware/controllers/sensors/ego.h | 1 + firmware/util/cyclic_buffer.h | 68 +++++++++--------- 4 files changed, 117 insertions(+), 35 deletions(-) diff --git a/firmware/controllers/engine_controller.cpp b/firmware/controllers/engine_controller.cpp index 06bf77c337..8734dda6a6 100644 --- a/firmware/controllers/engine_controller.cpp +++ b/firmware/controllers/engine_controller.cpp @@ -675,6 +675,8 @@ void initEngineContoller(Logging *sharedLogger DECLARE_ENGINE_PARAMETER_SUFFIX) } #endif /* EFI_MAP_AVERAGING */ + initEgoAveraging(PASS_ENGINE_PARAMETER_SIGNATURE); + #if EFI_ENGINE_CONTROL || defined(__DOXYGEN__) if (boardConfiguration->isEngineControlEnabled) { /** diff --git a/firmware/controllers/sensors/ego.cpp b/firmware/controllers/sensors/ego.cpp index 68aa578b36..d30245719d 100644 --- a/firmware/controllers/sensors/ego.cpp +++ b/firmware/controllers/sensors/ego.cpp @@ -12,9 +12,81 @@ #include "interpolation.h" #include "engine.h" #include "analog_input.h" +#include "cyclic_buffer.h" EXTERN_ENGINE; +#ifdef EFI_NARROW_EGO_AVERAGING +// Needed by narrow EGOs (see updateEgoAverage()). +// getAfr() is called at ~50Hz, so we store at most (1<<3)*32 EGO values for ~5 secs. +#define EGO_AVG_SHIFT 3 +#define EGO_AVG_BUF_SIZE 32 // 32*sizeof(float) + +static bool useAveraging = false; +// Circular running-average buffer, used by CIC-like averaging filter +static cyclic_buffer egoAfrBuf; +// Total ego iterations (>240 days max. for 10ms update period) +static int totalEgoCnt = 0; +// We need this to calculate the real number of values stored in the buffer. +static int prevEgoCnt = 0; + +// todo: move it to engineConfiguration +static const float stoichAfr = 14.7f; +static const float maxAfrDeviation = 5.0f; // 9.7..19.7 +static const int minAvgSize = (EGO_AVG_BUF_SIZE / 2); // ~0.6 sec for 20ms period of 'fast' callback, and it matches a lag time of most narrow EGOs +static const int maxAvgSize = (EGO_AVG_BUF_SIZE - 1); // the whole buffer + +// we store the last measured AFR value to predict the current averaging window size +static float lastAfr = stoichAfr; + + +void initEgoAveraging(DECLARE_ENGINE_PARAMETER_SIGNATURE) { + // Our averaging is intended for use only with Narrow EGOs. + if (boardConfiguration->afr_type == ES_NarrowBand) { + totalEgoCnt = prevEgoCnt = 0; + egoAfrBuf.clear(); + useAveraging = true; + } +} + +static float updateEgoAverage(float afr) { + // use a variation of cascaded integrator-comb (CIC) filtering + totalEgoCnt++; + int localBufPos = (totalEgoCnt >> EGO_AVG_SHIFT) % EGO_AVG_BUF_SIZE; + int localPrevBufPos = ((totalEgoCnt - 1) >> EGO_AVG_SHIFT) % EGO_AVG_BUF_SIZE; + + // reset old buffer cell + if (localPrevBufPos != localBufPos) { + egoAfrBuf.elements[localBufPos] = 0; + // Remove (1 << EGO_AVG_SHIFT) elements from our circular buffer (except for the 1st cycle). + if (totalEgoCnt >= (EGO_AVG_BUF_SIZE << EGO_AVG_SHIFT)) + prevEgoCnt += (1 << EGO_AVG_SHIFT); + } + // integrator stage + egoAfrBuf.elements[localBufPos] += afr; + + // Change window size depending on |AFR-stoich| deviation. + // The narrow EGO is very noisy when AFR is close to shoich. + // And we need the fastest EGO response when AFR has the extreme deviation (way too lean/rich). + float avgSize = maxAvgSize;//interpolateClamped(minAvgSize, maxAfrDeviation, maxAvgSize, 0.0f, absF(lastAfr - stoichAfr)); + // choose the number of recently filled buffer cells for averaging + int avgCnt = (int)efiRound(avgSize, 1.0f) << EGO_AVG_SHIFT; + // limit averaging count to the real stored count + int startAvgCnt = maxI(totalEgoCnt - avgCnt, prevEgoCnt); + + // return moving average of N last sums + float egoAfrSum = 0; + for (int i = (totalEgoCnt >> EGO_AVG_SHIFT); i >= (startAvgCnt >> EGO_AVG_SHIFT); i--) { + egoAfrSum += egoAfrBuf.elements[i % EGO_AVG_BUF_SIZE]; + } + // we divide by a real number of stored values to get an exact average + return egoAfrSum / float(totalEgoCnt - startAvgCnt); +} +#else +void initEgoAveraging(DECLARE_ENGINE_PARAMETER_SIGNATURE) { +} +#endif + bool hasAfrSensor(DECLARE_ENGINE_PARAMETER_SIGNATURE) { return engineConfiguration->afr.hwChannel != EFI_ADC_NONE; } @@ -25,7 +97,14 @@ float getAfr(DECLARE_ENGINE_PARAMETER_SIGNATURE) { float volts = getVoltageDivided("ego", sensor->hwChannel); if (boardConfiguration->afr_type == ES_NarrowBand) { - return interpolate2d("narrow", volts, engineConfiguration->narrowToWideOxygenBins, engineConfiguration->narrowToWideOxygen, NARROW_BAND_WIDE_BAND_CONVERSION_SIZE); + float afr = interpolate2d("narrow", volts, engineConfiguration->narrowToWideOxygenBins, engineConfiguration->narrowToWideOxygen, NARROW_BAND_WIDE_BAND_CONVERSION_SIZE); +#ifdef EFI_NARROW_EGO_AVERAGING + if (useAveraging) + afr = updateEgoAverage(afr); + return (lastAfr = afr); +#else + return afr; +#endif } return interpolate(sensor->v1, sensor->value1, sensor->v2, sensor->value2, volts) diff --git a/firmware/controllers/sensors/ego.h b/firmware/controllers/sensors/ego.h index 652343d69a..4b6e41b504 100644 --- a/firmware/controllers/sensors/ego.h +++ b/firmware/controllers/sensors/ego.h @@ -17,5 +17,6 @@ float getAfr(DECLARE_ENGINE_PARAMETER_SIGNATURE); bool hasAfrSensor(DECLARE_ENGINE_PARAMETER_SIGNATURE); void setEgoSensor(ego_sensor_e type DECLARE_ENGINE_PARAMETER_SUFFIX); +void initEgoAveraging(DECLARE_ENGINE_PARAMETER_SIGNATURE); #endif diff --git a/firmware/util/cyclic_buffer.h b/firmware/util/cyclic_buffer.h index c22311f3e6..8251dcf451 100644 --- a/firmware/util/cyclic_buffer.h +++ b/firmware/util/cyclic_buffer.h @@ -19,7 +19,7 @@ static const short CB_MAX_SIZE = 128; #define BUFFER_MAX_VALUE 200123 -template +template class cyclic_buffer { public: @@ -44,7 +44,7 @@ class cyclic_buffer int getSize(); int getCount(); void clear(); - volatile T elements[CB_MAX_SIZE]; + volatile T elements[maxSize]; volatile int currentIndex; private: @@ -56,24 +56,24 @@ class cyclic_buffer int size; }; -template -cyclic_buffer::cyclic_buffer() { - baseC(CB_MAX_SIZE); +template +cyclic_buffer::cyclic_buffer() { + baseC(maxSize); } -template -cyclic_buffer::cyclic_buffer(int size) { +template +cyclic_buffer::cyclic_buffer(int size) { baseC(size); } -template -void cyclic_buffer::baseC(int size) { +template +void cyclic_buffer::baseC(int size) { currentIndex = 0; setSize(size); } -template -cyclic_buffer::cyclic_buffer(const cyclic_buffer& cb) { +template +cyclic_buffer::cyclic_buffer(const cyclic_buffer& cb) { //Deep copy the data currentIndex = cb.currentIndex; count = cb.count; @@ -83,13 +83,13 @@ cyclic_buffer::cyclic_buffer(const cyclic_buffer& cb) { } } -template -cyclic_buffer::~cyclic_buffer() { +template +cyclic_buffer::~cyclic_buffer() { //No dynamic allocation - safe to leave } -//template -//cyclic_buffer& cyclic_buffer::operator=(const cyclic_buffer& rhCb) { +//template +//cyclic_buffer& cyclic_buffer::operator=(const cyclic_buffer& rhCb) { // //Deep copy // currentIndex = rhCb.currentIndex; // count = rhCb.count; @@ -99,8 +99,8 @@ cyclic_buffer::~cyclic_buffer() { // return *this; //} -template -void cyclic_buffer::add(T value) { +template +void cyclic_buffer::add(T value) { elements[currentIndex] = value; ++currentIndex; @@ -111,24 +111,24 @@ void cyclic_buffer::add(T value) { ++count; } -template -void cyclic_buffer::setSize(int size) { +template +void cyclic_buffer::setSize(int size) { clear(); - this->size = size < CB_MAX_SIZE ? size : CB_MAX_SIZE; + this->size = size < maxSize ? size : maxSize; } -template -int cyclic_buffer::getSize() { +template +int cyclic_buffer::getSize() { return size; } -template -int cyclic_buffer::getCount() { +template +int cyclic_buffer::getCount() { return count; } -template -T cyclic_buffer::get(int index) { +template +T cyclic_buffer::get(int index) { while (index < 0) { index += size; } @@ -138,8 +138,8 @@ T cyclic_buffer::get(int index) { return elements[index]; } -template -T cyclic_buffer::maxValue(int length) { +template +T cyclic_buffer::maxValue(int length) { if (length > count) { // not enough data in buffer length = count; @@ -159,8 +159,8 @@ T cyclic_buffer::maxValue(int length) { return result; } -template -T cyclic_buffer::minValue(int length) { +template +T cyclic_buffer::minValue(int length) { if (length > count) { length = count; } @@ -179,8 +179,8 @@ T cyclic_buffer::minValue(int length) { return result; } -template -T cyclic_buffer::sum(int length) { +template +T cyclic_buffer::sum(int length) { if (length > count) { length = count; } @@ -200,8 +200,8 @@ T cyclic_buffer::sum(int length) { return result; } -template -void cyclic_buffer::clear() { +template +void cyclic_buffer::clear() { memset((void*) elements, 0, sizeof(elements)); // I would usually use static_cast, but due to the elements being volatile we cannot. count = 0; currentIndex = 0;