stub lambda monitor #75

firmware/controllers/math/lambda_monitor.cpp

@@ -0,0 +1,79 @@
+#include "pch.h"
+
+#include "lambda_monitor.h"
+
+float LambdaMonitor::getMaxAllowedLambda(float rpm, float load) const {
+	return
+		engine->fuelComputer.targetLambda
+		+ interpolate3d(
+			config->lambdaMaxDeviationTable,
+			config->lambdaMaxDeviationLoadBins, load,
+			config->lambdaMaxDeviationRpmBins, rpm
+		);
+}
+
+void LambdaMonitorBase::update(float rpm, float load) {
+	if (isCurrentlyGood(rpm, load)) {
+		m_timeSinceGoodLambda.reset();
+	}
+
+	float timeout = engineConfiguration->lambdaProtectionTimeout;
+
+	if (m_timeSinceGoodLambda.hasElapsedSec(timeout)) {
+		// Things have been bad long enough, respond!
+
+		// TODO
+	}
+}
+
+
+bool LambdaMonitorBase::isCurrentlyGood(float rpm, float load) const {
+	// Lambda is always good if disabled
+	if (!engineConfiguration->lambdaProtectionEnable) {
+		return true;
+	}
+
+	// Below min RPM, don't check
+	if (rpm < engineConfiguration->lambdaProtectionMinRpm) {
+		return true;
+	}
+
+	// Below min load, don't check
+	if (load < engineConfiguration->lambdaProtectionMinLoad) {
+		return true;
+	}
+
+	// Below min TPS, don't check
+	if (Sensor::getOrZero(SensorType::Tps1) <= engineConfiguration->lambdaProtectionMinTps) {
+		return true;
+	}
+
+	// Pause checking if DFCO was active recently
+	auto timeSinceDfco = engine->module<DfcoController>()->getTimeSinceCut();
+	if (timeSinceDfco < engineConfiguration->noFuelTrimAfterDfcoTime) {
+		return true;
+	}
+
+	// Pause checking if some other cut was active recently
+	auto timeSinceFuelCut = engine->module<LimpManager>()->getTimeSinceAnyCut();
+	// TODO: should this duration be configurable?
+	if (timeSinceFuelCut < 2) {
+		return true;
+	}
+
+	// TODO: multiple banks
+	if (auto lambda = Sensor::get(SensorType::Lambda1)) {
+		if (lambda.Value < getMaxAllowedLambda(rpm, load)) {
+			// Lambda is OK, we're good.
+			return true;
+		}
+	} else {
+		// Broken lambda sensor doesn't imply bad lambda
+
+		// TODO: can/should we be smarter here?
+		return true;
+	}
+
+	// All checks failed, lambda is currently bad.
+	return false;
+}

firmware/controllers/math/lambda_monitor.h

@@ -0,0 +1,22 @@
+#pragma once
+
+struct LambdaMonitorBase {
+	void update(float rpm, float load);
+
+protected:
+	// Returns whether lambda checking should happen at all
+	bool shouldCheckLambda(float rpm, float load) const;
+
+	// Returns false if the current lambda reading is leaner than allowable.
+	// Returns true in any other case (rich enough, bad sensor, recent fuel cut, rpm to low, load too low, etc)
+	bool isCurrentlyGood(float rpm, float load) const;
+	virtual float getMaxAllowedLambda(float rpm, float load) const = 0;
+
+private:
+	Timer m_timeSinceGoodLambda;
+
+};
+
+class LambdaMonitor : public LambdaMonitorBase {
+	float getMaxAllowedLambda(float rpm, float load) const override;
+};

firmware/controllers/math/math.mk

@@ -4,5 +4,6 @@ CONTROLLERS_MATH_SRC_CPP = $(PROJECT_DIR)/controllers/math/engine_math.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 \
+	$(PROJECT_DIR)/controllers/math/lambda_monitor.cpp \
 	$(PROJECT_DIR)/controllers/math/throttle_model.cpp \