diff --git a/firmware/controllers/actuators/idle_thread.cpp b/firmware/controllers/actuators/idle_thread.cpp index f88e5cdf3e..c2c275c040 100644 --- a/firmware/controllers/actuators/idle_thread.cpp +++ b/firmware/controllers/actuators/idle_thread.cpp @@ -43,6 +43,7 @@ #if ! EFI_UNIT_TEST #include "stepper.h" #include "pin_repository.h" +static StepDirectionStepper iacStepperHw; static StepperMotor iacMotor; #endif /* EFI_UNIT_TEST */ @@ -557,13 +558,17 @@ void stopIdleHardware(DECLARE_ENGINE_PARAMETER_SIGNATURE) { void initIdleHardware(DECLARE_ENGINE_PARAMETER_SIGNATURE) { if (CONFIG(useStepperIdle)) { - iacMotor.initialize(CONFIG(idle).stepperStepPin, - CONFIG(idle).stepperDirectionPin, - engineConfiguration->stepperDirectionPinMode, - engineConfiguration->idleStepperReactionTime, - engineConfiguration->idleStepperTotalSteps, - engineConfiguration->stepperEnablePin, engineConfiguration->stepperEnablePinMode, - logger); + iacStepperHw.initialize( + CONFIG(idle).stepperStepPin, + CONFIG(idle).stepperDirectionPin, + CONFIG(stepperDirectionPinMode), + CONFIG(idleStepperReactionTime), + CONFIG(stepperEnablePin), + CONFIG(stepperEnablePinMode) + ); + + iacMotor.initialize(&iacStepperHw, CONFIG(idleStepperTotalSteps), logger); + // This greatly improves PID accuracy for steppers with a small number of steps idlePositionSensitivityThreshold = 1.0f / engineConfiguration->idleStepperTotalSteps; } else if (!engineConfiguration->useETBforIdleControl) { diff --git a/firmware/hw_layer/stepper.cpp b/firmware/hw_layer/stepper.cpp index eb7055b339..d1bceab345 100644 --- a/firmware/hw_layer/stepper.cpp +++ b/firmware/hw_layer/stepper.cpp @@ -37,6 +37,11 @@ static int loadStepperPos() { } void StepperMotor::ThreadTask() { + // Require hardware to be set + if (!m_hw) { + return; + } + // try to get saved stepper position (-1 for no data) m_currentPosition = loadStepperPos(); @@ -69,7 +74,7 @@ void StepperMotor::ThreadTask() { */ int numParkingSteps = (int)efiRound((1.0f + (float)CONFIG(stepperParkingExtraSteps) / PERCENT_MULT) * m_totalSteps, 1.0f); for (int i = 0; i < numParkingSteps; i++) { - pulse(); + m_hw->step(false); } // set & save zero stepper position after the parking completion @@ -86,18 +91,18 @@ void StepperMotor::ThreadTask() { int currentPosition = m_currentPosition; if (targetPosition == currentPosition) { - chThdSleepMilliseconds(m_reactionTime); + m_hw->pause(); continue; } bool isIncrementing = targetPosition > currentPosition; - setDirection(isIncrementing); + if (isIncrementing) { m_currentPosition++; } else { m_currentPosition--; } - pulse(); + m_hw->step(isIncrementing); // save position to backup RTC register #if EFI_PROD_CODE @@ -116,40 +121,58 @@ void StepperMotor::setTargetPosition(int targetPosition) { m_targetPosition = targetPosition; } -void StepperMotor::setDirection(bool isIncrementing) { +void StepDirectionStepper::setDirection(bool isIncrementing) { if (isIncrementing != m_currentDirection) { // compensate stepper motor inertia - chThdSleepMilliseconds(m_reactionTime); + pause(); m_currentDirection = isIncrementing; } - + directionPin.setValue(isIncrementing); } -void StepperMotor::pulse() { +void StepDirectionStepper::pulse() { enablePin.setValue(false); // enable stepper stepPin.setValue(true); - chThdSleepMilliseconds(m_reactionTime); + pause(); stepPin.setValue(false); - chThdSleepMilliseconds(m_reactionTime); + pause(); enablePin.setValue(true); // disable stepper } -void StepperMotor::initialize(brain_pin_e stepPin, brain_pin_e directionPin, pin_output_mode_e directionPinMode, - float reactionTime, int totalSteps, - brain_pin_e enablePin, pin_output_mode_e enablePinMode, Logging *sharedLogger) { - m_reactionTime = maxF(1, reactionTime); +void StepperHw::pause() const { + chThdSleepMilliseconds(m_reactionTime); +} + +void StepperHw::setReactionTime(float ms) { + m_reactionTime = maxF(1, ms); +} + +void StepDirectionStepper::step(bool positive) { + setDirection(positive); + pulse(); +} + +void StepperMotor::initialize(StepperHw *hardware, int totalSteps, Logging *sharedLogger) { m_totalSteps = maxI(3, totalSteps); - + + m_hw = hardware; + logger = sharedLogger; + Start(); +} + +void StepDirectionStepper::initialize(brain_pin_e stepPin, brain_pin_e directionPin, pin_output_mode_e directionPinMode, float reactionTime, brain_pin_e enablePin, pin_output_mode_e enablePinMode) { if (stepPin == GPIO_UNASSIGNED || directionPin == GPIO_UNASSIGNED) { return; } + setReactionTime(reactionTime); + this->directionPinMode = directionPinMode; this->directionPin.initPin("stepper dir", directionPin, &this->directionPinMode); @@ -164,8 +187,6 @@ void StepperMotor::initialize(brain_pin_e stepPin, brain_pin_e directionPin, pin this->stepPin.setValue(false); this->directionPin.setValue(false); m_currentDirection = false; - - Start(); } #endif diff --git a/firmware/hw_layer/stepper.h b/firmware/hw_layer/stepper.h index b587d15fa7..de4d356367 100644 --- a/firmware/hw_layer/stepper.h +++ b/firmware/hw_layer/stepper.h @@ -12,31 +12,53 @@ #include "backup_ram.h" #include "thread_controller.h" +class StepperHw { +public: + virtual void step(bool positive) = 0; + void pause() const; + +protected: + void setReactionTime(float ms); + +private: + float m_reactionTime = 5.0f; +}; + +class StepDirectionStepper final : public StepperHw { +public: + void initialize(brain_pin_e stepPin, brain_pin_e directionPin, pin_output_mode_e directionPinMode, float reactionTime, brain_pin_e enablePin, pin_output_mode_e enablePinMode); + + void step(bool positive) override; + +private: + void pulse(); + void setDirection(bool isIncrementing); + + bool m_currentDirection = false; + + OutputPin directionPin, stepPin, enablePin; + pin_output_mode_e directionPinMode, stepPinMode, enablePinMode; +}; + class StepperMotor final : private ThreadController { public: StepperMotor(); - void initialize(brain_pin_e stepPin, brain_pin_e directionPin, pin_output_mode_e directionPinMode, float reactionTime, int totalSteps, - brain_pin_e enablePin, pin_output_mode_e enablePinMode, Logging *sharedLogger); + void initialize(StepperHw *hardware, int totalSteps, Logging *sharedLogger); - void pulse(); void setTargetPosition(int targetPosition); int getTargetPosition() const; - void setDirection(bool isIncrementing); - OutputPin directionPin, stepPin, enablePin; int m_currentPosition = 0; - bool m_currentDirection = false; - float m_reactionTime = 0; int m_totalSteps = 0; protected: void ThreadTask() override; private: - int m_targetPosition = 0; + StepperHw* m_hw = nullptr; - pin_output_mode_e directionPinMode, stepPinMode, enablePinMode; + int m_targetPosition = 0; }; #endif /* STEPPER_H_ */