From 6a3d2123706a2d8e45dd736f78696d928ee34237 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 (cherry picked from commit a2f26ac99b66c5cff408abce55a89cc19012e7c0) --- firmware/controllers/algo/engine.cpp | 86 ++++++++++++++++++++++------ firmware/controllers/algo/engine.h | 26 ++++++++- firmware/hw_layer/stepper.cpp | 84 +++++++++++++++++++++------ firmware/hw_layer/stepper.h | 36 ++++++++++-- 4 files changed, 188 insertions(+), 44 deletions(-) diff --git a/firmware/controllers/algo/engine.cpp b/firmware/controllers/algo/engine.cpp index 2d76d49615..54af1d64ab 100644 --- a/firmware/controllers/algo/engine.cpp +++ b/firmware/controllers/algo/engine.cpp @@ -133,7 +133,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); @@ -414,36 +414,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(PASS_ENGINE_PARAMETER_SIGNATURE) ? CONFIG(crankingInjectionMode) : CONFIG(injectionMode); } @@ -487,9 +534,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 b11c384c58..bf78699f3a 100644 --- a/firmware/controllers/algo/engine.h +++ b/firmware/controllers/algo/engine.h @@ -173,6 +173,11 @@ public: */ efitick_t stopEngineRequestTimeNt = 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) @@ -295,14 +300,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; monitoring_timestamps_s m; diff --git a/firmware/hw_layer/stepper.cpp b/firmware/hw_layer/stepper.cpp index d1bceab345..b955ade7ad 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(PASS_ENGINE_PARAMETER_SIGNATURE); #else @@ -61,6 +75,7 @@ void StepperMotor::ThreadTask() { scheduleMsg(logger, "Stepper: savedStepperPos=%d forceStepperParking=%d (tps=%.2f)", m_currentPosition, (forceStepperParking ? 1 : 0), getTPS(PASS_ENGINE_PARAMETER_SIGNATURE)); if (m_currentPosition < 0 || forceStepperParking) { + scheduleMsg(logger, "Stepper: starting parking..."); // reset saved value saveStepperPos(-1); @@ -74,36 +89,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); @@ -118,7 +155,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) { @@ -131,7 +175,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); @@ -141,6 +189,8 @@ void StepDirectionStepper::pulse() { pause(); enablePin.setValue(true); // disable stepper + + return true; } void StepperHw::pause() const { @@ -151,9 +201,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 de4d356367..91ebcffc36 100644 --- a/firmware/hw_layer/stepper.h +++ b/firmware/hw_layer/stepper.h @@ -4,8 +4,8 @@ * @date Dec 24, 2014 * @author Andrey Belomutskiy, (c) 2012-2020 */ -#ifndef STEPPER_H_ -#define STEPPER_H_ + +#pragma once #include "global.h" #include "efi_gpio.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; @@ -40,6 +40,21 @@ private: pin_output_mode_e directionPinMode, stepPinMode, enablePinMode; }; +class DcMotor; + +class DualHBridgeStepper final : public StepperHw { +public: + void initialize(DcMotor* motorPhaseA, DcMotor* motorPhaseB, float reactionTime); + + bool step(bool positive) override; + +private: + DcMotor* m_motorPhaseA = nullptr; + DcMotor* m_motorPhaseB = nullptr; + + uint8_t m_phase = 0; +}; + class StepperMotor final : private ThreadController { public: StepperMotor(); @@ -49,16 +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; }; -#endif /* STEPPER_H_ */