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=%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 : // 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