ETB testing: output, position clamping (#1333)

* add clampf

* more tests

* public

* missed a mock

* fix output duty clamping

* do it that way

* more

* ah ha!

* test negative too

* clamp pedal

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

firmware/controllers/actuators/electronic_throttle.cpp

@@ -110,7 +110,7 @@ static percent_t currentEtbDuty;
 
 #define ETB_DUTY_LIMIT 0.9
 // this macro clamps both positive and negative percentages from about -100% to 100%
-#define ETB_PERCENT_TO_DUTY(X) (maxF(minF((X * 0.01), ETB_DUTY_LIMIT - 0.01), 0.01 - ETB_DUTY_LIMIT))
+#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) {
 	m_motor = motor;
@@ -155,8 +155,10 @@ expected<percent_t> EtbController::getSetpoint() const {
 		return unexpected;
 	}
 
+	float sanitizedPedal = clampF(0, pedalPosition.Value, 100);
+	
 	float rpm = GET_RPM();
-	engine->engineState.targetFromTable = pedal2tpsMap.getValue(rpm / RPM_1_BYTE_PACKING_MULT, pedalPosition.Value);
+	engine->engineState.targetFromTable = pedal2tpsMap.getValue(rpm / RPM_1_BYTE_PACKING_MULT, sanitizedPedal);
 	percent_t etbIdleAddition = CONFIG(useETBforIdleControl) ? engine->engineState.idle.etbIdleAddition : 0;
 
 	float target = engine->engineState.targetFromTable + etbIdleAddition;

firmware/util/efilib.cpp

@@ -59,6 +59,10 @@ float minF(float i1, float i2) {
 	return i1 < i2 ? i1 : i2;
 }
 
+float clampF(float min, float clamp, float max) {
+	return maxF(min, minF(clamp, max));
+}
+
 uint32_t efiStrlen(const char *param) {
 	register const char *s;
 	for (s = param; *s; ++s)

firmware/util/efilib.h

@@ -61,6 +61,7 @@ float maxF(float i1, float i2);
 float minF(float i1, float i2);
 char* itoa10(char *p, int num);
 bool isSameF(float v1, float v2);
+float clampF(float min, float clamp, float max);
 
 bool strEqualCaseInsensitive(const char *str1, const char *str2);
 bool strEqual(const char *str1, const char *str2);

unit_tests/test_basic_math/test_efilib.cpp

@@ -21,3 +21,18 @@ TEST(EfiLibTest, ExpTaylor)
         EXPECT_NEAR(expf_taylor(x), expf(x), 0.01f);
     }
 }
+
+TEST(EfiLibTest, clampf) {
+	// off scale low
+	EXPECT_EQ(clampF(10, 5, 20), 10);
+	EXPECT_EQ(clampF(-10, -50, 10), -10);
+
+	// in range (unclamped)
+	EXPECT_EQ(clampF(10, 15, 20), 15);
+	EXPECT_EQ(clampF(-10, -5, 10), -5);
+
+	// off scale high
+	EXPECT_EQ(clampF(10, 25, 20), 20);
+	EXPECT_EQ(clampF(-10, 50, 10), 10);
+
+}

unit_tests/tests/test_etb.cpp

@@ -33,6 +33,14 @@ public:
 	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));
+};
 
 TEST(etb, initializationNoPedal) {
 	StrictMock<MockEtb> mocks[ETB_COUNT];
@@ -127,6 +135,12 @@ TEST(etb, testSetpointOnlyPedal) {
 	Sensor::setMockValue(SensorType::AcceleratorPedal, 51.6605f);
 	EXPECT_NEAR(51.6605, etb.getSetpoint().value_or(-1), EPS4D);
 
+	// Valid but out of range - should clamp to [0, 100]
+	Sensor::setMockValue(SensorType::AcceleratorPedal, -5);
+	EXPECT_EQ(0, etb.getSetpoint().value_or(-1));
+	Sensor::setMockValue(SensorType::AcceleratorPedal, 105);
+	EXPECT_EQ(100, etb.getSetpoint().value_or(-1));
+
 	// Test invalid pedal position - should give unexpected
 	Sensor::resetMockValue(SensorType::AcceleratorPedal);
 	EXPECT_EQ(etb.getSetpoint(), unexpected);
@@ -151,3 +165,71 @@ TEST(etb, etbTpsSensor) {
 		EXPECT_EQ(etb.observePlant().Value, 75.0f);
 	}
 }
+
+TEST(etb, setOutputInvalid) {
+	StrictMock<MockMotor> motor;
+
+	EtbController etb;
+	etb.init(&motor, 0, nullptr);
+
+	// Should be disabled in case of unexpected
+	EXPECT_CALL(motor, disable());
+
+	etb.setOutput(unexpected);
+}
+
+TEST(etb, setOutputValid) {
+	StrictMock<MockMotor> motor;
+
+	EtbController etb;
+	etb.init(&motor, 0, nullptr);
+
+	// Should be enabled and value set
+	EXPECT_CALL(motor, enable());
+	EXPECT_CALL(motor, set(0.25f))
+		.WillOnce(Return(false));
+
+	etb.setOutput(25.0f);
+}
+
+TEST(etb, setOutputValid2) {
+	StrictMock<MockMotor> motor;
+
+	EtbController etb;
+	etb.init(&motor, 0, nullptr);
+
+	// Should be enabled and value set
+	EXPECT_CALL(motor, enable());
+	EXPECT_CALL(motor, set(-0.25f))
+		.WillOnce(Return(false));
+
+	etb.setOutput(-25.0f);
+}
+
+TEST(etb, setOutputOutOfRangeHigh) {
+	StrictMock<MockMotor> motor;
+
+	EtbController etb;
+	etb.init(&motor, 0, nullptr);
+
+	// Should be enabled and value set
+	EXPECT_CALL(motor, enable());
+	EXPECT_CALL(motor, set(0.90f));
+
+	// Off scale - should get clamped to 90%
+	etb.setOutput(110);
+}
+
+TEST(etb, setOutputOutOfRangeLow) {
+	StrictMock<MockMotor> motor;
+
+	EtbController etb;
+	etb.init(&motor, 0, nullptr);
+
+	// Should be enabled and value set
+	EXPECT_CALL(motor, enable());
+	EXPECT_CALL(motor, set(-0.90f));
+
+	// Off scale - should get clamped to -90%
+	etb.setOutput(-110);
+}