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 (std::abs(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=%lums", 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=%lums", 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             :         // stepper requires +12V
     130             :         if (!isIgnVoltage()) {
     131             :           initialPositionSet = false;
     132             :                 m_hw->pause();
     133             :                 return;
     134             :         }
     135             : 
     136             :         if (!initialPositionSet) {
     137             :                 setInitialPosition();
     138             :                 return;
     139             :         }
     140             : 
     141             :         if (targetPosition == currentPosition) {
     142             :                 m_hw->sleep();
     143             :                 m_isBusy = false;
     144             :                 return;
     145             :         }
     146             : 
     147             :         m_isBusy = true;
     148             : 
     149             :         bool isIncrementing = targetPosition > currentPosition;
     150             : 
     151             :         if (m_hw->step(isIncrementing)) {
     152             :                 changeCurrentPosition(isIncrementing);
     153             :         }
     154             : 
     155             :         // save position to backup RTC register
     156             : #if EFI_PROD_CODE
     157             :         saveStepperPos(m_currentPosition);
     158             : #endif
     159             : }
     160             : 
     161             : bool StepperMotorBase::isBusy() const {
     162             :         return m_isBusy;
     163             : }
     164             : 
     165             : void StepDirectionStepper::setDirection(bool isIncrementing) {
     166             :         if (isIncrementing != m_currentDirection) {
     167             :                 // compensate stepper motor inertia
     168             :                 pause();
     169             :                 m_currentDirection = isIncrementing;
     170             :         }
     171             : 
     172             :         directionPin.setValue(isIncrementing);
     173             : }
     174             : 
     175             : bool StepDirectionStepper::pulse() {
     176             :         // we move the motor only of it is powered from the main relay
     177             :         if (!engine->isMainRelayEnabled())
     178             :                 return false;
     179             : 
     180             :         enablePin.setValue(false); // enable stepper
     181             : 
     182             :         stepPin.setValue(true);
     183             :         pause();
     184             : 
     185             :         stepPin.setValue(false);
     186             :         pause();
     187             : 
     188             :         enablePin.setValue(true); // disable stepper
     189             : 
     190             :         return true;
     191             : }
     192             : 
     193             : void StepperHw::sleep() {
     194             :         pause();
     195             : }
     196             : 
     197             : void StepperHw::pause(int divisor) const {
     198             :         // currently we can't sleep less than 1ms (see #3214)
     199             :         chThdSleepMicroseconds(maxI(MS2US(1), (int)(MS2US(m_reactionTime)) / divisor));
     200             : }
     201             : 
     202             : void StepperHw::setReactionTime(float ms) {
     203             :         m_reactionTime = std::max(1.0f, ms);
     204             : }
     205             : 
     206             : bool StepDirectionStepper::step(bool positive) {
     207             :         setDirection(positive);
     208             :         return pulse();
     209             : }
     210             : 
     211             : void StepperMotor::initialize(StepperHw *hardware, int totalSteps) {
     212             :         StepperMotorBase::initialize(hardware, totalSteps);
     213             : 
     214             :         start();
     215             : }
     216             : 
     217             : 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) {
     218             :         if (!isBrainPinValid(p_stepPin) || !isBrainPinValid(p_directionPin)) {
     219             :                 return;
     220             :         }
     221             : 
     222             :         setReactionTime(reactionTime);
     223             : 
     224             :         directionPinMode = p_directionPinMode;
     225             :         directionPin.initPin("Stepper DIR", p_directionPin, directionPinMode);
     226             : 
     227             :         stepPinMode = OM_DEFAULT;       // todo: do we need configurable stepPinMode?
     228             :         stepPin.initPin("Stepper step", p_stepPin, stepPinMode);
     229             : 
     230             :         enablePinMode = p_enablePinMode;
     231             :         enablePin.initPin("Stepper EN", p_enablePin, enablePinMode);
     232             : 
     233             :         // All pins must be 0 for correct hardware startup (e.g. stepper auto-disabling circuit etc.).
     234             :         enablePin.setValue(true); // disable stepper
     235             :         stepPin.setValue(false);
     236             :         directionPin.setValue(false);
     237             :         m_currentDirection = false;
     238             : }
     239             : 
     240             : #endif
     241             : 
     242             : #if EFI_UNIT_TEST
     243           0 : void StepperHw::sleep() { }
     244             : #endif // EFI_UNIT_TEST