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

@@ -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) {
 		/**

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<float, EGO_AVG_BUF_SIZE> 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)

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

firmware/util/cyclic_buffer.h

@@ -19,7 +19,7 @@ static const short CB_MAX_SIZE = 128;
 
 #define BUFFER_MAX_VALUE 200123
 
-template<typename T>
+template<typename T, size_t maxSize = CB_MAX_SIZE>
 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<typename T>
-cyclic_buffer<T>::cyclic_buffer() {
-  baseC(CB_MAX_SIZE);
+template<typename T, size_t maxSize>
+cyclic_buffer<T, maxSize>::cyclic_buffer() {
+  baseC(maxSize);
 }
 
-template<typename T>
-cyclic_buffer<T>::cyclic_buffer(int size) {
+template<typename T, size_t maxSize>
+cyclic_buffer<T, maxSize>::cyclic_buffer(int size) {
   baseC(size);
 }
 
-template<typename T>
-void cyclic_buffer<T>::baseC(int size) {
+template<typename T, size_t maxSize>
+void cyclic_buffer<T, maxSize>::baseC(int size) {
 	currentIndex = 0;
 	setSize(size);
 }
 
-template<typename T>
-cyclic_buffer<T>::cyclic_buffer(const cyclic_buffer& cb) {
+template<typename T, size_t maxSize>
+cyclic_buffer<T, maxSize>::cyclic_buffer(const cyclic_buffer& cb) {
 	//Deep copy the data
 	currentIndex = cb.currentIndex;
 	count = cb.count;
@@ -83,13 +83,13 @@ cyclic_buffer<T>::cyclic_buffer(const cyclic_buffer& cb) {
 	}
 }
 
-template<typename T>
-cyclic_buffer<T>::~cyclic_buffer() {
+template<typename T, size_t maxSize>
+cyclic_buffer<T, maxSize>::~cyclic_buffer() {
 	//No dynamic allocation - safe to leave
 }
 
-//template<typename T>
-//cyclic_buffer& cyclic_buffer<T>::operator=(const cyclic_buffer<T>& rhCb) {
+//template<typename T, size_t maxSize>
+//cyclic_buffer& cyclic_buffer<T, maxSize>::operator=(const cyclic_buffer<T, maxSize>& rhCb) {
 //	//Deep copy
 //	currentIndex = rhCb.currentIndex;
 //	count = rhCb.count;
@@ -99,8 +99,8 @@ cyclic_buffer<T>::~cyclic_buffer() {
 //	return *this;
 //}
 
-template<typename T>
-void cyclic_buffer<T>::add(T value) {
+template<typename T, size_t maxSize>
+void cyclic_buffer<T, maxSize>::add(T value) {
 	elements[currentIndex] = value;
 
 	++currentIndex;
@@ -111,24 +111,24 @@ void cyclic_buffer<T>::add(T value) {
 	++count;
 }
 
-template<typename T>
-void cyclic_buffer<T>::setSize(int size) {
+template<typename T, size_t maxSize>
+void cyclic_buffer<T, maxSize>::setSize(int size) {
 	clear();
-	this->size = size < CB_MAX_SIZE ? size : CB_MAX_SIZE;
+	this->size = size < maxSize ? size : maxSize;
 }
 
-template<typename T>
-int cyclic_buffer<T>::getSize() {
+template<typename T, size_t maxSize>
+int cyclic_buffer<T, maxSize>::getSize() {
 	return size;
 }
 
-template<typename T>
-int cyclic_buffer<T>::getCount() {
+template<typename T, size_t maxSize>
+int cyclic_buffer<T, maxSize>::getCount() {
 	return count;
 }
 
-template<typename T>
-T cyclic_buffer<T>::get(int index) {
+template<typename T, size_t maxSize>
+T cyclic_buffer<T, maxSize>::get(int index) {
 	while (index < 0) {
 		index += size;
 	}
@@ -138,8 +138,8 @@ T cyclic_buffer<T>::get(int index) {
 	return elements[index];
 }
 
-template<typename T>
-T cyclic_buffer<T>::maxValue(int length) {
+template<typename T, size_t maxSize>
+T cyclic_buffer<T, maxSize>::maxValue(int length) {
 	if (length > count) {
 		// not enough data in buffer
 		length = count;
@@ -159,8 +159,8 @@ T cyclic_buffer<T>::maxValue(int length) {
 	return result;
 }
 
-template<typename T>
-T cyclic_buffer<T>::minValue(int length) {
+template<typename T, size_t maxSize>
+T cyclic_buffer<T, maxSize>::minValue(int length) {
 	if (length > count) {
 		length = count;
 	}
@@ -179,8 +179,8 @@ T cyclic_buffer<T>::minValue(int length) {
 	return result;
 }
 
-template<typename T>
-T cyclic_buffer<T>::sum(int length) {
+template<typename T, size_t maxSize>
+T cyclic_buffer<T, maxSize>::sum(int length) {
 	if (length > count) {
 		length = count;
 	}
@@ -200,8 +200,8 @@ T cyclic_buffer<T>::sum(int length) {
 	return result;
 }
 
-template<typename T>
-void cyclic_buffer<T>::clear() {
+template<typename T, size_t maxSize>
+void cyclic_buffer<T, maxSize>::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;