diff --git a/firmware/controllers/actuators/electronic_throttle.cpp b/firmware/controllers/actuators/electronic_throttle.cpp index cf6d872fbc..9460fd6743 100644 --- a/firmware/controllers/actuators/electronic_throttle.cpp +++ b/firmware/controllers/actuators/electronic_throttle.cpp @@ -269,7 +269,7 @@ expected EtbController::getSetpoint() { expected 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 EtbController::getOutput() { - // total open + closed loop parts - expected 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() { diff --git a/firmware/controllers/actuators/electronic_throttle.h b/firmware/controllers/actuators/electronic_throttle.h index 96885e4af3..04d84e7d97 100644 --- a/firmware/controllers/actuators/electronic_throttle.h +++ b/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 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; }; diff --git a/firmware/controllers/actuators/electronic_throttle.txt b/firmware/controllers/actuators/electronic_throttle.txt index 009980d2c5..0f565e5d14 100644 --- a/firmware/controllers/actuators/electronic_throttle.txt +++ b/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 \ No newline at end of file diff --git a/firmware/controllers/actuators/electronic_throttle_impl.h b/firmware/controllers/actuators/electronic_throttle_impl.h index a0613b29f1..d485716ec2 100644 --- a/firmware/controllers/actuators/electronic_throttle_impl.h +++ b/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 getOutput() override; // Called when the configuration may have changed. Controller will // reset if necessary. @@ -57,8 +56,10 @@ public: void setOutput(expected 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; diff --git a/firmware/controllers/closed_loop_controller.h b/firmware/controllers/closed_loop_controller.h index 680de134d9..b73e57fbfa 100644 --- a/firmware/controllers/closed_loop_controller.h +++ b/firmware/controllers/closed_loop_controller.h @@ -9,12 +9,15 @@ template class ClosedLoopController { public: - void update() { + expected update() { expected outputValue = getOutput(); setOutput(outputValue); + + return outputValue; } - virtual expected getOutput() { +private: + expected getOutput() { expected 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 getSetpoint() = 0; diff --git a/unit_tests/mocks.h b/unit_tests/mocks.h index b4d45c5034..011d19acc4 100644 --- a/unit_tests/mocks.h +++ b/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, getOutput, (), (override)); MOCK_METHOD(expected, getSetpoint, (), (override)); MOCK_METHOD(expected, observePlant, (), (const, override)); MOCK_METHOD(expected, getOpenLoop, (percent_t setpoint), (override)); diff --git a/unit_tests/tests/actuators/test_etb.cpp b/unit_tests/tests/actuators/test_etb.cpp index 2655788584..ceff7e9372 100644 --- a/unit_tests/tests/actuators/test_etb.cpp +++ b/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);