Inject idle to ETB (#1335)

* inject idle position

* pointer

* etb idle tests

* extract mocks

* ops

* test negative case too

* don't convert for etb

* guard

firmware/controllers/actuators/electronic_throttle.cpp

@@ -141,6 +141,10 @@ expected<percent_t> EtbController::observePlant() const {
 	return Sensor::get(indexToTpsSensor(m_myIndex));
 }
 
+void EtbController::setIdlePosition(percent_t pos) {
+	m_idlePosition = pos;
+}
+
 expected<percent_t> EtbController::getSetpoint() const {
 	// A few extra preconditions if throttle control is invalid
 	if (startupPositionError) {
@@ -165,7 +169,13 @@ expected<percent_t> EtbController::getSetpoint() const {
 	
 	float rpm = GET_RPM();
 	engine->engineState.targetFromTable = m_pedalMap->getValue(rpm / RPM_1_BYTE_PACKING_MULT, sanitizedPedal);
-	percent_t etbIdleAddition = CONFIG(useETBforIdleControl) ? engine->engineState.idle.etbIdleAddition : 0;
+
+	percent_t etbIdlePosition = clampF(
+									0,
+									CONFIG(useETBforIdleControl) ? m_idlePosition : 0,
+									100
+								);
+	percent_t etbIdleAddition = 0.01f * CONFIG(etbIdleThrottleRange) * etbIdlePosition;
 
 	float target = engine->engineState.targetFromTable + etbIdleAddition;
 
@@ -644,4 +654,14 @@ void initElectronicThrottle(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	doInitElectronicThrottle(PASS_ENGINE_PARAMETER_SIGNATURE);
 }
 
+void setEtbIdlePosition(percent_t pos DECLARE_ENGINE_PARAMETER_SUFFIX) {
+	for (int i = 0; i < ETB_COUNT; i++) {
+		auto etb = engine->etbControllers[i];
+
+		if (etb) {
+			etb->setIdlePosition(pos);
+		}
+	}
+}
+
 #endif /* EFI_ELECTRONIC_THROTTLE_BODY */

firmware/controllers/actuators/electronic_throttle.h

@@ -24,11 +24,13 @@ public:
 	DECLARE_ENGINE_PTR;
 	virtual void init(DcMotor *motor, int ownIndex, pid_s *pidParameters, const ValueProvider3D* pedalMap) = 0;
 	virtual void reset() = 0;
+	virtual void setIdlePosition(percent_t pos) = 0;
 };
 
 class EtbController final : public IEtbController {
 public:
 	void init(DcMotor *motor, int ownIndex, pid_s *pidParameters, const ValueProvider3D* pedalMap) override;
+	void setIdlePosition(percent_t pos) override;
 
 	// PeriodicTimerController implementation
 	int getPeriodMs() override;
@@ -63,6 +65,8 @@ private:
 	// Pedal -> target map
 	const ValueProvider3D* m_pedalMap = nullptr;
 
+	float m_idlePosition = 0;
+
 	// Autotune helpers
 	bool m_lastIsPositive = false;
 	efitick_t m_cycleStartTime = 0;
@@ -74,6 +78,7 @@ private:
 
 void initElectronicThrottle(DECLARE_ENGINE_PARAMETER_SIGNATURE);
 void doInitElectronicThrottle(DECLARE_ENGINE_PARAMETER_SIGNATURE);
+void setEtbIdlePosition(percent_t pos DECLARE_ENGINE_PARAMETER_SUFFIX);
 
 void setDefaultEtbBiasCurve(DECLARE_CONFIG_PARAMETER_SIGNATURE);
 void setDefaultEtbParameters(DECLARE_CONFIG_PARAMETER_SIGNATURE);

firmware/controllers/actuators/idle_thread.cpp

@@ -40,6 +40,7 @@
 #include "periodic_task.h"
 #include "allsensors.h"
 #include "sensor.h"
+#include "electronic_throttle.h"
 
 #if ! EFI_UNIT_TEST
 #include "stepper.h"
@@ -154,14 +155,14 @@ static void applyIACposition(percent_t position) {
 	float duty = PERCENT_TO_DUTY(position);
 
 	if (CONFIG(useETBforIdleControl)) {
-
 		if (!Sensor::hasSensor(SensorType::AcceleratorPedal)) {
 			firmwareError(CUSTOM_NO_ETB_FOR_IDLE, "No ETB to use for idle");
 			return;
 		}
 
-
-		engine->engineState.idle.etbIdleAddition = duty * CONFIG(etbIdleThrottleRange);
+#if EFI_ELECTRONIC_THROTTLE_BODY
+		setEtbIdlePosition(position);
+#endif
 #if ! EFI_UNIT_TEST
 	} if (CONFIG(useStepperIdle)) {
 		iacMotor.setTargetPosition(duty * engineConfiguration->idleStepperTotalSteps);

unit_tests/mocks.h

@@ -0,0 +1,39 @@
+#include "electronic_throttle.h"
+#include "dc_motor.h"
+#include "table_helper.h"
+
+#include "gmock/gmock.h"
+
+class MockEtb : public IEtbController {
+public:
+	// PeriodicTimerController mocks
+	MOCK_METHOD(void, PeriodicTask, (), (override));
+	MOCK_METHOD(int, getPeriodMs, (), (override));
+
+	// IEtbController mocks
+	MOCK_METHOD(void, reset, (), ());
+	MOCK_METHOD(void, Start, (), (override));
+	MOCK_METHOD(void, init, (DcMotor* motor, int ownIndex, pid_s* pidParameters, const ValueProvider3D* pedalMap), (override));
+	MOCK_METHOD(void, setIdlePosition, (percent_t pos), (override));
+
+	// ClosedLoopController mocks
+	MOCK_METHOD(expected<percent_t>, getSetpoint, (), (const, override));
+	MOCK_METHOD(expected<percent_t>, observePlant, (), (const, override));
+	MOCK_METHOD(expected<percent_t>, getOpenLoop, (percent_t setpoint), (const, override));
+	MOCK_METHOD(expected<percent_t>, getClosedLoop, (percent_t setpoint, percent_t observation), (override));
+	MOCK_METHOD(void, setOutput, (expected<percent_t> outputValue), (override));
+};
+
+class MockMotor : public DcMotor {
+public:
+	MOCK_METHOD(bool, set, (float duty), (override));
+	MOCK_METHOD(float, get, (), (const, override));
+	MOCK_METHOD(void, enable, (), (override));
+	MOCK_METHOD(void, disable, (), (override));
+	MOCK_METHOD(bool, isOpenDirection, (), (const, override));
+};
+
+class MockVp3d : public ValueProvider3D {
+public:
+	MOCK_METHOD(float, getValue, (float xRpm, float y), (const, override));
+};

unit_tests/tests/test_etb.cpp

@@ -11,43 +11,12 @@
 #include "engine_controller.h"
 #include "sensor.h"
 
+#include "mocks.h"
+
 using ::testing::_;
 using ::testing::Ne;
 using ::testing::StrictMock;
 
-class MockEtb : public IEtbController {
-public:
-	// PeriodicTimerController mocks
-	MOCK_METHOD(void, PeriodicTask, (), (override));
-	MOCK_METHOD(int, getPeriodMs, (), (override));
-
-	// IEtbController mocks
-	MOCK_METHOD(void, reset, (), ());
-	MOCK_METHOD(void, Start, (), (override));
-	MOCK_METHOD(void, init, (DcMotor* motor, int ownIndex, pid_s* pidParameters, const ValueProvider3D* pedalMap), (override));
-
-	// ClosedLoopController mocks
-	MOCK_METHOD(expected<percent_t>, getSetpoint, (), (const, override));
-	MOCK_METHOD(expected<percent_t>, observePlant, (), (const, override));
-	MOCK_METHOD(expected<percent_t>, getOpenLoop, (percent_t setpoint), (const, override));
-	MOCK_METHOD(expected<percent_t>, getClosedLoop, (percent_t setpoint, percent_t observation), (override));
-	MOCK_METHOD(void, setOutput, (expected<percent_t> outputValue), (override));
-};
-
-class MockMotor : public DcMotor {
-public:
-	MOCK_METHOD(bool, set, (float duty), (override));
-	MOCK_METHOD(float, get, (), (const, override));
-	MOCK_METHOD(void, enable, (), (override));
-	MOCK_METHOD(void, disable, (), (override));
-	MOCK_METHOD(bool, isOpenDirection, (), (const, override));
-};
-
-class MockVp3d : public ValueProvider3D {
-public:
-	MOCK_METHOD(float, getValue, (float xRpm, float y), (const, override));
-};
-
 TEST(etb, initializationNoPedal) {
 	StrictMock<MockEtb> mocks[ETB_COUNT];
 
@@ -113,16 +82,26 @@ TEST(etb, initializationDualThrottle) {
 	doInitElectronicThrottle(PASS_ENGINE_PARAMETER_SIGNATURE);
 }
 
+TEST(etb, idlePlumbing) {
+	StrictMock<MockEtb> mocks[ETB_COUNT];
+
+	WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
+
+	for (int i = 0; i < ETB_COUNT; i++) {
+		engine->etbControllers[i] = &mocks[i];
+
+		EXPECT_CALL(mocks[i], setIdlePosition(33.0f));
+	}
+
+	setEtbIdlePosition(33.0f PASS_ENGINE_PARAMETER_SUFFIX);
+}
+
 TEST(etb, testSetpointOnlyPedal) {
 	WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
 
 	// Don't use ETB for idle, we aren't testing that yet - just pedal table for now
 	engineConfiguration->useETBforIdleControl = false;
 
-	// Must have a sensor configured before init
-	Sensor::setMockValue(SensorType::AcceleratorPedal, 0);
-	Sensor::setMockValue(SensorType::Tps1, 0);
-
 	EtbController etb;
 	INJECT_ENGINE_REFERENCE(&etb);
 
@@ -158,11 +137,67 @@ TEST(etb, testSetpointOnlyPedal) {
 	Sensor::setMockValue(SensorType::AcceleratorPedal, 105);
 	EXPECT_EQ(100, etb.getSetpoint().value_or(-1));
 
+	// Check that ETB idle does NOT work - it's disabled
+	etb.setIdlePosition(50);
+	Sensor::setMockValue(SensorType::AcceleratorPedal, 0);
+	EXPECT_EQ(0, etb.getSetpoint().value_or(-1));
+	Sensor::setMockValue(SensorType::AcceleratorPedal, 20);
+	EXPECT_EQ(20, etb.getSetpoint().value_or(-1));
+
 	// Test invalid pedal position - should give unexpected
 	Sensor::resetMockValue(SensorType::AcceleratorPedal);
 	EXPECT_EQ(etb.getSetpoint(), unexpected);
 }
 
+TEST(etb, setpointIdle) {
+	WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
+
+	// Use ETB for idle, but don't give it any range (yet)
+	engineConfiguration->useETBforIdleControl = true;
+	engineConfiguration->etbIdleThrottleRange = 0;
+
+	EtbController etb;
+	INJECT_ENGINE_REFERENCE(&etb);
+
+	// Mock pedal map that's just passthru pedal -> target
+	StrictMock<MockVp3d> pedalMap;
+	EXPECT_CALL(pedalMap, getValue(_, _))
+		.WillRepeatedly([](float xRpm, float y) {
+			return y;
+		});
+	etb.init(nullptr, 0, nullptr, &pedalMap);
+
+	// No idle range, should just pass pedal
+	Sensor::setMockValue(SensorType::AcceleratorPedal, 0.0f);
+	EXPECT_EQ(0, etb.getSetpoint().value_or(-1));
+	Sensor::setMockValue(SensorType::AcceleratorPedal, 50.0f);
+	EXPECT_EQ(50, etb.getSetpoint().value_or(-1));
+
+	// Idle should now have 10% range
+	engineConfiguration->etbIdleThrottleRange = 10;
+
+	// 50% idle position should increase setpoint by 5%
+	etb.setIdlePosition(50);
+	Sensor::setMockValue(SensorType::AcceleratorPedal, 0.0f);
+	EXPECT_FLOAT_EQ(5, etb.getSetpoint().value_or(-1));
+	Sensor::setMockValue(SensorType::AcceleratorPedal, 50.0f);
+	EXPECT_FLOAT_EQ(55, etb.getSetpoint().value_or(-1));
+
+	// 100% setpoint should increase by 10%
+	etb.setIdlePosition(100);
+	Sensor::setMockValue(SensorType::AcceleratorPedal, 0.0f);
+	EXPECT_FLOAT_EQ(10, etb.getSetpoint().value_or(-1));
+	Sensor::setMockValue(SensorType::AcceleratorPedal, 50.0f);
+	EXPECT_FLOAT_EQ(60, etb.getSetpoint().value_or(-1));
+
+	// 125% setpoint should clamp to 10% increase
+	etb.setIdlePosition(125);
+	Sensor::setMockValue(SensorType::AcceleratorPedal, 0.0f);
+	EXPECT_FLOAT_EQ(10, etb.getSetpoint().value_or(-1));
+	Sensor::setMockValue(SensorType::AcceleratorPedal, 50.0f);
+	EXPECT_FLOAT_EQ(60, etb.getSetpoint().value_or(-1));
+}
+
 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);