From a2f26ac99b66c5cff408abce55a89cc19012e7c0 Mon Sep 17 00:00:00 2001 From: rusefi Date: Mon, 7 Sep 2020 14:41:04 -0400 Subject: [PATCH] Hellen says merge #1772 Hellen says stepper --- firmware/controllers/algo/engine.cpp | 86 +++++++++++++++++----- firmware/controllers/algo/engine.h | 26 ++++++- firmware/hw_layer/stepper.cpp | 84 ++++++++++++++++----- firmware/hw_layer/stepper.h | 18 ++++- firmware/hw_layer/stepper_dual_hbridge.cpp | 5 +- 5 files changed, 175 insertions(+), 44 deletions(-) diff --git a/firmware/controllers/algo/engine.cpp b/firmware/controllers/algo/engine.cpp index 49a72940ad..7e4c46d70b 100644 --- a/firmware/controllers/algo/engine.cpp +++ b/firmware/controllers/algo/engine.cpp @@ -162,7 +162,7 @@ void Engine::periodicSlowCallback(DECLARE_ENGINE_PARAMETER_SIGNATURE) { watchdog(); updateSlowSensors(PASS_ENGINE_PARAMETER_SIGNATURE); - checkShutdown(); + checkShutdown(PASS_ENGINE_PARAMETER_SIGNATURE); #if EFI_FSIO runFsio(PASS_ENGINE_PARAMETER_SIGNATURE); @@ -468,36 +468,83 @@ void Engine::watchdog() { #endif } -void Engine::checkShutdown() { +void Engine::checkShutdown(DECLARE_ENGINE_PARAMETER_SIGNATURE) { #if EFI_MAIN_RELAY_CONTROL - int rpm = rpmCalculator.getRpm(); + // if we are already in the "ignition_on" mode, then do nothing + if (ignitionOnTimeNt > 0) { + return; + } - /** - * Something is weird here: "below 5.0 volts on battery" what is it about? Is this about - * Frankenso powering everything while driver has already turned ignition off? or what is this condition about? - */ - const float vBattThreshold = 5.0f; - if (isValidRpm(rpm) && sensors.vBatt < vBattThreshold && stopEngineRequestTimeNt == 0) { - scheduleStopEngine(); - // todo: add stepper motor parking + // here we are in the shutdown (the ignition is off) or initial mode (after the firmware fresh start) + const efitick_t engineStopWaitTimeoutUs = 500000LL; // 0.5 sec + // in shutdown mode, we need a small cooldown time between the ignition off and on + if (stopEngineRequestTimeNt == 0 || (getTimeNowNt() - stopEngineRequestTimeNt) > US2NT(engineStopWaitTimeoutUs)) { + // if the ignition key is turned on again, + // we cancel the shutdown mode, but only if all shutdown procedures are complete + const float vBattThresholdOn = 8.0f; + if ((sensors.vBatt > vBattThresholdOn) && !isInShutdownMode(PASS_ENGINE_PARAMETER_SIGNATURE)) { + ignitionOnTimeNt = getTimeNowNt(); + stopEngineRequestTimeNt = 0; + scheduleMsg(&engineLogger, "Ingition voltage detected! Cancel the engine shutdown!"); + } } #endif /* EFI_MAIN_RELAY_CONTROL */ } -bool Engine::isInShutdownMode() const { +bool Engine::isInShutdownMode(DECLARE_ENGINE_PARAMETER_SIGNATURE) const { #if EFI_MAIN_RELAY_CONTROL - if (stopEngineRequestTimeNt == 0) // the shutdown procedure is not started + // if we are in "ignition_on" mode and not in shutdown mode + if (stopEngineRequestTimeNt == 0 && ignitionOnTimeNt > 0) { + const float vBattThresholdOff = 5.0f; + // start the shutdown process if the ignition voltage dropped low + if (sensors.vBatt <= vBattThresholdOff) { + scheduleStopEngine(); + } + } + + // we are not in the shutdown mode? + if (stopEngineRequestTimeNt == 0) { return false; - - const efitick_t engineStopWaitTimeoutNt = 5LL * 1000000LL; - // The engine is still spinning! Give it some time to stop (but wait no more than 5 secs) - if (isSpinning && (getTimeNowNt() - stopEngineRequestTimeNt) < US2NT(engineStopWaitTimeoutNt)) + } + + const efitick_t turnOffWaitTimeoutUs = 1LL * 1000000LL; + // We don't want any transients to step in, so we wait at least 1 second whatever happens. + // Also it's good to give the stepper motor some time to start moving to the initial position (or parking) + if ((getTimeNowNt() - stopEngineRequestTimeNt) < US2NT(turnOffWaitTimeoutUs)) + return true; + + const efitick_t engineSpinningWaitTimeoutUs = 5LL * 1000000LL; + // The engine is still spinning! Give it some time to stop (but wait no more than 5 secs) + if (isSpinning && (getTimeNowNt() - stopEngineRequestTimeNt) < US2NT(engineSpinningWaitTimeoutUs)) + return true; + + // The idle motor valve is still moving! Give it some time to park (but wait no more than 10 secs) + // Usually it can move to the initial 'cranking' position or zero 'parking' position. + const efitick_t idleMotorWaitTimeoutUs = 10LL * 1000000LL; + if (isIdleMotorBusy(PASS_ENGINE_PARAMETER_SIGNATURE) && (getTimeNowNt() - stopEngineRequestTimeNt) < US2NT(idleMotorWaitTimeoutUs)) return true; - // todo: add checks for stepper motor parking #endif /* EFI_MAIN_RELAY_CONTROL */ return false; } +bool Engine::isMainRelayEnabled(DECLARE_ENGINE_PARAMETER_SIGNATURE) const { +#if EFI_MAIN_RELAY_CONTROL + return enginePins.mainRelay.getLogicValue(); +#else + // if no main relay control, we assume it's always turned on + return true; +#endif /* EFI_MAIN_RELAY_CONTROL */ +} + + +float Engine::getTimeIgnitionSeconds(void) const { + // return negative if the ignition is turned off + if (ignitionOnTimeNt == 0) + return -1; + float numSeconds = (float)NT2US(getTimeNowNt() - ignitionOnTimeNt) / 1000000.0f; + return numSeconds; +} + injection_mode_e Engine::getCurrentInjectionMode(DECLARE_ENGINE_PARAMETER_SIGNATURE) { return rpmCalculator.isCranking() ? CONFIG(crankingInjectionMode) : CONFIG(injectionMode); } @@ -544,9 +591,12 @@ void Engine::periodicFastCallback(DECLARE_ENGINE_PARAMETER_SIGNATURE) { } void doScheduleStopEngine(DECLARE_ENGINE_PARAMETER_SIGNATURE) { + scheduleMsg(&engineLogger, "Starting doScheduleStopEngine"); engine->stopEngineRequestTimeNt = getTimeNowNt(); + engine->ignitionOnTimeNt = 0; // let's close injectors or else if these happen to be open right now enginePins.stopPins(); + // todo: initiate stepper motor parking } void action_s::execute() { diff --git a/firmware/controllers/algo/engine.h b/firmware/controllers/algo/engine.h index dd50347b7b..2771b363b4 100644 --- a/firmware/controllers/algo/engine.h +++ b/firmware/controllers/algo/engine.h @@ -225,6 +225,11 @@ public: efitick_t startStopStateLastPushTime = 0; int startStopStateToggleCounter = 0; + /** + * this is needed by getTimeIgnitionSeconds() and checkShutdown() + */ + efitick_t ignitionOnTimeNt = 0; + /** * This counter is incremented every time user adjusts ECU parameters online (either via rusEfi console or other * tuning software) @@ -353,14 +358,29 @@ public: /** * Needed by EFI_MAIN_RELAY_CONTROL to shut down the engine correctly. + * This method cancels shutdown if the ignition voltage is detected. */ - void checkShutdown(); - + void checkShutdown(DECLARE_ENGINE_PARAMETER_SIGNATURE); + /** * Allows to finish some long-term shutdown procedures (stepper motor parking etc.) + Called when the ignition switch is turned off (vBatt is too low). Returns true if some operations are in progress on background. */ - bool isInShutdownMode() const; + bool isInShutdownMode(DECLARE_ENGINE_PARAMETER_SIGNATURE) const; + + /** + * The stepper does not work if the main relay is turned off (it requires +12V). + * Needed by the stepper motor code to detect if it works. + */ + bool isMainRelayEnabled(DECLARE_ENGINE_PARAMETER_SIGNATURE) const; + + /** + * Needed by EFI_MAIN_RELAY_CONTROL to handle fuel pump and shutdown timings correctly. + * This method returns the number of seconds since the ignition voltage is present. + * The return value is float for more FSIO flexibility. + */ + float getTimeIgnitionSeconds(void) const; void knockLogic(float knockVolts DECLARE_ENGINE_PARAMETER_SUFFIX); void printKnockState(void); diff --git a/firmware/hw_layer/stepper.cpp b/firmware/hw_layer/stepper.cpp index 473e5f2fe4..c838453dd8 100644 --- a/firmware/hw_layer/stepper.cpp +++ b/firmware/hw_layer/stepper.cpp @@ -21,14 +21,15 @@ EXTERN_ENGINE; static Logging *logger; -static void saveStepperPos(int pos) { +void StepperMotor::saveStepperPos(int pos) { // use backup-power RTC registers to store the data #if EFI_PROD_CODE backupRamSave(BACKUP_STEPPER_POS, pos + 1); #endif + postCurrentPosition(); } -static int loadStepperPos() { +int StepperMotor::loadStepperPos() { #if EFI_PROD_CODE return (int)backupRamLoad(BACKUP_STEPPER_POS) - 1; #else @@ -36,12 +37,24 @@ static int loadStepperPos() { #endif } -void StepperMotor::ThreadTask() { - // Require hardware to be set - if (!m_hw) { - return; +void StepperMotor::changeCurrentPosition(bool positive) { + if (positive) { + m_currentPosition++; + } else { + m_currentPosition--; } + postCurrentPosition(); +} +void StepperMotor::postCurrentPosition(void) { + if (engineConfiguration->debugMode == DBG_IDLE_CONTROL) { +#if EFI_TUNER_STUDIO + tsOutputChannels.debugIntField5 = m_currentPosition; +#endif /* EFI_TUNER_STUDIO */ + } +} + +void StepperMotor::setInitialPosition(void) { // try to get saved stepper position (-1 for no data) m_currentPosition = loadStepperPos(); @@ -49,6 +62,7 @@ void StepperMotor::ThreadTask() { // first wait until at least 1 slowADC sampling is complete waitForSlowAdc(); #endif + #if EFI_SHAFT_POSITION_INPUT bool isRunning = engine->rpmCalculator.isRunning(); #else @@ -62,6 +76,7 @@ void StepperMotor::ThreadTask() { scheduleMsg(logger, "Stepper: savedStepperPos=%d forceStepperParking=%d (tps=%.2f)", m_currentPosition, (forceStepperParking ? 1 : 0), tpsPos); if (m_currentPosition < 0 || forceStepperParking) { + scheduleMsg(logger, "Stepper: starting parking..."); // reset saved value saveStepperPos(-1); @@ -75,36 +90,58 @@ 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++) { - m_hw->step(false); + if (!m_hw->step(false)) { + initialPositionSet = false; + return; + } + changeCurrentPosition(false); } // set & save zero stepper position after the parking completion m_currentPosition = 0; saveStepperPos(m_currentPosition); + scheduleMsg(logger, "Stepper: parking finished!"); } else { // The initial target position should correspond to the saved stepper position. // Idle thread starts later and sets a new target position. setTargetPosition(m_currentPosition); } + initialPositionSet = true; +} + +void StepperMotor::ThreadTask() { + // Require hardware to be set + if (!m_hw) { + return; + } + while (true) { int targetPosition = getTargetPosition(); 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(); + continue; + } + + if (!initialPositionSet) { + setInitialPosition(); + continue; + } + if (targetPosition == currentPosition) { m_hw->pause(); continue; } + bool isIncrementing = targetPosition > currentPosition; - if (isIncrementing) { - m_currentPosition++; - } else { - m_currentPosition--; + if (m_hw->step(isIncrementing)) { + changeCurrentPosition(isIncrementing); } - m_hw->step(isIncrementing); - // save position to backup RTC register #if EFI_PROD_CODE saveStepperPos(m_currentPosition); @@ -119,7 +156,14 @@ int StepperMotor::getTargetPosition() const { } void StepperMotor::setTargetPosition(int targetPosition) { - m_targetPosition = 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) { @@ -132,7 +176,11 @@ void StepDirectionStepper::setDirection(bool isIncrementing) { directionPin.setValue(isIncrementing); } -void StepDirectionStepper::pulse() { +bool StepDirectionStepper::pulse() { + // we move the motor only of it is powered from the main relay + if (!engine->isMainRelayEnabled()) + return false; + enablePin.setValue(false); // enable stepper stepPin.setValue(true); @@ -142,6 +190,8 @@ void StepDirectionStepper::pulse() { pause(); enablePin.setValue(true); // disable stepper + + return true; } void StepperHw::pause() const { @@ -152,9 +202,9 @@ void StepperHw::setReactionTime(float ms) { m_reactionTime = maxF(1, ms); } -void StepDirectionStepper::step(bool positive) { +bool StepDirectionStepper::step(bool positive) { setDirection(positive); - pulse(); + return pulse(); } void StepperMotor::initialize(StepperHw *hardware, int totalSteps, Logging *sharedLogger) { diff --git a/firmware/hw_layer/stepper.h b/firmware/hw_layer/stepper.h index b79aa431ce..34048298b1 100644 --- a/firmware/hw_layer/stepper.h +++ b/firmware/hw_layer/stepper.h @@ -14,7 +14,7 @@ class StepperHw { public: - virtual void step(bool positive) = 0; + virtual bool step(bool positive) = 0; void pause() const; protected: @@ -28,10 +28,10 @@ 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; + bool step(bool positive) override; private: - void pulse(); + bool pulse(); void setDirection(bool isIncrementing); bool m_currentDirection = false; @@ -46,7 +46,7 @@ class DualHBridgeStepper final : public StepperHw { public: void initialize(DcMotor* motorPhaseA, DcMotor* motorPhaseB, float reactionTime); - void step(bool positive) override; + bool step(bool positive) override; private: DcMotor* m_motorPhaseA = nullptr; @@ -64,15 +64,25 @@ public: void setTargetPosition(int targetPosition); int getTargetPosition() const; + bool isBusy() const; + int m_currentPosition = 0; int m_totalSteps = 0; protected: void ThreadTask() override; + void setInitialPosition(void); + + void saveStepperPos(int pos); + int loadStepperPos(); + + void changeCurrentPosition(bool positive); + void postCurrentPosition(void); private: StepperHw* m_hw = nullptr; int m_targetPosition = 0; + bool initialPositionSet = false; }; diff --git a/firmware/hw_layer/stepper_dual_hbridge.cpp b/firmware/hw_layer/stepper_dual_hbridge.cpp index b89feeac94..df473e98a9 100644 --- a/firmware/hw_layer/stepper_dual_hbridge.cpp +++ b/firmware/hw_layer/stepper_dual_hbridge.cpp @@ -30,10 +30,10 @@ void DualHBridgeStepper::initialize(DcMotor* motorPhaseA, DcMotor* motorPhaseB, m_motorPhaseB = motorPhaseB; } -void DualHBridgeStepper::step(bool positive) { +bool DualHBridgeStepper::step(bool positive) { // Check that we've been initialized if (!m_motorPhaseA || !m_motorPhaseB) { - return; + return false; } // step phase, wrapping @@ -48,6 +48,7 @@ void DualHBridgeStepper::step(bool positive) { m_motorPhaseB->set(phaseB[m_phase]); pause(); + return true; } #endif