| Line | Branch | Decision | Exec | Source |
|---|---|---|---|---|
| 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 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 2 times.
|
2/2✓ Decision 'true' taken 3 times.
✓ Decision 'false' taken 2 times.
|
5 | if (std::abs(m_targetPosition - targetPositionSteps) >= 1) { |
| 22 | 3 | m_targetPosition = targetPositionSteps; | ||
| 23 | } | |||
| 24 | 5 | } | ||
| 25 | ||||
| 26 | ✗ | void StepperMotorBase::initialize(StepperHw *hardware, int totalSteps) { | ||
| 27 | ✗ | m_totalSteps = maxI(3, totalSteps); | ||
| 28 | ||||
| 29 | ✗ | m_hw = hardware; | ||
| 30 | ✗ | } | ||
| 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 | ✗ | void StepperHw::sleep() { } | ||
| 244 | #endif // EFI_UNIT_TEST | |||
| 245 |