short term fuel trim: part 2 (#1405)

* add tooltip

* add other direction to deadband

* add impl

* test partitioning

* makefile

* wrong comment

* fix include

firmware/controllers/math/closed_loop_fuel.cpp

@@ -0,0 +1,105 @@
+#include "closed_loop_fuel.h"
+#include "closed_loop_fuel_cell.h"
+
+#include "engine.h"
+
+#include "sensor.h"
+#include "engine_math.h"
+#include "deadband.h"
+
+EXTERN_ENGINE;
+
+ClosedLoopFuelCellImpl cells[STFT_CELL_COUNT];
+
+static Deadband<25> idleDeadband;
+static Deadband<2> overrunDeadband;
+static Deadband<2> loadDeadband;
+
+size_t computeStftBin(int rpm, float load, stft_s& cfg) {
+	// Low RPM -> idle
+	if (idleDeadband.lt(rpm, cfg.maxIdleRegionRpm * RPM_1_BYTE_PACKING_MULT))
+	{
+		return 0;
+	}
+
+	// Low load -> overrun
+	if (overrunDeadband.lt(load, cfg.maxOverrunLoad))
+	{
+		return 1;
+	}
+
+	// High load -> power
+	if (loadDeadband.gt(load, cfg.minPowerLoad))
+	{
+		return 2;
+	}
+
+	// Default -> normal "in the middle" cell
+	return 3;
+}
+
+static bool shouldCorrect(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
+	const auto& cfg = CONFIG(stft);
+
+	// User disable bit
+	if (!CONFIG(fuelClosedLoopCorrectionEnabled)) {
+		return false;
+	}
+
+	// Don't correct if not running
+	if (!ENGINE(rpmCalculator).isRunning(PASS_ENGINE_PARAMETER_SIGNATURE)) {
+		return false;
+	}
+
+	// Startup delay - allow O2 sensor to warm up, etc
+	if (cfg.startupDelay > ENGINE(engineState.running.timeSinceCrankingInSecs)) {
+		return false;
+	}
+
+	// Check that the engine is hot enough (and clt not failed)
+	auto clt = Sensor::get(SensorType::Clt);
+	if (!clt.Valid || clt.Value < cfg.minClt) {
+		return false;
+	}
+
+	// If all was well, then we're enabled!
+	return true;
+}
+
+static bool shouldUpdateCorrection(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
+	const auto& cfg = CONFIG(stft);
+
+	// Pause (but don't reset) correction if the AFR is off scale.
+	// It's probably a transient and poorly tuned transient correction
+	float afr = ENGINE(sensors.currentAfr);
+	if (afr < (cfg.minAfr * 0.1f) || afr > (cfg.maxAfr * 0.1f)) {
+		return false;
+	}
+
+	return true;
+}
+
+float fuelClosedLoopCorrection(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
+	if (!shouldCorrect(PASS_ENGINE_PARAMETER_SIGNATURE)) {
+		return 1.0f;
+	}
+
+	size_t binIdx = computeStftBin(GET_RPM(), getEngineLoadT(PASS_ENGINE_PARAMETER_SIGNATURE), CONFIG(stft));
+
+#if EFI_TUNER_STUDIO
+	if (engineConfiguration->debugMode == DBG_FUEL_PID_CORRECTION) {
+		tsOutputChannels.debugIntField1 = binIdx;
+	}
+#endif // EFI_TUNER_STUDIO
+
+	auto& cell = cells[binIdx];
+
+	// todo: push configuration at startup
+	cell.configure(&CONFIG(stft.cellCfgs[binIdx]));
+
+	if (shouldUpdateCorrection(PASS_ENGINE_PARAMETER_SIGNATURE)) {
+		cell.update(CONFIG(stft.deadband) * 0.001f, CONFIG(stftIgnoreErrorMagnitude) PASS_ENGINE_PARAMETER_SUFFIX);
+	}
+
+	return cell.getAdjustment();
+}

firmware/controllers/math/closed_loop_fuel.h

@@ -0,0 +1,7 @@
+#pragma once
+
+#include "globalaccess.h"
+#include "engine_configuration_generated_structures.h"
+
+float fuelClosedLoopCorrection(DECLARE_ENGINE_PARAMETER_SIGNATURE);
+size_t computeStftBin(int rpm, float load, stft_s& cfg);

firmware/controllers/math/math.mk

@@ -4,5 +4,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/closed_loop_fuel.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 stftIgnoreErrorMagnitude
+	bit stftIgnoreErrorMagnitude;+If enabled, adjust at a constant rate instead of a rate proportional to the current lambda error.  This mode may be easier to tune, and more tolerant of sensor noise.  Use of this mode is required if you have a narrowband O2 sensor.;
 	bit unusedBit_251_11
 	bit unusedBit_251_12
 	bit unusedBit_251_13

firmware/util/math/deadband.h

@@ -24,6 +24,11 @@ public:
 		return m_lastState;
 	}
 
+	// Deadband has no concept of equal - only greater and less, so to compute gt, we just swap args
+	bool lt(float lhs, float rhs) {
+		return gt(rhs, lhs);
+	}
+
 private:
 	bool m_lastState =false;
 };

unit_tests/tests/test_stft.cpp

@@ -1,5 +1,6 @@
 
 #include "closed_loop_fuel_cell.h"
+#include "closed_loop_fuel.h"
 
 #include "engine.h"
 
@@ -34,7 +35,6 @@ TEST(ClosedLoopCell, TestDeadband) {
 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())
@@ -50,3 +50,32 @@ TEST(ClosedLoopFuelCell, AdjustRate) {
 	// dt = 1000.0f / FAST_CALLBACK_PERIOD_MS
 	EXPECT_FLOAT_EQ(cl.getAdjustment(), 1 + (0.2f / (1000.0f / FAST_CALLBACK_PERIOD_MS)));
 }
+
+TEST(ClosedLoopFuel, CellSelection) {
+	stft_s cfg;
+
+	// Sensible region config
+	cfg.maxIdleRegionRpm = 1500 / RPM_1_BYTE_PACKING_MULT;
+	cfg.minPowerLoad = 80;
+	cfg.maxOverrunLoad = 30;
+
+	// Test idle
+	EXPECT_EQ(0, computeStftBin(1000, 10, cfg));
+	EXPECT_EQ(0, computeStftBin(1000, 50, cfg));
+	EXPECT_EQ(0, computeStftBin(1000, 90, cfg));
+
+	// Test overrun
+	EXPECT_EQ(1, computeStftBin(2000, 10, cfg));
+	EXPECT_EQ(1, computeStftBin(4000, 10, cfg));
+	EXPECT_EQ(1, computeStftBin(10000, 10, cfg));
+
+	// Test load
+	EXPECT_EQ(2, computeStftBin(2000, 90, cfg));
+	EXPECT_EQ(2, computeStftBin(4000, 90, cfg));
+	EXPECT_EQ(2, computeStftBin(10000, 90, cfg));
+
+	// Main cell
+	EXPECT_EQ(3, computeStftBin(2000, 50, cfg));
+	EXPECT_EQ(3, computeStftBin(4000, 50, cfg));
+	EXPECT_EQ(3, computeStftBin(10000, 50, cfg));
+}