ETB: inject pedal map (#1334)

* add clampf

* more tests

* public

* missed a mock

* fix output duty clamping

* do it that way

* more

* ah ha!

* test negative too

* clamp pedal

* inject pedal map

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

firmware/controllers/actuators/electronic_throttle.cpp

@@ -112,10 +112,11 @@ static percent_t currentEtbDuty;
 // this macro clamps both positive and negative percentages from about -100% to 100%
 #define ETB_PERCENT_TO_DUTY(x) (clampF(-ETB_DUTY_LIMIT, 0.01f * (x), ETB_DUTY_LIMIT))
 
-void EtbController::init(DcMotor *motor, int ownIndex, pid_s *pidParameters) {
+void EtbController::init(DcMotor *motor, int ownIndex, pid_s *pidParameters, const ValueProvider3D* pedalMap) {
 	m_motor = motor;
 	m_myIndex = ownIndex;
 	m_pid.initPidClass(pidParameters);
+	m_pedalMap = pedalMap;
 }
 
 void EtbController::reset() {
@@ -150,6 +151,11 @@ expected<percent_t> EtbController::getSetpoint() const {
 		return unexpected;
 	}
 
+	// If the pedal map hasn't been set, we can't provide a setpoint.
+	if (!m_pedalMap) {
+		return unexpected;
+	}
+
 	auto pedalPosition = Sensor::get(SensorType::AcceleratorPedal);
 	if (!pedalPosition.Valid) {
 		return unexpected;
@@ -158,7 +164,7 @@ expected<percent_t> EtbController::getSetpoint() const {
 	float sanitizedPedal = clampF(0, pedalPosition.Value, 100);
 	
 	float rpm = GET_RPM();
-	engine->engineState.targetFromTable = pedal2tpsMap.getValue(rpm / RPM_1_BYTE_PACKING_MULT, sanitizedPedal);
+	engine->engineState.targetFromTable = m_pedalMap->getValue(rpm / RPM_1_BYTE_PACKING_MULT, sanitizedPedal);
 	percent_t etbIdleAddition = CONFIG(useETBforIdleControl) ? engine->engineState.idle.etbIdleAddition : 0;
 
 	float target = engine->engineState.targetFromTable + etbIdleAddition;
@@ -572,6 +578,8 @@ void doInitElectronicThrottle(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 		return;
 	}
 
+	pedal2tpsMap.init(config->pedalToTpsTable, config->pedalToTpsPedalBins, config->pedalToTpsRpmBins);
+
 	engine->etbActualCount = Sensor::hasSensor(SensorType::Tps2) ? 2 : 1;
 
 	for (int i = 0 ; i < engine->etbActualCount; i++) {
@@ -580,13 +588,11 @@ void doInitElectronicThrottle(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 		// If this motor is actually set up, init the etb
 		if (motor)
 		{
-			engine->etbControllers[i]->init(motor, i, &engineConfiguration->etb);
+			engine->etbControllers[i]->init(motor, i, &engineConfiguration->etb, &pedal2tpsMap);
 			INJECT_ENGINE_REFERENCE(engine->etbControllers[i]);
 		}
 	}
 
-	pedal2tpsMap.init(config->pedalToTpsTable, config->pedalToTpsPedalBins, config->pedalToTpsRpmBins);
-
 #if 0 && ! EFI_UNIT_TEST
 	percent_t startupThrottlePosition = getTPS(PASS_ENGINE_PARAMETER_SIGNATURE);
 	if (absF(startupThrottlePosition - engineConfiguration->etbNeutralPosition) > STARTUP_NEUTRAL_POSITION_ERROR_THRESHOLD) {

firmware/controllers/actuators/electronic_throttle.h

@@ -22,13 +22,13 @@ class Logging;
 class IEtbController : public PeriodicTimerController, public ClosedLoopController<percent_t, percent_t> {
 public:
 	DECLARE_ENGINE_PTR;
-	virtual void init(DcMotor *motor, int ownIndex, pid_s *pidParameters) = 0;
+	virtual void init(DcMotor *motor, int ownIndex, pid_s *pidParameters, const ValueProvider3D* pedalMap) = 0;
 	virtual void reset() = 0;
 };
 
 class EtbController final : public IEtbController {
 public:
-	void init(DcMotor *motor, int ownIndex, pid_s *pidParameters) override;
+	void init(DcMotor *motor, int ownIndex, pid_s *pidParameters, const ValueProvider3D* pedalMap) override;
 
 	// PeriodicTimerController implementation
 	int getPeriodMs() override;
@@ -55,11 +55,14 @@ public:
 	const pid_state_s* getPidState() const { return &m_pid; };
 
 private:
-	int m_myIndex;
+	int m_myIndex = 0;
-	DcMotor *m_motor;
+	DcMotor *m_motor = nullptr;
 	Pid m_pid;
 	bool m_shouldResetPid = false;
 
+	// Pedal -> target map
+	const ValueProvider3D* m_pedalMap = nullptr;
+
 	// Autotune helpers
 	bool m_lastIsPositive = false;
 	efitick_t m_cycleStartTime = 0;

unit_tests/tests/test_etb.cpp

@@ -12,6 +12,7 @@
 #include "sensor.h"
 
 using ::testing::_;
+using ::testing::Ne;
 using ::testing::StrictMock;
 
 class MockEtb : public IEtbController {
@@ -23,7 +24,7 @@ public:
 	// IEtbController mocks
 	MOCK_METHOD(void, reset, (), ());
 	MOCK_METHOD(void, Start, (), (override));
-	MOCK_METHOD(void, init, (DcMotor* motor, int ownIndex, pid_s* pidParameters));
+	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));
@@ -42,6 +43,11 @@ public:
 	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];
 
@@ -70,7 +76,7 @@ TEST(etb, initializationSingleThrottle) {
 	Sensor::setMockValue(SensorType::AcceleratorPedal, 0);
 
 	// Expect mock0 to be init with index 0, and PID params
-	EXPECT_CALL(mocks[0], init(_, 0, &engineConfiguration->etb));
+	EXPECT_CALL(mocks[0], init(_, 0, &engineConfiguration->etb, Ne(nullptr)));
 	EXPECT_CALL(mocks[0], reset);
 	EXPECT_CALL(mocks[0], Start);
 
@@ -95,12 +101,12 @@ TEST(etb, initializationDualThrottle) {
 	Sensor::setMockValue(SensorType::Tps2, 25.0f);
 
 	// Expect mock0 to be init with index 0, and PID params
-	EXPECT_CALL(mocks[0], init(_, 0, &engineConfiguration->etb));
+	EXPECT_CALL(mocks[0], init(_, 0, &engineConfiguration->etb, Ne(nullptr)));
 	EXPECT_CALL(mocks[0], reset);
 	EXPECT_CALL(mocks[0], Start);
 
 	// Expect mock1 to be init with index 2, and PID params
-	EXPECT_CALL(mocks[1], init(_, 1, &engineConfiguration->etb));
+	EXPECT_CALL(mocks[1], init(_, 1, &engineConfiguration->etb, Ne(nullptr)));
 	EXPECT_CALL(mocks[1], reset);
 	EXPECT_CALL(mocks[1], Start);
 
@@ -118,8 +124,19 @@ TEST(etb, testSetpointOnlyPedal) {
 	Sensor::setMockValue(SensorType::Tps1, 0);
 
 	EtbController etb;
-	engine->etbControllers[0] = &etb;
+	INJECT_ENGINE_REFERENCE(&etb);
-	doInitElectronicThrottle(PASS_ENGINE_PARAMETER_SIGNATURE);
+
+	// Mock pedal map that's just passthru pedal -> target
+	StrictMock<MockVp3d> pedalMap;
+	EXPECT_CALL(pedalMap, getValue(_, _))
+		.WillRepeatedly([](float xRpm, float y) {
+			return y;
+		});
+
+	// Uninitialized ETB must return unexpected (and not deference a null pointer)
+	EXPECT_EQ(etb.getSetpoint(), unexpected);
+
+	etb.init(nullptr, 0, nullptr, &pedalMap);
 
 	// Check endpoints and midpoint
 	Sensor::setMockValue(SensorType::AcceleratorPedal, 0.0f);
@@ -154,14 +171,14 @@ TEST(etb, etbTpsSensor) {
 	// Test first throttle
 	{
 		EtbController etb;
-		etb.init(nullptr, 0, nullptr);
+		etb.init(nullptr, 0, nullptr, nullptr);
 		EXPECT_EQ(etb.observePlant().Value, 25.0f);
 	}
 
 	// Test second throttle
 	{
 		EtbController etb;
-		etb.init(nullptr, 1, nullptr);
+		etb.init(nullptr, 1, nullptr, nullptr);
 		EXPECT_EQ(etb.observePlant().Value, 75.0f);
 	}
 }
@@ -170,7 +187,7 @@ TEST(etb, setOutputInvalid) {
 	StrictMock<MockMotor> motor;
 
 	EtbController etb;
-	etb.init(&motor, 0, nullptr);
+	etb.init(&motor, 0, nullptr, nullptr);
 
 	// Should be disabled in case of unexpected
 	EXPECT_CALL(motor, disable());
@@ -182,7 +199,7 @@ TEST(etb, setOutputValid) {
 	StrictMock<MockMotor> motor;
 
 	EtbController etb;
-	etb.init(&motor, 0, nullptr);
+	etb.init(&motor, 0, nullptr, nullptr);
 
 	// Should be enabled and value set
 	EXPECT_CALL(motor, enable());
@@ -196,7 +213,8 @@ TEST(etb, setOutputValid2) {
 	StrictMock<MockMotor> motor;
 
 	EtbController etb;
-	etb.init(&motor, 0, nullptr);
+
+	etb.init(&motor, 0, nullptr, nullptr);
 
 	// Should be enabled and value set
 	EXPECT_CALL(motor, enable());
@@ -210,7 +228,7 @@ TEST(etb, setOutputOutOfRangeHigh) {
 	StrictMock<MockMotor> motor;
 
 	EtbController etb;
-	etb.init(&motor, 0, nullptr);
+	etb.init(&motor, 0, nullptr, nullptr);
 
 	// Should be enabled and value set
 	EXPECT_CALL(motor, enable());
@@ -224,7 +242,7 @@ TEST(etb, setOutputOutOfRangeLow) {
 	StrictMock<MockMotor> motor;
 
 	EtbController etb;
-	etb.init(&motor, 0, nullptr);
+	etb.init(&motor, 0, nullptr, nullptr);
 
 	// Should be enabled and value set
 	EXPECT_CALL(motor, enable());