move lambda in to sensor model (#1736)

* lambda sens

* fix init

* fix

* a test for good measure

* he's climbin in your windows, snatchin you ram up

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

firmware/console/status_loop.cpp

@@ -525,10 +525,9 @@ void updateTunerStudioState(TunerStudioOutputChannels *tsOutputChannels DECLARE_
 	// offset 16
 	tsOutputChannels->massAirFlowVoltage = hasMafSensor() ? getMafVoltage(PASS_ENGINE_PARAMETER_SIGNATURE) : 0;
 
-	if (hasAfrSensor(PASS_ENGINE_PARAMETER_SIGNATURE)) {
-		// offset 20
-		tsOutputChannels->airFuelRatio = getAfr(PASS_ENGINE_PARAMETER_SIGNATURE);
-	}
+	// offset 20
+	tsOutputChannels->airFuelRatio = Sensor::get(SensorType::Lambda).value_or(0) * 14.7f;
+
 	// offset 24
 	tsOutputChannels->engineLoad = getEngineLoadT(PASS_ENGINE_PARAMETER_SIGNATURE);
 

firmware/controllers/algo/engine2.cpp

@@ -138,9 +138,6 @@ void EngineState::periodicFastCallback(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	int rpm = ENGINE(rpmCalculator).getRpm(PASS_ENGINE_PARAMETER_SIGNATURE);
 	sparkDwell = getSparkDwell(rpm PASS_ENGINE_PARAMETER_SUFFIX);
 	dwellAngle = cisnan(rpm) ? NAN :  sparkDwell / getOneDegreeTimeMs(rpm);
-	if (hasAfrSensor(PASS_ENGINE_PARAMETER_SIGNATURE)) {
-		engine->sensors.currentAfr = getAfr(PASS_ENGINE_PARAMETER_SIGNATURE);
-	}
 
 	// todo: move this into slow callback, no reason for IAT corr to be here
 	running.intakeTemperatureCoefficient = getIatFuelCorrection(PASS_ENGINE_PARAMETER_SIGNATURE);

firmware/controllers/algo/engine_parts.h

@@ -37,7 +37,6 @@ public:
 	Accelerometer accelerometer;
 
 	float vBatt = 0;
-	float currentAfr = 0;
 	/**
 	 * that's fuel in tank - just a gauge
 	 */

firmware/controllers/can/can_verbose.cpp

@@ -118,7 +118,7 @@ struct Sensors2 {
 };
 
 static void populateFrame(Sensors2& msg) {
-    msg.afr = getAfr();
+    msg.afr = Sensor::get(SensorType::Lambda).value_or(0) * 14.7f;
     msg.oilPressure = Sensor::get(SensorType::OilPressure).value_or(-1);
     msg.vvtPos = engine->triggerCentral.getVVTPosition();
     msg.vbatt = getVBatt();

firmware/controllers/can/obd2.cpp

@@ -35,7 +35,6 @@
 #include "engine_math.h"
 #include "fuel_math.h"
 #include "thermistors.h"
-#include "ego.h"
 
 EXTERN_ENGINE;
 
@@ -158,9 +157,9 @@ static void handleGetDataRequest(const CANRxFrame& rx) {
 		obdSendValue(_1_MODE, pid, 1, Sensor::get(SensorType::Tps1).value_or(0) * 2.55f);	// (A*100/255)
 		break;
 	case PID_FUEL_AIR_RATIO_1: {
-		float afr = getAfr(PASS_ENGINE_PARAMETER_SIGNATURE);
+		float lambda = Sensor::get(SensorType::Lambda).value_or(0);
 		// phi = 1 / lambda
-		float phi = clampF(0, 14.7f / afr, 1.99f);
+		float phi = clampF(0, 1 / lambda, 1.99f);
 
 		uint16_t scaled = phi * 32768;
 

firmware/controllers/engine_controller.cpp

@@ -701,7 +701,7 @@ void initEngineContoller(Logging *sharedLogger DECLARE_ENGINE_PARAMETER_SUFFIX)
  * UNUSED_SIZE constants.
  */
 #ifndef RAM_UNUSED_SIZE
-#define RAM_UNUSED_SIZE 6600
+#define RAM_UNUSED_SIZE 6500
 #endif
 #ifndef CCM_UNUSED_SIZE
 #define CCM_UNUSED_SIZE 2900

firmware/controllers/gauges/lcd_controller.cpp

@@ -220,8 +220,8 @@ static void showLine(lcd_line_e line, int screenY) {
 		return;
 #endif
 	case LL_AFR:
-		if (hasAfrSensor(PASS_ENGINE_PARAMETER_SIGNATURE)) {
-			lcdPrintf("AFR: %.2f", getAfr());
+		if (Sensor::hasSensor(SensorType::Lambda)) {
+			lcdPrintf("AFR: %.2f", Sensor::get(SensorType::Lambda).value_or(0));
 		} else {
 			lcdPrintf("AFR: none");
 		}

firmware/controllers/math/closed_loop_fuel.cpp

@@ -71,8 +71,8 @@ static bool shouldUpdateCorrection(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 
 	// 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)) {
+	auto afr = Sensor::get(SensorType::Lambda);
+	if (!afr || afr.value_or(0) < (cfg.minAfr * 0.1f) || afr.value_or(0) > (cfg.maxAfr * 0.1f)) {
 		return false;
 	}
 

firmware/controllers/math/closed_loop_fuel_cell.cpp

@@ -1,6 +1,7 @@
 #include "closed_loop_fuel_cell.h"
 #include "engine.h"
 #include "engine_configuration_generated_structures.h"
+#include "sensor.h"
 
 EXTERN_ENGINE;
 
@@ -48,7 +49,14 @@ float ClosedLoopFuelCellBase::getAdjustment() const {
 }
 
 float ClosedLoopFuelCellImpl::getLambdaError(DECLARE_ENGINE_PARAMETER_SIGNATURE) const {
-	return (ENGINE(sensors.currentAfr) - ENGINE(engineState.targetAFR)) / 14.7f;
+	auto lambda = Sensor::get(SensorType::Lambda);
+
+	// Failed sensor -> no error
+	if (!lambda) {
+		return 0;
+	}
+
+	return lambda.Value - (ENGINE(engineState.targetAFR) / 14.7f);
 }
 
 #define MAX_ADJ (0.25f)

firmware/controllers/sensors/sensor.cpp

@@ -74,6 +74,8 @@ static const char* s_sensorNames[] = {
 
 	"Aux Temp 1",
 	"Aux Temp 2",
+
+	"Lambda",
 };
 
 static_assert(efi::size(s_sensorNames) == efi::size(s_sensorRegistry));

firmware/controllers/sensors/sensor_type.h

@@ -48,6 +48,8 @@ enum class SensorType : unsigned char {
 	AuxTemp1,
 	AuxTemp2,
 
+	Lambda,
+
 	// Leave me at the end!
 	PlaceholderLast
 };

firmware/init/init.h

@@ -23,6 +23,7 @@ void initTps(DECLARE_CONFIG_PARAMETER_SIGNATURE);
 void initOilPressure(DECLARE_CONFIG_PARAMETER_SIGNATURE);
 void initThermistors(DECLARE_CONFIG_PARAMETER_SIGNATURE);
 void initCanSensors();
+void initLambda(DECLARE_ENGINE_PARAMETER_SIGNATURE);
 
 // Sensor reconfiguration
 void reconfigureTps(DECLARE_CONFIG_PARAMETER_SIGNATURE);

firmware/init/init.mk

@@ -4,3 +4,4 @@ INIT_SRC_CPP =	$(PROJECT_DIR)/init/sensor/init_sensors.cpp \
 				$(PROJECT_DIR)/init/sensor/init_tps.cpp \
 				$(PROJECT_DIR)/init/sensor/init_can_sensors.cpp \
 				$(PROJECT_DIR)/init/sensor/init_thermistors.cpp \
+				$(PROJECT_DIR)/init/sensor/init_lambda.cpp \

firmware/init/sensor/init_lambda.cpp

@@ -0,0 +1,32 @@
+#include "init.h"
+#include "adc_subscription.h"
+#include "engine.h"
+#include "error_handling.h"
+#include "global.h"
+#include "function_pointer_sensor.h"
+#include "ego.h"
+
+EXTERN_ENGINE;
+
+struct GetAfrWrapper {
+	DECLARE_ENGINE_PTR;
+
+	float getLambda() {
+		return getAfr(PASS_ENGINE_PARAMETER_SIGNATURE) / 14.7f;
+	}
+};
+
+static GetAfrWrapper afrWrapper;
+
+static FunctionPointerSensor lambdaSensor(SensorType::Lambda,
+[]() {
+	return afrWrapper.getLambda();
+});
+
+void initLambda(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
+	INJECT_ENGINE_REFERENCE(&afrWrapper);
+
+	if (!lambdaSensor.Register()) {
+		warning(OBD_PCM_Processor_Fault, "Duplicate lambda sensor registration, ignoring");
+	}
+}

firmware/init/sensor/init_sensors.cpp

@@ -16,6 +16,7 @@ void initNewSensors(Logging* logger DECLARE_ENGINE_PARAMETER_SUFFIX) {
 	initTps(PASS_CONFIG_PARAMETER_SIGNATURE);
 	initOilPressure(PASS_CONFIG_PARAMETER_SIGNATURE);
 	initThermistors(PASS_CONFIG_PARAMETER_SIGNATURE);
+	initLambda(PASS_ENGINE_PARAMETER_SIGNATURE);
 
 	// Init CLI functionality for sensors (mocking)
 	initSensorCli(logger);

unit_tests/tests/sensor/test_sensor_init.cpp

@@ -163,3 +163,12 @@ TEST(SensorInit, Clt) {
 	EXPECT_POINT_INVALID(s, 0.0f);
 	EXPECT_POINT_INVALID(s, 5.0f);
 }
+
+TEST(SensorInit, Lambda) {
+	WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
+
+	initLambda(PASS_ENGINE_PARAMETER_SIGNATURE);
+
+	auto s = Sensor::getSensorOfType(SensorType::Lambda);
+	ASSERT_NE(nullptr, s);
+}