diff --git a/firmware/controllers/actuators/idle_hardware.cpp b/firmware/controllers/actuators/idle_hardware.cpp index b405b6e52f..8467f4b4ff 100644 --- a/firmware/controllers/actuators/idle_hardware.cpp +++ b/firmware/controllers/actuators/idle_hardware.cpp @@ -22,25 +22,12 @@ static DualHBridgeStepper iacHbridgeHw; StepperMotor iacMotor; #endif /* EFI_UNIT_TEST */ -/** - * When the IAC position value change is insignificant (lower than this threshold), leave the poor valve alone - * This is about stepper motors, it equals to 1 step to avoid excessive micro-movement. - */ -static percent_t idlePositionSensitivityThreshold = 0.0f; - static SimplePwm idleSolenoidOpen("idle open"); static SimplePwm idleSolenoidClose("idle close"); extern efitimeus_t timeToStopIdleTest; void applyIACposition(percent_t position DECLARE_ENGINE_PARAMETER_SUFFIX) { - bool prettyClose = absF(position - engine->engineState.idle.currentIdlePosition) < idlePositionSensitivityThreshold; - // The threshold is dependent on IAC type (see initIdleHardware()) - if (prettyClose) { - return; // value is pretty close, let's leave the poor valve alone - } - - /** * currently idle level is an percent value (0-100 range), and PWM takes a float in the 0..1 range * todo: unify? @@ -131,9 +118,6 @@ void initIdleHardware(DECLARE_ENGINE_PARAMETER_SUFFIX) { } iacMotor.initialize(hw, CONFIG(idleStepperTotalSteps)); - - // This greatly improves PID accuracy for steppers with a small number of steps - idlePositionSensitivityThreshold = 1.0f / engineConfiguration->idleStepperTotalSteps; } else if (engineConfiguration->useETBforIdleControl || !isBrainPinValid(CONFIG(idle).solenoidPin)) { // here we do nothing for ETB idle and for no idle } else { @@ -159,8 +143,6 @@ void initIdleHardware(DECLARE_ENGINE_PARAMETER_SUFFIX) { &enginePins.secondIdleSolenoidPin, CONFIG(idle).solenoidFrequency, PERCENT_TO_DUTY(CONFIG(manIdlePosition))); } - - idlePositionSensitivityThreshold = 0.0f; } } diff --git a/firmware/hw_layer/stepper.cpp b/firmware/hw_layer/stepper.cpp index f3624792e4..62b54db55b 100644 --- a/firmware/hw_layer/stepper.cpp +++ b/firmware/hw_layer/stepper.cpp @@ -9,13 +9,30 @@ #include "pch.h" +#include "stepper.h" + +float StepperMotorBase::getTargetPosition() const { + return m_targetPosition; +} + +void StepperMotorBase::setTargetPosition(float targetPositionSteps) { + // When the IAC position value change is insignificant (lower than this threshold), leave the poor valve alone + // When we get a larger change, actually update the target stepper position + if (absF(m_targetPosition - targetPositionSteps) >= 1) { + m_targetPosition = targetPositionSteps; + } +} + +void StepperMotorBase::initialize(StepperHw *hardware, int totalSteps) { + m_totalSteps = maxI(3, totalSteps); + + m_hw = hardware; +} + // todo: EFI_STEPPER macro #if EFI_PROD_CODE || EFI_SIMULATOR -#include "stepper.h" -#include "adc_inputs.h" -#include "thread_priority.h" -void StepperMotor::saveStepperPos(int pos) { +void StepperMotorBase::saveStepperPos(int pos) { // use backup-power RTC registers to store the data #if EFI_PROD_CODE backupRamSave(BACKUP_STEPPER_POS, pos + 1); @@ -23,7 +40,7 @@ void StepperMotor::saveStepperPos(int pos) { postCurrentPosition(); } -int StepperMotor::loadStepperPos() { +int StepperMotorBase::loadStepperPos() { #if EFI_PROD_CODE return (int)backupRamLoad(BACKUP_STEPPER_POS) - 1; #else @@ -31,7 +48,7 @@ int StepperMotor::loadStepperPos() { #endif } -void StepperMotor::changeCurrentPosition(bool positive) { +void StepperMotorBase::changeCurrentPosition(bool positive) { if (positive) { m_currentPosition++; } else { @@ -40,7 +57,7 @@ void StepperMotor::changeCurrentPosition(bool positive) { postCurrentPosition(); } -void StepperMotor::postCurrentPosition(void) { +void StepperMotorBase::postCurrentPosition(void) { if (engineConfiguration->debugMode == DBG_IDLE_CONTROL) { #if EFI_TUNER_STUDIO tsOutputChannels.debugIntField5 = m_currentPosition; @@ -48,7 +65,7 @@ void StepperMotor::postCurrentPosition(void) { } } -void StepperMotor::setInitialPosition(void) { +void StepperMotorBase::setInitialPosition(void) { // try to get saved stepper position (-1 for no data) m_currentPosition = loadStepperPos(); @@ -104,60 +121,43 @@ void StepperMotor::setInitialPosition(void) { initialPositionSet = true; } -void StepperMotor::ThreadTask() { - // Require hardware to be set - if (!m_hw) { +void StepperMotorBase::doIteration() { + int targetPosition = efiRound(getTargetPosition(), 1); + int currentPosition = m_currentPosition; + + // the stepper does not work if the main relay is turned off (it requires +12V) + if (!engine->isMainRelayEnabled()) { + m_hw->pause(); return; } - while (true) { - int targetPosition = getTargetPosition(); - int currentPosition = m_currentPosition; + if (!initialPositionSet) { + setInitialPosition(); + return; + } - // the stepper does not work if the main relay is turned off (it requires +12V) - if (!engine->isMainRelayEnabled()) { - m_hw->pause(); - continue; - } + if (targetPosition == currentPosition) { + m_hw->pause(); + m_isBusy = false; + return; + } - if (!initialPositionSet) { - setInitialPosition(); - continue; - } + m_isBusy = true; - if (targetPosition == currentPosition) { - m_hw->pause(); - continue; - } + bool isIncrementing = targetPosition > currentPosition; - bool isIncrementing = targetPosition > currentPosition; + if (m_hw->step(isIncrementing)) { + changeCurrentPosition(isIncrementing); + } - if (m_hw->step(isIncrementing)) { - changeCurrentPosition(isIncrementing); - } - - // save position to backup RTC register + // save position to backup RTC register #if EFI_PROD_CODE - saveStepperPos(m_currentPosition); + saveStepperPos(m_currentPosition); #endif - } } -StepperMotor::StepperMotor() : ThreadController("stepper", PRIO_STEPPER) {} - -int StepperMotor::getTargetPosition() const { - return m_targetPosition; -} - -void StepperMotor::setTargetPosition(int targetPosition) { - // we accept a new target position only if the motor is powered from the main relay - if (engine->isMainRelayEnabled()) { - m_targetPosition = targetPosition; - } -} - -bool StepperMotor::isBusy() const { - return m_currentPosition != m_targetPosition; +bool StepperMotorBase::isBusy() const { + return m_isBusy; } void StepDirectionStepper::setDirection(bool isIncrementing) { @@ -202,9 +202,7 @@ bool StepDirectionStepper::step(bool positive) { } void StepperMotor::initialize(StepperHw *hardware, int totalSteps) { - m_totalSteps = maxI(3, totalSteps); - - m_hw = hardware; + StepperMotorBase::initialize(hardware, totalSteps); Start(); } diff --git a/firmware/hw_layer/stepper.h b/firmware/hw_layer/stepper.h index 0a3b5bacca..fa68d29fa7 100644 --- a/firmware/hw_layer/stepper.h +++ b/firmware/hw_layer/stepper.h @@ -55,14 +55,13 @@ private: uint8_t m_phase = 0; }; -class StepperMotor final : private ThreadController { +class StepperMotorBase { public: - StepperMotor(); + virtual void initialize(StepperHw* hardware, int totalSteps); + void doIteration(); - void initialize(StepperHw *hardware, int totalSteps); - - void setTargetPosition(int targetPosition); - int getTargetPosition() const; + void setTargetPosition(float targetPositionSteps); + float getTargetPosition() const; bool isBusy() const; @@ -70,7 +69,6 @@ public: int m_totalSteps = 0; protected: - void ThreadTask() override; void setInitialPosition(void); void saveStepperPos(int pos); @@ -79,10 +77,32 @@ protected: void changeCurrentPosition(bool positive); void postCurrentPosition(void); -private: StepperHw* m_hw = nullptr; - int m_targetPosition = 0; + float m_targetPosition = 0; bool initialPositionSet = false; + bool m_isBusy = false; }; +#if !EFI_UNIT_TEST + +#include "thread_priority.h" + +class StepperMotor final : public StepperMotorBase, private ThreadController { +public: + StepperMotor() : ThreadController("stepper", PRIO_STEPPER) {} + + void initialize(StepperHw* hardware, int totalSteps); + + void ThreadTask() override { + // Require hardware to be set + if (!m_hw) { + return; + } + + while (true) { + doIteration(); + } + } +}; +#endif diff --git a/unit_tests/mocks.h b/unit_tests/mocks.h index c22d4c6da4..c323411c27 100644 --- a/unit_tests/mocks.h +++ b/unit_tests/mocks.h @@ -6,6 +6,7 @@ #include "pwm_generator_logic.h" #include "airmass.h" #include "injector_model.h" +#include "stepper.h" #include "gmock/gmock.h" @@ -74,3 +75,8 @@ public: MOCK_METHOD(floatms_t, getInjectionDuration, (float fuelMassGram), (const, override)); MOCK_METHOD(float, getFuelMassForDuration, (floatms_t duration), (const, override)); }; + +class MockStepperHardware : public StepperHw { +public: + MOCK_METHOD(bool, step, (bool positive), (override)); +}; diff --git a/unit_tests/tests/test_stepper.cpp b/unit_tests/tests/test_stepper.cpp new file mode 100644 index 0000000000..b02a1e467f --- /dev/null +++ b/unit_tests/tests/test_stepper.cpp @@ -0,0 +1,26 @@ +#include "stepper.h" +#include "engine_test_helper.h" +#include "mocks.h" + +TEST(Stepper, IgnoreSmallChanges) { + StepperMotorBase dut; + + dut.setTargetPosition(10); + + // Record initial reported position + auto initialPosition = dut.getTargetPosition(); + + // Small changes should be ignored + dut.setTargetPosition(10.5f); + EXPECT_EQ(initialPosition, dut.getTargetPosition()); + dut.setTargetPosition(9.5f); + EXPECT_EQ(initialPosition, dut.getTargetPosition()); + + // Change of >= 1 should cause a change + dut.setTargetPosition(11.5f); + EXPECT_EQ(11.5f, dut.getTargetPosition()); + + // Now go back the other way + dut.setTargetPosition(9.5f); + EXPECT_EQ(9.5f, dut.getTargetPosition()); +} diff --git a/unit_tests/tests/tests.mk b/unit_tests/tests/tests.mk index 90a5532627..6ae2214b24 100644 --- a/unit_tests/tests/tests.mk +++ b/unit_tests/tests/tests.mk @@ -84,5 +84,6 @@ TESTS_SRC_CPP = \ tests/test_gpio.cpp \ tests/test_limp.cpp \ tests/trigger/test_all_triggers.cpp \ + tests/test_stepper.cpp \ tests/sensor/test_frequency_sensor.cpp \