error accumulator (#2434)

* error accumulator

* makefile

firmware/util/math/error_accumulator.cpp

@@ -0,0 +1,28 @@
+#include "error_accumulator.h"
+#include "efilib.h"
+
+float ErrorAccumulator::accumulate(float error) {
+	// We only care about the absolute value of the error
+	error = absF(error);
+
+	// If m_ignoreError is 5, for example:
+	// 0 error		-> bleeds down at 5 per second
+	// 5 error		-> integral stays where it is
+	// 10 error		-> integral grows at 5 per second
+	float accumulationRate = error - m_ignoreError;
+
+	float newIntegral = accumulationRate * m_dt + m_errorIntegral;
+
+	// Don't allow less than 0 error
+	if (newIntegral < 0) {
+		newIntegral = 0;
+	}
+
+	m_errorIntegral = newIntegral;
+
+	return newIntegral;
+}
+
+void ErrorAccumulator::reset() {
+	m_errorIntegral = 0;
+}

firmware/util/math/error_accumulator.h

@@ -0,0 +1,25 @@
+#pragma once
+
+class ErrorAccumulator {
+public:
+	void init(float ignoreError, float dt) {
+		m_ignoreError = ignoreError;
+		m_dt = dt;
+	}
+
+	// Accumulate the current error, returning the total integrated error
+	float accumulate(float error);
+
+	// Get the current accumulator value
+	float getAccumulator() const {
+		return m_errorIntegral;
+	}
+
+	// Reset the integrator to 0 error.
+	void reset();
+
+private:
+	float m_ignoreError = 0;
+	float m_errorIntegral = 0;
+	float m_dt = 0;
+};

firmware/util/util.mk

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

unit_tests/tests/tests.mk

@@ -17,6 +17,7 @@ TESTS_SRC_CPP = \
 	tests/ignition_injection/test_injector_model.cpp \
 	tests/sensor/test_cj125.cpp \
 	tests/util/test_buffered_writer.cpp \
+	tests/util/test_error_accumulator.cpp \
 	tests/test_util.cpp \
 	tests/test_start_stop.cpp \
 	tests/test_hardware_reinit.cpp \

unit_tests/tests/util/test_error_accumulator.cpp

@@ -0,0 +1,52 @@
+#include <gtest/gtest.h>
+#include "error_accumulator.h"
+
+TEST(errorAccumulator, ignoreSmallError) {
+	ErrorAccumulator dut;
+	dut.init(5, 0.01);
+
+	for (size_t i = 0; i < 1'000'000; i++) {
+		// An error just below the threshold should never trip
+		ASSERT_EQ(0, dut.accumulate(4));
+	}
+}
+
+TEST(errorAccumulator, integrateError) {
+	ErrorAccumulator dut;
+	dut.init(5, 0.01);
+
+	for (size_t i = 0; i < 100; i++) {
+		// error of 1 over the ignore value
+		dut.accumulate(6);
+	}
+
+	// Integral should be 1 * dt * 100 = 1.0
+	ASSERT_NEAR(dut.getAccumulator(), 1, 0.001f);
+}
+
+TEST(errorAccumulator, integrateNegativeError) {
+	ErrorAccumulator dut;
+	dut.init(5, 0.01);
+
+	for (size_t i = 0; i < 100; i++) {
+		// error of 1 over the ignore value, but negative
+		dut.accumulate(-6);
+	}
+
+	// Integral should be 1 * dt * 100 = 1.0
+	ASSERT_NEAR(dut.getAccumulator(), 1, 0.001f);
+}
+
+TEST(errorAccumulator, integrateErrorBothSigns) {
+	ErrorAccumulator dut;
+	dut.init(5, 0.01);
+
+	for (size_t i = 0; i < 100; i++) {
+		// These should collectively integrate 1 * dt worth of error
+		dut.accumulate(-5.5);
+		dut.accumulate(5.5);
+	}
+
+	// Integral should be 2 * 0.5 * dt * 100 = 1.0
+	ASSERT_NEAR(dut.getAccumulator(), 1, 0.001f);
+}