perform extra-step-prevention in the stepper driver itself (#3038)

* perform extra-step-prevention in the stepper driver itself

* isBusy flag

* I am unable to type

* the court of public opinion has decided that greater-equal is correct

* that sets target in steps, not percent

* correct var name

* mock stepper motor

* testable stepper

* build is happy

* helping build

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;
 	}
 }
 

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() {
+void StepperMotorBase::doIteration() {
-	// Require hardware to be set
+	int targetPosition = efiRound(getTargetPosition(), 1);
-	if (!m_hw) {
+	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) {
+	if (!initialPositionSet) {
-		int targetPosition = getTargetPosition();
+		setInitialPosition();
-		int currentPosition = m_currentPosition;
+		return;
+	}
 
-		// the stepper does not work if the main relay is turned off (it requires +12V)
+	if (targetPosition == currentPosition) {
-		if (!engine->isMainRelayEnabled()) {
+		m_hw->pause();
-			m_hw->pause();
+		m_isBusy = false;
-			continue;
+		return;
-		}
+	}
 
-		if (!initialPositionSet) {
+	m_isBusy = true;
-			setInitialPosition();
-			continue;
-		}
 
-		if (targetPosition == currentPosition) {
+	bool isIncrementing = targetPosition > currentPosition;
-			m_hw->pause();
-			continue;
-		}
 
-		bool isIncrementing = targetPosition > currentPosition;
+	if (m_hw->step(isIncrementing)) {
+		changeCurrentPosition(isIncrementing);
+	}
 
-		if (m_hw->step(isIncrementing)) {
+	// save position to backup RTC register
-			changeCurrentPosition(isIncrementing);
-		}
-
-		// save position to backup RTC register
 #if EFI_PROD_CODE
-		saveStepperPos(m_currentPosition);
+	saveStepperPos(m_currentPosition);
 #endif
-	}
 }
 
-StepperMotor::StepperMotor() : ThreadController("stepper", PRIO_STEPPER) {}
+bool StepperMotorBase::isBusy() const {
-
+	return m_isBusy;
-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;
 }
 
 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);
+	StepperMotorBase::initialize(hardware, totalSteps);
-
-	m_hw = hardware;
 
 	Start();
 }

firmware/hw_layer/stepper.h

@@ -55,14 +55,13 @@ private:
     uint8_t m_phase = 0;
 };
 
-class StepperMotor final : private ThreadController<UTILITY_THREAD_STACK_SIZE> {
+class StepperMotorBase {
 public:
-	StepperMotor();
+	virtual void initialize(StepperHw* hardware, int totalSteps);
+	void doIteration();
 
-	void initialize(StepperHw *hardware, int totalSteps);
+	void setTargetPosition(float targetPositionSteps);
-
+	float getTargetPosition() const;
-	void setTargetPosition(int targetPosition);
-	int 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<UTILITY_THREAD_STACK_SIZE> {
+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

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));
+};

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());
+}

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 \