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 |