Line data    Source code

       1             : /**
       2             :  * @file        stepper.cpp
       3             :  *
       4             :  * http://rusefi.com/wiki/index.php?title=Hardware:Stepper_motor
       5             :  *
       6             :  * @date Dec 24, 2014
       7             :  * @author Andrey Belomutskiy, (c) 2012-2020
       8             :  */
       9             : 
      10             : #include "pch.h"
      11             : 
      12             : #include "stepper.h"
      13             : 
      14           5 : float StepperMotorBase::getTargetPosition() const {
      15           5 :         return m_targetPosition;
      16             : }
      17             : 
      18           5 : void StepperMotorBase::setTargetPosition(float targetPositionSteps) {
      19             :         // When the IAC position value change is insignificant (lower than this threshold), leave the poor valve alone
      20             :         // When we get a larger change, actually update the target stepper position
      21           5 :         if (absF(m_targetPosition - targetPositionSteps) >= 1) {
      22           3 :                 m_targetPosition = targetPositionSteps;
      23             :         }
      24           5 : }
      25             : 
      26           0 : void StepperMotorBase::initialize(StepperHw *hardware, int totalSteps) {
      27           0 :         m_totalSteps = maxI(3, totalSteps);
      28             : 
      29           0 :         m_hw = hardware;
      30           0 : }
      31             : 
      32             : // todo: EFI_STEPPER macro
      33             : #if EFI_PROD_CODE || EFI_SIMULATOR
      34             : 
      35             : void StepperMotorBase::saveStepperPos(int pos) {
      36             :         // use backup-power RTC registers to store the data
      37             : #if EFI_PROD_CODE && EFI_BACKUP_SRAM
      38             :         backupRamSave(backup_ram_e::StepperPosition, pos + 1);
      39             : #endif
      40             :         postCurrentPosition();
      41             : }
      42             : 
      43             : int StepperMotorBase::loadStepperPos() {
      44             : #if EFI_PROD_CODE && EFI_BACKUP_SRAM
      45             :         return (int)backupRamLoad(backup_ram_e::StepperPosition) - 1;
      46             : #else
      47             :         return 0;
      48             : #endif
      49             : }
      50             : 
      51             : void StepperMotorBase::changeCurrentPosition(bool positive) {
      52             :         if (positive) {
      53             :                 m_currentPosition++;
      54             :         } else {
      55             :                 m_currentPosition--;
      56             :         }
      57             :         postCurrentPosition();
      58             : }
      59             : 
      60             : void StepperMotorBase::postCurrentPosition() {
      61             :         if (engineConfiguration->debugMode == DBG_STEPPER_IDLE_CONTROL) {
      62             : #if EFI_TUNER_STUDIO
      63             :                 engine->outputChannels.debugIntField5 = m_currentPosition;
      64             : #endif /* EFI_TUNER_STUDIO */
      65             :         }
      66             : }
      67             : 
      68             : void StepperMotorBase::setInitialPosition() {
      69             :         // try to get saved stepper position (-1 for no data)
      70             :         m_currentPosition = loadStepperPos();
      71             : 
      72             : #if HAL_USE_ADC
      73             :         // first wait until at least 1 slowADC sampling is complete
      74             :         waitForSlowAdc();
      75             : #endif
      76             : 
      77             : #if EFI_SHAFT_POSITION_INPUT
      78             :         bool isRunning = engine->rpmCalculator.isRunning();
      79             : #else
      80             :         bool isRunning = false;
      81             : #endif /* EFI_SHAFT_POSITION_INPUT */
      82             :         // now check if stepper motor re-initialization is requested - if the throttle pedal is pressed at startup
      83             :         auto tpsPos = Sensor::getOrZero(SensorType::DriverThrottleIntent);
      84             :         bool forceStepperParking = !isRunning && tpsPos > STEPPER_PARKING_TPS;
      85             :         if (engineConfiguration->stepperForceParkingEveryRestart)
      86             :                 forceStepperParking = true;
      87             :         efiPrintf("Stepper: savedStepperPos=%d forceStepperParking=%d (tps=%.2f)", m_currentPosition, (forceStepperParking ? 1 : 0), tpsPos);
      88             : 
      89             :         if (m_currentPosition < 0 || forceStepperParking) {
      90             :                 efiPrintf("Stepper: starting parking time=%dms", getTimeNowMs());
      91             :                 // reset saved value
      92             :                 saveStepperPos(-1);
      93             : 
      94             :                 /**
      95             :                  * let's park the motor in a known position to begin with
      96             :                  *
      97             :                  * I believe it's safer to retract the valve for parking - at least on a bench I've seen valves
      98             :                  * disassembling themselves while pushing too far out.
      99             :                  *
     100             :                  * Add extra steps to compensate step skipping by some old motors.
     101             :                  */
     102             :                 int numParkingSteps = (int)efiRound((1.0f + (float)engineConfiguration->stepperParkingExtraSteps / PERCENT_MULT) * m_totalSteps, 1.0f);
     103             :                 for (int i = 0; i < numParkingSteps; i++) {
     104             :                         if (!m_hw->step(false)) {
     105             :                                 initialPositionSet = false;
     106             :                                 return;
     107             :                         }
     108             :                         changeCurrentPosition(false);
     109             :                 }
     110             : 
     111             :                 // set & save zero stepper position after the parking completion
     112             :                 m_currentPosition = 0;
     113             :                 saveStepperPos(m_currentPosition);
     114             :                 // todo: is this a slow operation on the start-up path?
     115             :                 efiPrintf("Stepper: parking finished time=%dms", getTimeNowMs());
     116             :         } else {
     117             :                 // The initial target position should correspond to the saved stepper position.
     118             :                 // Idle thread starts later and sets a new target position.
     119             :                 setTargetPosition(m_currentPosition);
     120             :         }
     121             : 
     122             :         initialPositionSet = true;
     123             : }
     124             : 
     125             : void StepperMotorBase::doIteration() {
     126             :         int targetPosition = efiRound(getTargetPosition(), 1);
     127             :         int currentPosition = m_currentPosition;
     128             : 
     129             :         // the stepper does not work if the main relay is turned off (it requires +12V)
     130             :         if (!engine->isMainRelayEnabled()) {
     131             :                 m_hw->pause();
     132             :                 return;
     133             :         }
     134             : 
     135             :         if (!initialPositionSet) {
     136             :                 setInitialPosition();
     137             :                 return;
     138             :         }
     139             : 
     140             :         if (targetPosition == currentPosition) {
     141             :                 m_hw->sleep();
     142             :                 m_isBusy = false;
     143             :                 return;
     144             :         }
     145             : 
     146             :         m_isBusy = true;
     147             : 
     148             :         bool isIncrementing = targetPosition > currentPosition;
     149             : 
     150             :         if (m_hw->step(isIncrementing)) {
     151             :                 changeCurrentPosition(isIncrementing);
     152             :         }
     153             : 
     154             :         // save position to backup RTC register
     155             : #if EFI_PROD_CODE
     156             :         saveStepperPos(m_currentPosition);
     157             : #endif
     158             : }
     159             : 
     160             : bool StepperMotorBase::isBusy() const {
     161             :         return m_isBusy;
     162             : }
     163             : 
     164             : void StepDirectionStepper::setDirection(bool isIncrementing) {
     165             :         if (isIncrementing != m_currentDirection) {
     166             :                 // compensate stepper motor inertia
     167             :                 pause();
     168             :                 m_currentDirection = isIncrementing;
     169             :         }
     170             : 
     171             :         directionPin.setValue(isIncrementing);
     172             : }
     173             : 
     174             : bool StepDirectionStepper::pulse() {
     175             :         // we move the motor only of it is powered from the main relay
     176             :         if (!engine->isMainRelayEnabled())
     177             :                 return false;
     178             : 
     179             :         enablePin.setValue(false); // enable stepper
     180             : 
     181             :         stepPin.setValue(true);
     182             :         pause();
     183             : 
     184             :         stepPin.setValue(false);
     185             :         pause();
     186             : 
     187             :         enablePin.setValue(true); // disable stepper
     188             : 
     189             :         return true;
     190             : }
     191             : 
     192             : void StepperHw::sleep() {
     193             :         pause();
     194             : }
     195             : 
     196             : void StepperHw::pause(int divisor) const {
     197             :         // currently we can't sleep less than 1ms (see #3214)
     198             :         chThdSleepMicroseconds(maxI(MS2US(1), (int)(MS2US(m_reactionTime)) / divisor));
     199             : }
     200             : 
     201             : void StepperHw::setReactionTime(float ms) {
     202             :         m_reactionTime = maxF(1, ms);
     203             : }
     204             : 
     205             : bool StepDirectionStepper::step(bool positive) {
     206             :         setDirection(positive);
     207             :         return pulse();
     208             : }
     209             : 
     210             : void StepperMotor::initialize(StepperHw *hardware, int totalSteps) {
     211             :         StepperMotorBase::initialize(hardware, totalSteps);
     212             : 
     213             :         start();
     214             : }
     215             : 
     216             : void StepDirectionStepper::initialize(brain_pin_e p_stepPin, brain_pin_e p_directionPin, pin_output_mode_e p_directionPinMode, float reactionTime, brain_pin_e p_enablePin, pin_output_mode_e p_enablePinMode) {
     217             :         if (!isBrainPinValid(p_stepPin) || !isBrainPinValid(p_directionPin)) {
     218             :                 return;
     219             :         }
     220             : 
     221             :         setReactionTime(reactionTime);
     222             : 
     223             :         directionPinMode = p_directionPinMode;
     224             :         directionPin.initPin("Stepper DIR", p_directionPin, directionPinMode);
     225             : 
     226             :         stepPinMode = OM_DEFAULT;       // todo: do we need configurable stepPinMode?
     227             :         stepPin.initPin("Stepper step", p_stepPin, stepPinMode);
     228             : 
     229             :         enablePinMode = p_enablePinMode;
     230             :         enablePin.initPin("Stepper EN", p_enablePin, enablePinMode);
     231             : 
     232             :         // All pins must be 0 for correct hardware startup (e.g. stepper auto-disabling circuit etc.).
     233             :         enablePin.setValue(true); // disable stepper
     234             :         stepPin.setValue(false);
     235             :         directionPin.setValue(false);
     236             :         m_currentDirection = false;
     237             : }
     238             : 
     239             : #endif
     240             : 
     241             : #if EFI_UNIT_TEST
     242           0 : void StepperHw::sleep() { }
     243             : #endif // EFI_UNIT_TEST