diff --git a/firmware/controllers/actuators/electronic_throttle.cpp b/firmware/controllers/actuators/electronic_throttle.cpp index 72c8e1fdbd..d470d23a99 100644 --- a/firmware/controllers/actuators/electronic_throttle.cpp +++ b/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 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; diff --git a/firmware/util/efilib.cpp b/firmware/util/efilib.cpp index b8d4b1173c..8360321914 100644 --- a/firmware/util/efilib.cpp +++ b/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) diff --git a/firmware/util/efilib.h b/firmware/util/efilib.h index dd994b97c1..2f8ca605d6 100644 --- a/firmware/util/efilib.h +++ b/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); diff --git a/unit_tests/test_basic_math/test_efilib.cpp b/unit_tests/test_basic_math/test_efilib.cpp index 713083f6b3..e6070a4b58 100644 --- a/unit_tests/test_basic_math/test_efilib.cpp +++ b/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); + +} diff --git a/unit_tests/tests/test_etb.cpp b/unit_tests/tests/test_etb.cpp index f07b30bdab..73cdc46dd8 100644 --- a/unit_tests/tests/test_etb.cpp +++ b/unit_tests/tests/test_etb.cpp @@ -33,6 +33,14 @@ public: MOCK_METHOD(void, setOutput, (expected 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 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 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 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 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 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 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); +}