Closed loop controller base & ETB testing (#1322)

* closed loop controller base

* etb tps test

* closed loop controller tests

* test support

* update gtest

* fix for change

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

firmware/controllers/actuators/electronic_throttle.cpp

@@ -136,92 +136,66 @@ int EtbController::getPeriodMs() {
 	return GET_PERIOD_LIMITED(&engineConfiguration->etb);
 }
 
-void EtbController::PeriodicTask() {
-#if EFI_TUNER_STUDIO
-	// Only debug throttle #0
-	if (m_myIndex == 0) {
-		// set debug_mode 17
-		if (engineConfiguration->debugMode == DBG_ELECTRONIC_THROTTLE_PID) {
-			m_pid.postState(&tsOutputChannels);
-			tsOutputChannels.debugIntField5 = engine->engineState.etbFeedForward;
-		} else if (engineConfiguration->debugMode == DBG_ELECTRONIC_THROTTLE_EXTRA) {
-			// set debug_mode 29
-			tsOutputChannels.debugFloatField1 = directPwmValue;
-		}
-	}
-#endif /* EFI_TUNER_STUDIO */
-
-	if (!m_motor) {
-		return;
-	}
+expected<percent_t> EtbController::observePlant() const {
+	return Sensor::get(indexToTpsSensor(m_myIndex));
+}
 
+expected<percent_t> EtbController::getSetpoint() const {
+	// A few extra preconditions if throttle control is invalid
 	if (startupPositionError) {
-		m_motor->disable();
-		return;
-	}
-
-	if (m_shouldResetPid) {
-		m_pid.reset();
-		m_shouldResetPid = false;
-	}
-
-	if (!cisnan(directPwmValue)) {
-		m_motor->set(directPwmValue);
-		return;
+		return unexpected;
 	}
 
 	if (engineConfiguration->pauseEtbControl) {
-		m_motor->disable();
-		return;
+		return unexpected;
 	}
 
 	auto pedalPosition = Sensor::get(SensorType::AcceleratorPedal);
-
 	if (!pedalPosition.Valid) {
-		m_motor->disable();
-		return;
-	}
-
-	SensorResult actualThrottlePosition = Sensor::get(indexToTpsSensor(m_myIndex));
-
-	if (!actualThrottlePosition.Valid) {
-		m_motor->disable();
-		return;
+		return unexpected;
 	}
 
 	float rpm = GET_RPM();
 	engine->engineState.targetFromTable = pedal2tpsMap.getValue(rpm / RPM_1_BYTE_PACKING_MULT, pedalPosition.Value);
 	percent_t etbIdleAddition = CONFIG(useETBforIdleControl) ? engine->engineState.idle.etbIdleAddition : 0;
-	percent_t targetPosition = engine->engineState.targetFromTable + etbIdleAddition;
 
-	if (engineConfiguration->debugMode == DBG_ETB_LOGIC) {
+	float target = engine->engineState.targetFromTable + etbIdleAddition;
+
 #if EFI_TUNER_STUDIO
-		tsOutputChannels.debugFloatField1 = engine->engineState.targetFromTable;
-		tsOutputChannels.debugFloatField2 = engine->engineState.idle.etbIdleAddition;
+	if (m_myIndex == 0) {
+		tsOutputChannels.etbTarget = target;
+	}
+#endif
+
+	return target;
+}
+
+expected<percent_t> EtbController::getOpenLoop(percent_t target) const {
+	float ff = interpolate2d("etbb", target, engineConfiguration->etbBiasBins, engineConfiguration->etbBiasValues);
+	engine->engineState.etbFeedForward = ff;
+	return ff;
+}
+
+expected<percent_t> EtbController::getClosedLoop(percent_t target, percent_t actualThrottlePosition) {
+	if (m_shouldResetPid) {
+		m_pid.reset();
+		m_shouldResetPid = false;
+	}
+
+	// Only report the 0th throttle
+	if (m_myIndex == 0) {
+#if EFI_TUNER_STUDIO
+		// Error is positive if the throttle needs to open further
+		tsOutputChannels.etb1Error = target - actualThrottlePosition;
 #endif /* EFI_TUNER_STUDIO */
 	}
 
-	if (cisnan(targetPosition)) {
-		// this could happen while changing settings
-		warning(CUSTOM_ERR_ETB_TARGET, "target");
-		return;
-	}
-	engine->engineState.etbFeedForward = interpolate2d("etbb", targetPosition, engineConfiguration->etbBiasBins, engineConfiguration->etbBiasValues);
-
-	m_pid.iTermMin = engineConfiguration->etb_iTermMin;
-	m_pid.iTermMax = engineConfiguration->etb_iTermMax;
-
-	float closedLoop;
-
 	// Only allow autotune with stopped engine
-	if (rpm == 0 && engine->etbAutoTune) {
-		bool isPositive = actualThrottlePosition.Value > targetPosition;
+	if (GET_RPM() == 0 && engine->etbAutoTune) {
+		bool isPositive = actualThrottlePosition > target;
 
 		float autotuneAmplitude = 20;
 
-		// Bang-bang control the output to induce oscillation
-		closedLoop = autotuneAmplitude * (isPositive ? -1 : 1);
-
 		// End of cycle - record & reset
 		if (!isPositive && m_lastIsPositive) {
 			efitick_t now = getTimeNowNt();
@@ -277,27 +251,76 @@ void EtbController::PeriodicTask() {
 		m_lastIsPositive = isPositive;
 
 		// Find the min/max of each cycle
-		if (actualThrottlePosition.Value < m_minCycleTps) {
-			m_minCycleTps = actualThrottlePosition.Value;
+		if (actualThrottlePosition < m_minCycleTps) {
+			m_minCycleTps = actualThrottlePosition;
 		}
 
-		if (actualThrottlePosition.Value > m_maxCycleTps) {
-			m_maxCycleTps = actualThrottlePosition.Value;
+		if (actualThrottlePosition > m_maxCycleTps) {
+			m_maxCycleTps = actualThrottlePosition;
 		}
+
+		// Bang-bang control the output to induce oscillation
+		return autotuneAmplitude * (isPositive ? -1 : 1);
 	} else {
 		// Normal case - use PID to compute closed loop part
-		closedLoop = m_pid.getOutput(targetPosition, actualThrottlePosition.Value);
+		return m_pid.getOutput(target, actualThrottlePosition);
+	}
+}
+
+void EtbController::setOutput(expected<percent_t> outputValue) {
+#if EFI_TUNER_STUDIO
+	// Only report first-throttle stats
+	if (m_myIndex == 0) {
+		tsOutputChannels.etb1DutyCycle = outputValue.value_or(0);
+	}
+#endif
+
+	if (!m_motor) return;
+
+	if (outputValue) {
+		m_motor->enable();
+		m_motor->set(ETB_PERCENT_TO_DUTY(outputValue.Value));
+	} else {
+		m_motor->disable();
+	}
+}
+
+void EtbController::PeriodicTask() {
+#if EFI_TUNER_STUDIO
+	// Only debug throttle #0
+	if (m_myIndex == 0) {
+		// set debug_mode 17
+		if (engineConfiguration->debugMode == DBG_ELECTRONIC_THROTTLE_PID) {
+			m_pid.postState(&tsOutputChannels);
+			tsOutputChannels.debugIntField5 = engine->engineState.etbFeedForward;
+		} else if (engineConfiguration->debugMode == DBG_ELECTRONIC_THROTTLE_EXTRA) {
+			// set debug_mode 29
+			tsOutputChannels.debugFloatField1 = directPwmValue;
+		}
+	}
+#endif /* EFI_TUNER_STUDIO */
+
+	if (!cisnan(directPwmValue)) {
+		m_motor->set(directPwmValue);
+		return;
 	}
 
-	currentEtbDuty = engine->engineState.etbFeedForward + closedLoop;
+	if (engineConfiguration->debugMode == DBG_ETB_LOGIC) {
+#if EFI_TUNER_STUDIO
+		tsOutputChannels.debugFloatField1 = engine->engineState.targetFromTable;
+		tsOutputChannels.debugFloatField2 = engine->engineState.idle.etbIdleAddition;
+#endif /* EFI_TUNER_STUDIO */
+	}
 
-	m_motor->enable();
-	m_motor->set(ETB_PERCENT_TO_DUTY(currentEtbDuty));
+	m_pid.iTermMin = engineConfiguration->etb_iTermMin;
+	m_pid.iTermMax = engineConfiguration->etb_iTermMax;
 
 	if (engineConfiguration->isVerboseETB) {
 		m_pid.showPidStatus(&logger, "ETB");
 	}
 
+	update();
+
 	DISPLAY_STATE(Engine)
 	DISPLAY_TEXT(Electronic_Throttle);
 	DISPLAY_SENSOR(TPS)
@@ -337,19 +360,6 @@ void EtbController::PeriodicTask() {
 /* DISPLAY_ELSE */
 	DISPLAY_TEXT(No_Pedal_Sensor);
 /* DISPLAY_ENDIF */
-
-	// Only report the 0th throttle
-	if (m_myIndex == 0) {
-#if EFI_TUNER_STUDIO
-		// 312
-		tsOutputChannels.etbTarget = targetPosition;
-		// 316
-		tsOutputChannels.etb1DutyCycle = currentEtbDuty;
-		// 320
-		// Error is positive if the throttle needs to open further
-		tsOutputChannels.etb1Error = targetPosition - actualThrottlePosition.Value;
-#endif /* EFI_TUNER_STUDIO */
-	}
 }
 
 // real implementation (we mock for some unit tests)

firmware/controllers/actuators/electronic_throttle.h

@@ -12,6 +12,8 @@
 #define DEFAULT_ETB_PWM_FREQUENCY 300
 
 #include "engine.h"
+#include "closed_loop_controller.h"
+#include "expected.h"
 #include "periodic_task.h"
 
 class DcMotor;
@@ -24,7 +26,7 @@ public:
 	virtual void reset() = 0;
 };
 
-class EtbController final : public IEtbController {
+class EtbController final : public IEtbController, protected ClosedLoopController<percent_t, percent_t> {
 public:
 	void init(DcMotor *motor, int ownIndex, pid_s *pidParameters) override;
 
@@ -40,6 +42,15 @@ public:
 	// Print this throttle's status.
 	void showStatus(Logging* logger);
 
+	// Helpers for individual parts of throttle control
+	expected<percent_t> observePlant() const override;
+	expected<percent_t> getSetpoint() const override;
+
+	expected<percent_t> getOpenLoop(percent_t target) const override;
+	expected<percent_t> getClosedLoop(percent_t setpoint, percent_t target) override;
+
+	void setOutput(expected<percent_t> outputValue) override;
+
 	// Used to inspect the internal PID controller's state
 	const pid_state_s* getPidState() const { return &m_pid; };
 

firmware/controllers/closed_loop_controller.h

@@ -0,0 +1,56 @@
+#pragma once
+
+#include "expected.h"
+
+template <typename TInput, typename TOutput>
+class ClosedLoopController {
+public:
+	void update() {
+		expected<TOutput> outputValue = getOutput();
+		setOutput(outputValue);
+	}
+
+private:
+	expected<TOutput> getOutput() {
+		expected<TInput> setpoint = getSetpoint();
+		// If we don't know the setpoint, return failure.
+		if (!setpoint) {
+			return unexpected;
+		}
+
+		expected<TInput> observation = observePlant();
+		// If we couldn't observe the plant, return failure.
+		if (!observation) {
+			return unexpected;
+		}
+
+		expected<TOutput> openLoopResult = getOpenLoop(setpoint.Value);
+		// If we couldn't compute open loop, return failure.
+		if (!openLoopResult) {
+			return unexpected;
+		}
+
+		expected<TOutput> closedLoopResult = getClosedLoop(setpoint.Value, observation.Value);
+		// If we couldn't compute closed loop, return failure.
+		if (!closedLoopResult) {
+			return unexpected;
+		}
+
+		return openLoopResult.Value + closedLoopResult.Value;
+	}
+
+	// Get the setpoint: where should the controller put the plant?
+	virtual expected<TInput> getSetpoint() const = 0;
+
+	// Get the current observation: what is the current state of the world?
+	virtual expected<TInput> observePlant() const = 0;
+
+	// Get the open-loop output: output state based on only the setpoint
+	virtual expected<TOutput> getOpenLoop(TInput setpoint) const = 0;
+
+	// Get the closed-loop output: output state based on setpoint and observation
+	virtual expected<TOutput> getClosedLoop(TInput setpoint, TInput observation) = 0;
+
+	// Set the output: Drive whatever output system will perturb the plant in the real world
+	virtual void setOutput(expected<TOutput> outputValue) = 0;
+};

firmware/util/expected.h

@@ -41,6 +41,21 @@ struct expected {
 	constexpr float value_or(TValue valueIfInvalid) const {
 		return Valid ? Value : valueIfInvalid;
 	}
+
+	bool operator ==(const expected<TValue>& other) const {
+		// If validity mismatch, not equal
+		if (Valid != other.Valid) {
+			return false;
+		}
+		
+		// If both are invalid, they are equal
+		if (!Valid && !other.Valid) {
+			return true;
+		}
+
+		// Both are guaranteed valid - simply compare values
+		return Value == other.Value;
+	}
 };
 
 constexpr unexpected_t unexpected{};

unit_tests/googletest

@@ -1 +1 @@
-Subproject commit dea0216d0c6bc5e63cf5f6c8651cd268668032ec
+Subproject commit dcc92d0ab6c4ce022162a23566d44f673251eee4

unit_tests/tests/test_closed_loop_controller.cpp

@@ -0,0 +1,124 @@
+#include "closed_loop_controller.h"
+
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+using ::testing::StrictMock;
+using ::testing::Return;
+using ::testing::Eq;
+
+class TestController : public ClosedLoopController<float, float> {
+public:
+	MOCK_METHOD(expected<float>, getSetpoint, (), (const, override));
+	MOCK_METHOD(expected<float>, observePlant, (), (const, override));
+	MOCK_METHOD(expected<float>, getOpenLoop, (float setpoint), (const, override));
+	MOCK_METHOD(expected<float>, getClosedLoop, (float setpoint, float observation), (override));
+	MOCK_METHOD(void, setOutput, (expected<float> outputValue), (override));
+};
+
+TEST(ClosedLoopController, TestSetpoint) {
+	StrictMock<TestController> controller;
+
+	// If getSetpoint returns unexpected, no other functions should be called
+	EXPECT_CALL(controller, getSetpoint())
+		.WillOnce(Return(unexpected));
+
+	// And output should be called with unexpected
+	EXPECT_CALL(controller, setOutput(Eq(unexpected)));
+
+	controller.update();
+}
+
+TEST(ClosedLoopController, TestSetpointSuccessPlantFail) {
+	StrictMock<TestController> controller;
+
+	// If getSetpoint returns unexpected, no other functions should be called
+	EXPECT_CALL(controller, getSetpoint())
+		.WillOnce(Return(25.0f));
+
+	// Fail to observe plant - exec should stop there
+	EXPECT_CALL(controller, observePlant())
+		.WillOnce(Return(unexpected));
+
+	// And output should be called with unexpected
+	EXPECT_CALL(controller, setOutput(Eq(unexpected)));
+
+	controller.update();
+}
+
+TEST(ClosedLoopController, TestPlantSuccessOpenLoopFail) {
+	StrictMock<TestController> controller;
+
+	// Return valid setpoint
+	EXPECT_CALL(controller, getSetpoint())
+		.WillOnce(Return(25.0f));
+
+	// Return valid observation
+	EXPECT_CALL(controller, observePlant())
+		.WillOnce(Return(75.0f));
+
+	// Setpoint should get passed to open loop
+	// It will fail, nothing else should be called
+	EXPECT_CALL(controller, getOpenLoop(25.0f))
+		.WillOnce(Return(unexpected));
+
+	// And output should be called with unexpected
+	EXPECT_CALL(controller, setOutput(Eq(unexpected)));
+
+	controller.update();
+}
+
+
+TEST(ClosedLoopController, TestPlantOpenLoopSuccessClosedLoopFail) {
+	StrictMock<TestController> controller;
+
+	// Return valid setpoint
+	EXPECT_CALL(controller, getSetpoint())
+		.WillOnce(Return(25.0f));
+
+	// Return valid observation
+	EXPECT_CALL(controller, observePlant())
+		.WillOnce(Return(75.0f));
+
+	// Setpoint should get passed to open loop
+	// Return something valid
+	EXPECT_CALL(controller, getOpenLoop(25.0f))
+		.WillOnce(Return(37.0f));
+
+	// Setpoint & observation should get passed
+	// But return invalid
+	EXPECT_CALL(controller, getClosedLoop(25.0f, 75.0f))
+		.WillOnce(Return(unexpected));
+
+	// And output should be called with unexpected
+	EXPECT_CALL(controller, setOutput(Eq(unexpected)));
+
+	controller.update();
+}
+
+TEST(ClosedLoopController, TestAllSuccess) {
+	StrictMock<TestController> controller;
+
+	// Return valid setpoint
+	EXPECT_CALL(controller, getSetpoint())
+		.WillOnce(Return(25.0f));
+
+	// Return valid observation
+	EXPECT_CALL(controller, observePlant())
+		.WillOnce(Return(75.0f));
+
+	// Setpoint should get passed to open loop
+	// Return something valid
+	EXPECT_CALL(controller, getOpenLoop(25.0f))
+		.WillOnce(Return(37.0f));
+
+	// Setpoint & observation should get passed
+	// Return something valid
+	EXPECT_CALL(controller, getClosedLoop(25.0f, 75.0f))
+		.WillOnce(Return(22.0f));
+
+	// Output should get called with sum of open & closed loop
+	EXPECT_CALL(controller, setOutput(expected<float>(59.0f)));
+
+	controller.update();
+}

unit_tests/tests/test_etb.cpp

@@ -90,3 +90,23 @@ TEST(etb, testTargetTpsIsFloatBug945) {
 	engine->etbControllers[0]->PeriodicTask();
 	ASSERT_NEAR(51.6605, engine->engineState.targetFromTable, EPS4D);
 }
+
+TEST(etb, etbTpsSensor) {
+	// Throw some distinct values on the TPS sensors so we can identify that we're getting the correct one
+	Sensor::setMockValue(SensorType::Tps1, 25.0f);
+	Sensor::setMockValue(SensorType::Tps2, 75.0f);
+
+	// Test first throttle
+	{
+		EtbController etb;
+		etb.init(nullptr, 0, nullptr);
+		EXPECT_EQ(etb.observePlant().Value, 25.0f);
+	}
+
+	// Test second throttle
+	{
+		EtbController etb;
+		etb.init(nullptr, 1, nullptr);
+		EXPECT_EQ(etb.observePlant().Value, 75.0f);
+	}
+}

unit_tests/tests/tests.mk

@@ -48,4 +48,5 @@ TESTS_SRC_CPP = \
 	tests/sensor/func_chain.cpp \
 	tests/sensor/redundant.cpp \
 	tests/sensor/test_sensor_init.cpp \
+	tests/test_closed_loop_controller.cpp \