Start support for multi-bank closed loop fueling (#2184)

* cfg

* cell

* N banks

* tests

Co-authored-by: Matthew Kennedy <makenne@microsoft.com>

firmware/controllers/algo/engine2.cpp

@@ -139,7 +139,9 @@ void EngineState::periodicFastCallback(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	// todo: move this into slow callback, no reason for CLT corr to be here
 	running.coolantTemperatureCoefficient = getCltFuelCorrection(PASS_ENGINE_PARAMETER_SIGNATURE);
 
-	running.pidCorrection = fuelClosedLoopCorrection(PASS_ENGINE_PARAMETER_SIGNATURE);
+	// TODO: consume correction from the second bank
+	auto clResult = fuelClosedLoopCorrection(PASS_ENGINE_PARAMETER_SIGNATURE);
+	running.pidCorrection = clResult.banks[0];
 
 	// update fuel consumption states
 	fuelConsumption.update(nowNt PASS_ENGINE_PARAMETER_SUFFIX);

firmware/controllers/math/closed_loop_fuel.cpp

@@ -9,12 +9,24 @@
 
 EXTERN_ENGINE;
 
-ClosedLoopFuelCellImpl cells[STFT_CELL_COUNT];
+struct FuelingBank {
+	ClosedLoopFuelCellImpl cells[STFT_CELL_COUNT];
+};
+
+static FuelingBank banks[STFT_BANK_COUNT];
 
 static Deadband<25> idleDeadband;
 static Deadband<2> overrunDeadband;
 static Deadband<2> loadDeadband;
 
+static SensorType getSensorForBankIndex(size_t index) {
+	switch (index) {
+		case 0: return SensorType::Lambda1;
+		case 1: return SensorType::Lambda2;
+		default: return SensorType::Invalid;
+	}
+}
+
 size_t computeStftBin(int rpm, float load, stft_s& cfg) {
 	// Low RPM -> idle
 	if (idleDeadband.lt(rpm, cfg.maxIdleRegionRpm * RPM_1_BYTE_PACKING_MULT))
@@ -66,12 +78,12 @@ static bool shouldCorrect(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	return true;
 }
 
-bool shouldUpdateCorrection(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
+bool shouldUpdateCorrection(SensorType sensor DECLARE_ENGINE_PARAMETER_SUFFIX) {
 	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
-	auto afr = Sensor::get(SensorType::Lambda1).value_or(0) * 14.7f;
+	auto afr = Sensor::get(sensor).value_or(0) * 14.7f;
 	if (!afr || afr < (cfg.minAfr * 0.1f) || afr > (cfg.maxAfr * 0.1f)) {
 		return false;
 	}
@@ -79,9 +91,9 @@ bool shouldUpdateCorrection(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	return true;
 }
 
-float fuelClosedLoopCorrection(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
+ClosedLoopFuelResult fuelClosedLoopCorrection(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	if (!shouldCorrect(PASS_ENGINE_PARAMETER_SIGNATURE)) {
-		return 1.0f;
+		return {};
 	}
 
 	size_t binIdx = computeStftBin(GET_RPM(), getFuelingLoad(PASS_ENGINE_PARAMETER_SIGNATURE), CONFIG(stft));
@@ -92,14 +104,22 @@ float fuelClosedLoopCorrection(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	}
 #endif // EFI_TUNER_STUDIO
 
-	auto& cell = cells[binIdx];
+	ClosedLoopFuelResult result;
 
-	// todo: push configuration at startup
-	cell.configure(&CONFIG(stft.cellCfgs[binIdx]));
+	for (int i = 0; i < STFT_BANK_COUNT; i++) {
+		auto& cell = banks[i].cells[binIdx];
 
-	if (shouldUpdateCorrection(PASS_ENGINE_PARAMETER_SIGNATURE)) {
-		cell.update(CONFIG(stft.deadband) * 0.001f, CONFIG(stftIgnoreErrorMagnitude) PASS_ENGINE_PARAMETER_SUFFIX);
+		SensorType sensor = getSensorForBankIndex(i);
+
+		// todo: push configuration at startup
+		cell.configure(&CONFIG(stft.cellCfgs[binIdx]), sensor);
+
+		if (shouldUpdateCorrection(sensor PASS_ENGINE_PARAMETER_SUFFIX)) {
+			cell.update(CONFIG(stft.deadband) * 0.001f, CONFIG(stftIgnoreErrorMagnitude) PASS_ENGINE_PARAMETER_SUFFIX);
+		}
+
+		result.banks[i] = cell.getAdjustment();
 	}
 
-	return cell.getAdjustment();
+	return result;
 }

firmware/controllers/math/closed_loop_fuel.h

@@ -1,8 +1,22 @@
 #pragma once
 
-#include "globalaccess.h"
-#include "engine_configuration_generated_structures.h"
+#include "engine_ptr.h"
+#include "sensor.h"
+#include "rusefi_generated.h"
 
-float fuelClosedLoopCorrection(DECLARE_ENGINE_PARAMETER_SIGNATURE);
+struct stft_s;
+
+struct ClosedLoopFuelResult {
+	ClosedLoopFuelResult() {
+		// Default is no correction, aka 1.0 multiplier
+		for (size_t i = 0; i < STFT_BANK_COUNT; i++) {
+			banks[i] = 1.0f;
+		}
+	}
+
+	float banks[STFT_BANK_COUNT];
+};
+
+ClosedLoopFuelResult fuelClosedLoopCorrection(DECLARE_ENGINE_PARAMETER_SIGNATURE);
 size_t computeStftBin(int rpm, float load, stft_s& cfg);
-bool shouldUpdateCorrection(DECLARE_ENGINE_PARAMETER_SIGNATURE);
+bool shouldUpdateCorrection(SensorType sensor DECLARE_ENGINE_PARAMETER_SUFFIX);

firmware/controllers/math/closed_loop_fuel_cell.cpp

@@ -49,7 +49,7 @@ float ClosedLoopFuelCellBase::getAdjustment() const {
 }
 
 float ClosedLoopFuelCellImpl::getLambdaError(DECLARE_ENGINE_PARAMETER_SIGNATURE) const {
-	auto lambda = Sensor::get(SensorType::Lambda1);
+	auto lambda = Sensor::get(m_lambdaSensor);
 
 	// Failed sensor -> no error
 	if (!lambda) {

firmware/controllers/math/closed_loop_fuel_cell.h

@@ -1,6 +1,7 @@
 #pragma once
 
-#include "globalaccess.h"
+#include "engine_ptr.h"
+#include "sensor.h"
 
 class ClosedLoopFuelCellBase {
 public:
@@ -28,12 +29,14 @@ struct stft_cell_cfg_s;
 
 class ClosedLoopFuelCellImpl final : public ClosedLoopFuelCellBase {
 public:
-	void configure(const stft_cell_cfg_s* configuration) {
+	void configure(const stft_cell_cfg_s* configuration, SensorType lambdaSensor) {
 		m_config = configuration;
+		m_lambdaSensor = lambdaSensor;
 	}
 
 private:
 	const stft_cell_cfg_s *m_config = nullptr;
+	SensorType m_lambdaSensor = SensorType::Invalid;
 
 protected:
 	float getLambdaError(DECLARE_ENGINE_PARAMETER_SIGNATURE) const override;

firmware/integration/rusefi_config.txt

@@ -153,6 +153,7 @@ struct_no_prefix engine_configuration_s
 #define PEDAL_TO_TPS_SIZE 8
 
 #define STFT_CELL_COUNT 4
+#define STFT_BANK_COUNT 2
 
 #define CAN_DEFAULT_BASE 0x200
 

unit_tests/tests/test_stft.cpp

@@ -88,11 +88,11 @@ TEST(ClosedLoopFuel, afrLimits) {
 	engineConfiguration->stft.maxAfr = 180;  // 18.0 AFR
 
 	Sensor::setMockValue(SensorType::Lambda1, 0.1f);
-	EXPECT_FALSE(shouldUpdateCorrection(PASS_ENGINE_PARAMETER_SIGNATURE));
+	EXPECT_FALSE(shouldUpdateCorrection(SensorType::Lambda1 PASS_ENGINE_PARAMETER_SUFFIX));
 
 	Sensor::setMockValue(SensorType::Lambda1, 1.0f);
-	EXPECT_TRUE(shouldUpdateCorrection(PASS_ENGINE_PARAMETER_SIGNATURE));
+	EXPECT_TRUE(shouldUpdateCorrection(SensorType::Lambda1 PASS_ENGINE_PARAMETER_SUFFIX));
 
 	Sensor::setMockValue(SensorType::Lambda1, 2.0f);
-	EXPECT_FALSE(shouldUpdateCorrection(PASS_ENGINE_PARAMETER_SIGNATURE));
+	EXPECT_FALSE(shouldUpdateCorrection(SensorType::Lambda1 PASS_ENGINE_PARAMETER_SUFFIX));
 }