Jammed ETB detection (#4873)

* jammed ETB detection

* autoscale

* comment

* cleanup

* implement test, cleanup

firmware/controllers/actuators/electronic_throttle.cpp

@@ -269,7 +269,7 @@ expected<percent_t> EtbController::getSetpoint() {
 expected<percent_t> EtbController::getSetpointIdleValve() const {
 	// VW ETB idle mode uses an ETB only for idle (a mini-ETB sets the lower stop, and a normal cable
 	// can pull the throttle up off the stop.), so we directly control the throttle with the idle position.
-#if EFI_TUNER_STUDIO
+#if EFI_TUNER_STUDIO && (EFI_PROD_CODE || EFI_SIMULATOR)
 	engine->outputChannels.etbTarget = m_idlePosition;
 #endif // EFI_TUNER_STUDIO
 	return clampF(0, m_idlePosition, 100);
@@ -637,20 +637,41 @@ void EtbController::update() {
 	m_pid.iTermMin = engineConfiguration->etb_iTermMin;
 	m_pid.iTermMax = engineConfiguration->etb_iTermMax;
 
-	ClosedLoopController::update();
+	auto output = ClosedLoopController::update();
+
+	if (!output) {
+		return;
+	}
+
+	checkOutput(output.Value);
 }
 
-expected<percent_t> EtbController::getOutput() {
-	// total open + closed loop parts
-	expected<percent_t> output = ClosedLoopController::getOutput();
-	if (!output) {
-		return output;
-	}
-    etbDutyAverage = m_dutyAverage.average(absF(output.Value));
+void EtbController::checkOutput(percent_t output) {
+	etbDutyAverage = m_dutyAverage.average(absF(output));
 
-    etbDutyRateOfChange = m_dutyRocAverage.average(absF(output.Value - prevOutput));
-	prevOutput = output.Value;
-	return output;
+	etbDutyRateOfChange = m_dutyRocAverage.average(absF(output - prevOutput));
+	prevOutput = output;
+
+	float integrator = absF(m_pid.getIntegration());
+	auto integratorLimit = engineConfiguration->etbJamIntegratorLimit;
+
+	if (integratorLimit != 0) {
+		auto nowNt = getTimeNowNt();
+
+		if (integrator > integratorLimit) {
+			if (m_jamDetectTimer.hasElapsedSec(engineConfiguration->etbJamTimeout)) {
+				// ETB is jammed!
+				jamDetected = true;
+
+				// TODO: do something about it!
+			}
+		} else {
+			m_jamDetectTimer.reset(getTimeNowNt());
+			jamDetected = false;
+		}
+
+		jamTimer = m_jamDetectTimer.getElapsedSeconds(nowNt);
+	}
 }
 
 void EtbController::autoCalibrateTps() {

firmware/controllers/actuators/electronic_throttle.h

@@ -53,10 +53,9 @@ public:
 	virtual void setIdlePosition(percent_t pos) = 0;
 	virtual void setWastegatePosition(percent_t pos) = 0;
 	virtual void update() = 0;
-	virtual expected<percent_t> getOutput() = 0;
 	virtual void autoCalibrateTps() = 0;
 
-	virtual const pid_state_s* getPidState() const = 0;
+	virtual const pid_state_s& getPidState() const = 0;
 
 	virtual void setLuaAdjustment(percent_t adjustment) = 0;
 };

firmware/controllers/actuators/electronic_throttle.txt

@@ -12,6 +12,8 @@ float luaAdjustment;"ETB: luaAdjustment"
     float etbCurrentAdjustedTarget;;"%", 1, 0, -10000, 10000, 3
 
 	bit etbRevLimitActive
+	bit jamDetected
+
 	float etbDutyRateOfChange
 	float etbDutyAverage
 	uint16_t etbTpsErrorCounter;"ETB TPS error counter"
@@ -19,4 +21,6 @@ float luaAdjustment;"ETB: luaAdjustment"
 
 	int8_t etbErrorCode
 
+	uint16_t autoscale jamTimer;ETB jam timer;"sec", 0.01, 0, 0, 100, 2
+
 end_struct

firmware/controllers/actuators/electronic_throttle_impl.h

@@ -34,7 +34,6 @@ public:
 
 	// Update the controller's state: read sensors, send output, etc
 	void update() override;
-	expected<percent_t> getOutput() override;
 
 	// Called when the configuration may have changed.  Controller will
 	// reset if necessary.
@@ -57,8 +56,10 @@ public:
 
 	void setOutput(expected<percent_t> outputValue) override;
 
+	void checkOutput(percent_t output);
+
 	// Used to inspect the internal PID controller's state
-	const pid_state_s* getPidState() const override { return &m_pid; };
+	const pid_state_s& getPidState() const override { return m_pid; };
 
 	// Use the throttle to automatically calibrate the relevant throttle position sensor(s).
 	void autoCalibrateTps() override;
@@ -96,6 +97,8 @@ private:
 	ExpAverage m_dutyRocAverage;
 	ExpAverage m_dutyAverage;
 
+	Timer m_jamDetectTimer;
+
 	// Pedal -> target map
 	const ValueProvider3D* m_pedalMap = nullptr;
 

firmware/controllers/closed_loop_controller.h

@@ -9,12 +9,15 @@
 template <typename TInput, typename TOutput>
 class ClosedLoopController {
 public:
-	void update() {
+	expected<TOutput> update() {
 		expected<TOutput> outputValue = getOutput();
 		setOutput(outputValue);
+
+		return outputValue;
 	}
 
-	virtual expected<TOutput> getOutput() {
+private:
+	expected<TOutput> getOutput() {
 		expected<TInput> setpoint = getSetpoint();
 		// If we don't know the setpoint, return failure.
 		if (!setpoint) {
@@ -41,7 +44,6 @@ public:
 
 		return openLoopResult.Value + closedLoopResult.Value;
 	}
-private:
 
 	// Get the setpoint: where should the controller put the plant?
 	virtual expected<TInput> getSetpoint() = 0;

unit_tests/mocks.h

@@ -25,12 +25,11 @@ public:
 	MOCK_METHOD(void, setIdlePosition, (percent_t pos), (override));
 	MOCK_METHOD(void, setWastegatePosition, (percent_t pos), (override));
 	MOCK_METHOD(void, autoCalibrateTps, (), (override));
-	MOCK_METHOD(const pid_state_s*, getPidState, (), (const, override));
+	MOCK_METHOD(const pid_state_s&, getPidState, (), (const, override));
 	MOCK_METHOD(void, setLuaAdjustment, (percent_t adjustment), (override));
 
 
 	// ClosedLoopController mocks
-	MOCK_METHOD(expected<percent_t>, getOutput, (), (override));
 	MOCK_METHOD(expected<percent_t>, getSetpoint, (), (override));
 	MOCK_METHOD(expected<percent_t>, observePlant, (), (const, override));
 	MOCK_METHOD(expected<percent_t>, getOpenLoop, (percent_t setpoint), (override));

unit_tests/tests/actuators/test_etb.cpp

@@ -198,7 +198,6 @@ TEST(etb, initializationNoPrimarySensor) {
 	Sensor::resetAllMocks();
 
 	EtbController dut;
-	EngineTestHelper eth(TEST_ENGINE);
 
 	// Needs pedal for init
 	Sensor::setMockValue(SensorType::AcceleratorPedal, 0.0f, true);
@@ -470,7 +469,6 @@ TEST(etb, setpointNoPedalMap) {
 }
 
 TEST(etb, setpointIdleValveController) {
-	EngineTestHelper eth(TEST_ENGINE);
 	EtbController etb;
 
 	etb.init(ETB_IdleValve, nullptr, nullptr, nullptr, false);
@@ -490,7 +488,6 @@ TEST(etb, setpointIdleValveController) {
 }
 
 TEST(etb, setpointWastegateController) {
-	EngineTestHelper eth(TEST_ENGINE);
 	EtbController etb;
 
 	etb.init(ETB_Wastegate, nullptr, nullptr, nullptr, false);
@@ -561,10 +558,6 @@ TEST(etb, setpointLuaAdder) {
 }
 
 TEST(etb, etbTpsSensor) {
-    static engine_configuration_s localConfig;
-// huh? how is this breaking the test?   	EngineTestHelper eth(TEST_ENGINE);
-    engineConfiguration = &localConfig;
-
 	// 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, true);
 	Sensor::setMockValue(SensorType::Tps2, 75.0f, true);
@@ -601,7 +594,6 @@ TEST(etb, etbTpsSensor) {
 		etb.init(ETB_IdleValve, nullptr, nullptr, nullptr, true);
 		EXPECT_EQ(etb.observePlant().Value, 66.0f);
 	}
-	engineConfiguration = nullptr;
 }
 
 TEST(etb, setOutputInvalid) {
@@ -746,9 +738,6 @@ TEST(etb, setOutputLimpHome) {
 }
 
 TEST(etb, closedLoopPid) {
-    static engine_configuration_s localConfig;
-// huh? how is this breaking the test?   	EngineTestHelper eth(TEST_ENGINE);
-    engineConfiguration = &localConfig;
 	pid_s pid = {};
 	pid.pFactor = 5;
 	pid.maxValue = 75;
@@ -762,8 +751,6 @@ TEST(etb, closedLoopPid) {
 	EtbController etb;
 	etb.init(ETB_Throttle1, nullptr, &pid, nullptr, true);
 
-    // todo: second part dirty hack :(
-	engineConfiguration = nullptr;
 	// Disable autotune for now
 	Engine e;
 	EngineTestHelperBase base(&e, nullptr, nullptr);
@@ -779,6 +766,59 @@ TEST(etb, closedLoopPid) {
 	EXPECT_FLOAT_EQ(etb.getClosedLoop(50, 30).value_or(-1), 75);
 }
 
+extern int timeNowUs;
+
+TEST(etb, jamDetection) {
+	EngineTestHelper eth(TEST_ENGINE);
+
+	pid_s pid = {};
+
+	// I-only since we're testing out the integrator
+	pid.pFactor = 0;
+	pid.iFactor = 1;
+	pid.dFactor = 0;
+	pid.maxValue = 50;
+	pid.minValue = -50;
+
+	// Must have TPS & PPS initialized for ETB setup
+	Sensor::setMockValue(SensorType::Tps1Primary, 0);
+	Sensor::setMockValue(SensorType::Tps1, 0.0f, true);
+	Sensor::setMockValue(SensorType::AcceleratorPedal, 0.0f, true);
+
+	// Limit of 5%, 1 second
+	engineConfiguration->etbJamIntegratorLimit = 5;
+	engineConfiguration->etbJamTimeout = 1;
+
+	EtbController etb;
+	etb.init(ETB_Throttle1, nullptr, &pid, nullptr, true);
+
+	timeNowUs = 0;
+
+	// Reset timer while under integrator limit
+	EXPECT_EQ(etb.getPidState().iTerm, 0);
+	etb.checkOutput(0);
+	EXPECT_EQ(etb.jamTimer, 0);
+	EXPECT_FALSE(etb.jamDetected);
+
+	for (size_t i = 0; i < ETB_LOOP_FREQUENCY; i++) {
+		// Error of 10, should accumulate 10 integrator per second
+		etb.getClosedLoop(50, 40);
+	}
+
+	EXPECT_NEAR(etb.getPidState().iTerm, 10.0f, 1e-3);
+
+	// Just under time limit, no jam yet
+	timeNowUs = 0.9e6;
+	etb.checkOutput(0);
+	EXPECT_NEAR(etb.jamTimer, 0.9f, 1e-3);
+	EXPECT_FALSE(etb.jamDetected);
+
+	// Above the time limit, jam detected!
+	timeNowUs = 1.1e6;
+	etb.checkOutput(0);
+	EXPECT_TRUE(etb.jamDetected);
+}
+
 TEST(etb, openLoopThrottle) {
 	EngineTestHelper eth(TEST_ENGINE);