From 053835a1c038214d615d92db052c3531626a74a7 Mon Sep 17 00:00:00 2001
From: Matthew Kennedy <matthewkennedy@outlook.com>
Date: Mon, 8 Mar 2021 04:40:46 -0800
Subject: [PATCH] error accumulator (#2434)

* error accumulator

* makefile
---
 firmware/util/math/error_accumulator.cpp      | 28 ++++++++++
 firmware/util/math/error_accumulator.h        | 25 +++++++++
 firmware/util/util.mk                         |  1 +
 unit_tests/tests/tests.mk                     |  1 +
 .../tests/util/test_error_accumulator.cpp     | 52 +++++++++++++++++++
 5 files changed, 107 insertions(+)
 create mode 100644 firmware/util/math/error_accumulator.cpp
 create mode 100644 firmware/util/math/error_accumulator.h
 create mode 100644 unit_tests/tests/util/test_error_accumulator.cpp

diff --git a/firmware/util/math/error_accumulator.cpp b/firmware/util/math/error_accumulator.cpp
new file mode 100644
index 0000000000..10380659e4
--- /dev/null
+++ b/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;
+}
diff --git a/firmware/util/math/error_accumulator.h b/firmware/util/math/error_accumulator.h
new file mode 100644
index 0000000000..444258d9d7
--- /dev/null
+++ b/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;
+};
diff --git a/firmware/util/util.mk b/firmware/util/util.mk
index ef24d8396b..3f403c004d 100644
--- a/firmware/util/util.mk
+++ b/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 \
diff --git a/unit_tests/tests/tests.mk b/unit_tests/tests/tests.mk
index 0e16bbad62..07438cda00 100644
--- a/unit_tests/tests/tests.mk
+++ b/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 \
diff --git a/unit_tests/tests/util/test_error_accumulator.cpp b/unit_tests/tests/util/test_error_accumulator.cpp
new file mode 100644
index 0000000000..673ef737f6
--- /dev/null
+++ b/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);
+}