short term fuel trim: part 1 (#1402)

* add cell

* add stft cell tests

* add bit

* minimally generate

* config defaults

firmware/controllers/algo/engine_configuration.cpp

@@ -627,6 +627,44 @@ void setDefaultMultisparkParameters(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	engineConfiguration->multisparkMaxSparkingAngle = 30;
 }
 
+void setDefaultStftSettings(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
+	auto& cfg = CONFIG(stft);
+
+	// Default to disabled
+	CONFIG(fuelClosedLoopCorrectionEnabled) = false;
+
+	// Default to proportional mode (for wideband sensors)
+	CONFIG(stftIgnoreErrorMagnitude) = false;
+
+	// 60 second startup delay - some O2 sensors are slow to warm up.
+	cfg.startupDelay = 60;
+
+	// Only correct in [12.0, 17.0]
+	cfg.minAfr = 120;
+	cfg.maxAfr = 170;
+
+	// Above 60 deg C
+	cfg.minClt = 60;
+
+	// 0.5% deadband
+	cfg.deadband = 5;
+
+	// Sensible region defaults
+	cfg.maxIdleRegionRpm = 1000 / RPM_1_BYTE_PACKING_MULT;
+	cfg.maxOverrunLoad = 35;
+	cfg.minPowerLoad = 85;
+
+	// Sensible cell defaults
+	for (size_t i = 0; i < efi::size(cfg.cellCfgs); i++) {
+		// 30 second time constant - nice and slow
+		cfg.cellCfgs[i].timeConstant = 30 * 10;
+
+		/// Allow +-5%
+		cfg.cellCfgs[i].maxAdd = 5;
+		cfg.cellCfgs[i].maxRemove = -5;
+	}
+}
+
 void setDefaultGppwmParameters(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	// Same config for all channels
 	for (size_t i = 0; i < efi::size(CONFIG(gppwm)); i++) {
@@ -852,13 +890,7 @@ static void setDefaultEngineConfiguration(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 
 	setDefaultCrankingSettings(PASS_ENGINE_PARAMETER_SIGNATURE);
 
-	engineConfiguration->fuelClosedLoopCorrectionEnabled = false;
-	engineConfiguration->fuelClosedLoopCltThreshold = 70;
-	engineConfiguration->fuelClosedLoopRpmThreshold = 900;
-	engineConfiguration->fuelClosedLoopTpsThreshold = 80;
-	engineConfiguration->fuelClosedLoopAfrLowThreshold = 10.3;
-	engineConfiguration->fuelClosedLoopAfrHighThreshold = 19.8;
-	engineConfiguration->fuelClosedLoopPid.pFactor = -0.1;
+	setDefaultStftSettings(PASS_ENGINE_PARAMETER_SIGNATURE);
 
 	/**
 	 * Idle control defaults

firmware/controllers/math/closed_loop_fuel_cell.cpp

@@ -0,0 +1,90 @@
+#include "closed_loop_fuel_cell.h"
+#include "engine.h"
+#include "engine_configuration_generated_structures.h"
+
+EXTERN_ENGINE;
+
+constexpr float integrator_dt = FAST_CALLBACK_PERIOD_MS * 0.001f;
+
+void ClosedLoopFuelCellBase::update(float lambdaDeadband, bool ignoreErrorMagnitude DECLARE_ENGINE_PARAMETER_SUFFIX)
+{
+	// Compute how far off target we are
+	float lambdaError = getLambdaError(PASS_ENGINE_PARAMETER_SIGNATURE);
+
+	// If we're within the deadband, make no adjustment.
+	if (absF(lambdaError) < lambdaDeadband) {
+		return;
+	}
+
+	// Fixed magnitude - runs in constant adjustment rate mode
+	if (ignoreErrorMagnitude) {
+		if (lambdaError > 0) {
+			lambdaError = 0.1f;
+		} else {
+			lambdaError = -0.1f;
+		}
+	}
+
+	// Integrate
+	float adjust = getIntegratorGain() * lambdaError * integrator_dt
+					+ m_adjustment;
+
+	// Clamp to bounds
+	float minAdjust = getMinAdjustment();
+	float maxAdjust = getMaxAdjustment();
+
+	if (adjust > maxAdjust) {
+		adjust = maxAdjust;
+	} else if (adjust < minAdjust) {
+		adjust = minAdjust;
+	}
+
+	// Save state
+	m_adjustment = adjust;
+}
+
+float ClosedLoopFuelCellBase::getAdjustment() const {
+	return 1.0f + m_adjustment;
+}
+
+float ClosedLoopFuelCellImpl::getLambdaError(DECLARE_ENGINE_PARAMETER_SIGNATURE) const {
+	return (ENGINE(sensors.currentAfr) - ENGINE(engineState.targetAFR)) / 14.7f;
+}
+
+#define MAX_ADJ (0.25f)
+
+float ClosedLoopFuelCellImpl::getMaxAdjustment() const {
+	if (!m_config) {
+		// If no config, disallow adjustment.
+		return 0;
+	}
+
+	float raw = m_config->maxAdd * 0.01f;
+	// Don't allow maximum less than 0, or more than maximum adjustment
+	return minF(MAX_ADJ, maxF(raw, 0));
+}
+
+float ClosedLoopFuelCellImpl::getMinAdjustment() const {
+	if (!m_config) {
+		// If no config, disallow adjustment.
+		return 0;
+	}
+
+	float raw = m_config->maxRemove * 0.01f;
+	// Don't allow minimum more than 0, or more than maximum adjustment
+	return maxF(-MAX_ADJ, minF(raw, 0));
+}
+
+float ClosedLoopFuelCellImpl::getIntegratorGain() const {
+	if (!m_config) {
+		// If no config, disallow adjustment.
+		return 0.0f;
+	}
+
+	float timeConstant = m_config->timeConstant * 0.1f;
+
+	// Clamp to reasonable limits - 100ms to 100s
+	timeConstant = maxF(0.1f, minF(timeConstant, 100));
+
+	return 1 / timeConstant;
+}

firmware/controllers/math/closed_loop_fuel_cell.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include "globalaccess.h"
+
+class ClosedLoopFuelCellBase {
+public:
+	// Update the cell's internal state - adjusting fuel up/down as appropriate
+	void update(float lambdaDeadband, bool ignoreErrorMagnitude DECLARE_ENGINE_PARAMETER_SUFFIX);
+
+	// Get the current adjustment amount, without altering internal state.
+	float getAdjustment() const;
+
+protected:
+	// Helpers - virtual for mocking
+	virtual float getLambdaError(DECLARE_ENGINE_PARAMETER_SIGNATURE) const = 0;
+	virtual float getMaxAdjustment() const = 0;
+	virtual float getMinAdjustment() const = 0;
+	virtual float getIntegratorGain() const = 0;
+
+private:
+	// Current fueling adjustment.
+	// 0 = no adjustment.
+	// 0.1 = add 10% fuel.
+	float m_adjustment = 0;
+};
+
+struct stft_cell_cfg_s;
+
+class ClosedLoopFuelCellImpl final : public ClosedLoopFuelCellBase {
+public:
+	void configure(const stft_cell_cfg_s* configuration) {
+		m_config = configuration;
+	}
+
+private:
+	const stft_cell_cfg_s *m_config = nullptr;
+
+protected:
+	float getLambdaError(DECLARE_ENGINE_PARAMETER_SIGNATURE) const override;
+	float getMaxAdjustment() const override;
+	float getMinAdjustment() const override;
+	float getIntegratorGain() const override;
+};

firmware/controllers/math/math.mk

@@ -3,4 +3,6 @@ CONTROLLERS_MATH_SRC =
 
 CONTROLLERS_MATH_SRC_CPP = $(PROJECT_DIR)/controllers/math/engine_math.cpp \
 	$(PROJECT_DIR)/controllers/math/pid_auto_tune.cpp \
-	$(PROJECT_DIR)/controllers/math/speed_density.cpp
+	$(PROJECT_DIR)/controllers/math/speed_density.cpp \
+	$(PROJECT_DIR)/controllers/math/closed_loop_fuel_cell.cpp \
+

firmware/integration/rusefi_config.txt

@@ -817,7 +817,7 @@ custom maf_sensor_type_e 4 bits, S32, @OFFSET@, [0:7], @@maf_sensor_type_e_enum@
 	bit enableCanVss
 	bit enableInnovateLC2
 	bit showHumanReadableWarning
-	bit unusedBit_251_10
+	bit stftIgnoreErrorMagnitude
 	bit unusedBit_251_11
 	bit unusedBit_251_12
 	bit unusedBit_251_13

unit_tests/tests/test_stft.cpp

@@ -0,0 +1,52 @@
+
+#include "closed_loop_fuel_cell.h"
+
+#include "engine.h"
+
+#include "gtest/gtest.h"
+#include "gmock/gmock.h"
+
+using ::testing::_;
+using ::testing::Return;
+using ::testing::StrictMock;
+
+class MockClCell : public ClosedLoopFuelCellBase {
+public:
+	MOCK_METHOD(float, getLambdaError, (DECLARE_ENGINE_PARAMETER_SIGNATURE), (const));
+	MOCK_METHOD(float, getMaxAdjustment, (), (const));
+	MOCK_METHOD(float, getMinAdjustment, (), (const));
+	MOCK_METHOD(float, getIntegratorGain, (), (const));
+};
+
+TEST(ClosedLoopCell, TestDeadband) {
+	StrictMock<MockClCell> cl;
+
+	// Error is more than deadtime, so nothing else should be called
+	EXPECT_CALL(cl, getLambdaError(_, _, _))
+		.WillOnce(Return(0.05f));
+
+	cl.update(0.1f, true, nullptr, nullptr, nullptr);
+
+	// Should be zero adjustment
+	EXPECT_FLOAT_EQ(cl.getAdjustment(), 1.0f);
+}
+
+TEST(ClosedLoopFuelCell, AdjustRate) {
+	StrictMock<MockClCell> cl;
+
+	// Error is more than deadtime, so nothing else should be called
+	EXPECT_CALL(cl, getLambdaError(_, _, _))
+		.WillOnce(Return(0.1f));
+	EXPECT_CALL(cl, getMinAdjustment())
+		.WillOnce(Return(-0.2f));
+	EXPECT_CALL(cl, getMaxAdjustment())
+		.WillOnce(Return(0.2f));
+	EXPECT_CALL(cl, getIntegratorGain())
+		.WillOnce(Return(2.0f));
+
+	cl.update(0.0f, false, nullptr, nullptr, nullptr);
+
+	// Should have integrated 0.2 * dt
+	// dt = 1000.0f / FAST_CALLBACK_PERIOD_MS
+	EXPECT_FLOAT_EQ(cl.getAdjustment(), 1 + (0.2f / (1000.0f / FAST_CALLBACK_PERIOD_MS)));
+}

unit_tests/tests/tests.mk

@@ -49,6 +49,7 @@ TESTS_SRC_CPP = \
 	tests/sensor/redundant.cpp \
 	tests/sensor/test_sensor_init.cpp \
 	tests/test_closed_loop_controller.cpp \
+	tests/test_stft.cpp \
 	tests/test_boost.cpp \
 	tests/test_gppwm.cpp \
 	tests/test_fuel_math.cpp \