1 /*
2 * SPDX-FileCopyrightText: Copyright (c) 2024 Carl Zeiss Meditec AG
3 * SPDX-FileCopyrightText: Copyright (c) 2024 Jilay Sandeep Pandya
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #define DT_DRV_COMPAT zephyr_gpio_stepper
8
9 #include <zephyr/drivers/gpio.h>
10 #include <zephyr/kernel.h>
11 #include <zephyr/sys_clock.h>
12 #include <zephyr/drivers/stepper.h>
13 #include <zephyr/sys/__assert.h>
14
15 #include <zephyr/logging/log.h>
16 LOG_MODULE_REGISTER(gpio_stepper_motor_controller, CONFIG_STEPPER_LOG_LEVEL);
17
18 #define MAX_MICRO_STEP_RES STEPPER_MICRO_STEP_2
19 #define NUM_CONTROL_PINS 4
20
21 static const uint8_t
22 half_step_lookup_table[NUM_CONTROL_PINS * MAX_MICRO_STEP_RES][NUM_CONTROL_PINS] = {
23 {1u, 1u, 0u, 0u}, {0u, 1u, 0u, 0u}, {0u, 1u, 1u, 0u}, {0u, 0u, 1u, 0u},
24 {0u, 0u, 1u, 1u}, {0u, 0u, 0u, 1u}, {1u, 0u, 0u, 1u}, {1u, 0u, 0u, 0u}};
25
26 struct gpio_stepper_config {
27 const struct gpio_dt_spec *control_pins;
28 bool invert_direction;
29 };
30
31 struct gpio_stepper_data {
32 const struct device *dev;
33 struct k_spinlock lock;
34 enum stepper_direction direction;
35 enum stepper_run_mode run_mode;
36 uint8_t step_gap;
37 uint8_t coil_charge;
38 struct k_work_delayable stepper_dwork;
39 int32_t actual_position;
40 uint64_t delay_in_ns;
41 int32_t step_count;
42 bool is_enabled;
43 stepper_event_callback_t callback;
44 void *event_cb_user_data;
45 };
46
stepper_motor_set_coil_charge(const struct device * dev)47 static int stepper_motor_set_coil_charge(const struct device *dev)
48 {
49 struct gpio_stepper_data *data = dev->data;
50 const struct gpio_stepper_config *config = dev->config;
51
52 for (int i = 0; i < NUM_CONTROL_PINS; i++) {
53 (void)gpio_pin_set_dt(&config->control_pins[i],
54 half_step_lookup_table[data->coil_charge][i]);
55 }
56 return 0;
57 }
58
increment_coil_charge(const struct device * dev)59 static void increment_coil_charge(const struct device *dev)
60 {
61 struct gpio_stepper_data *data = dev->data;
62
63 if (data->coil_charge == NUM_CONTROL_PINS * MAX_MICRO_STEP_RES - data->step_gap) {
64 data->coil_charge = 0;
65 } else {
66 data->coil_charge = data->coil_charge + data->step_gap;
67 }
68 }
69
decrement_coil_charge(const struct device * dev)70 static void decrement_coil_charge(const struct device *dev)
71 {
72 struct gpio_stepper_data *data = dev->data;
73
74 if (data->coil_charge == 0) {
75 data->coil_charge = NUM_CONTROL_PINS * MAX_MICRO_STEP_RES - data->step_gap;
76 } else {
77 data->coil_charge = data->coil_charge - data->step_gap;
78 }
79 }
80
energize_coils(const struct device * dev,const bool energized)81 static int energize_coils(const struct device *dev, const bool energized)
82 {
83 const struct gpio_stepper_config *config = dev->config;
84
85 for (int i = 0; i < NUM_CONTROL_PINS; i++) {
86 const int err = gpio_pin_set_dt(&config->control_pins[i], energized);
87
88 if (err != 0) {
89 LOG_ERR("Failed to power down coil %d", i);
90 return err;
91 }
92 }
93 return 0;
94 }
95
update_coil_charge(const struct device * dev)96 static void update_coil_charge(const struct device *dev)
97 {
98 const struct gpio_stepper_config *config = dev->config;
99 struct gpio_stepper_data *data = dev->data;
100
101 if (data->direction == STEPPER_DIRECTION_POSITIVE) {
102 config->invert_direction ? decrement_coil_charge(dev) : increment_coil_charge(dev);
103 data->actual_position++;
104 } else if (data->direction == STEPPER_DIRECTION_NEGATIVE) {
105 config->invert_direction ? increment_coil_charge(dev) : decrement_coil_charge(dev);
106 data->actual_position--;
107 }
108 }
109
update_remaining_steps(struct gpio_stepper_data * data)110 static void update_remaining_steps(struct gpio_stepper_data *data)
111 {
112 if (data->step_count > 0) {
113 data->step_count--;
114 (void)k_work_reschedule(&data->stepper_dwork, K_NSEC(data->delay_in_ns));
115 } else if (data->step_count < 0) {
116 data->step_count++;
117 (void)k_work_reschedule(&data->stepper_dwork, K_NSEC(data->delay_in_ns));
118 } else {
119 if (!data->callback) {
120 LOG_WRN_ONCE("No callback set");
121 return;
122 }
123 data->callback(data->dev, STEPPER_EVENT_STEPS_COMPLETED, data->event_cb_user_data);
124 }
125 }
126
update_direction_from_step_count(const struct device * dev)127 static void update_direction_from_step_count(const struct device *dev)
128 {
129 struct gpio_stepper_data *data = dev->data;
130
131 if (data->step_count > 0) {
132 data->direction = STEPPER_DIRECTION_POSITIVE;
133 } else if (data->step_count < 0) {
134 data->direction = STEPPER_DIRECTION_NEGATIVE;
135 } else {
136 LOG_ERR("Step count is zero");
137 }
138 }
139
position_mode_task(const struct device * dev)140 static void position_mode_task(const struct device *dev)
141 {
142 struct gpio_stepper_data *data = dev->data;
143
144 if (data->step_count) {
145 (void)stepper_motor_set_coil_charge(dev);
146 update_coil_charge(dev);
147 }
148 update_remaining_steps(dev->data);
149 }
150
velocity_mode_task(const struct device * dev)151 static void velocity_mode_task(const struct device *dev)
152 {
153 struct gpio_stepper_data *data = dev->data;
154
155 (void)stepper_motor_set_coil_charge(dev);
156 update_coil_charge(dev);
157 (void)k_work_reschedule(&data->stepper_dwork, K_NSEC(data->delay_in_ns));
158 }
159
stepper_work_step_handler(struct k_work * work)160 static void stepper_work_step_handler(struct k_work *work)
161 {
162 struct k_work_delayable *dwork = k_work_delayable_from_work(work);
163 struct gpio_stepper_data *data =
164 CONTAINER_OF(dwork, struct gpio_stepper_data, stepper_dwork);
165
166 K_SPINLOCK(&data->lock) {
167 switch (data->run_mode) {
168 case STEPPER_RUN_MODE_POSITION:
169 position_mode_task(data->dev);
170 break;
171 case STEPPER_RUN_MODE_VELOCITY:
172 velocity_mode_task(data->dev);
173 break;
174 default:
175 LOG_WRN("Unsupported run mode %d", data->run_mode);
176 break;
177 }
178 }
179 }
180
gpio_stepper_move_by(const struct device * dev,int32_t micro_steps)181 static int gpio_stepper_move_by(const struct device *dev, int32_t micro_steps)
182 {
183 struct gpio_stepper_data *data = dev->data;
184
185 if (!data->is_enabled) {
186 LOG_ERR("Stepper motor is not enabled");
187 return -ECANCELED;
188 }
189
190 if (data->delay_in_ns == 0) {
191 LOG_ERR("Step interval not set or invalid step interval set");
192 return -EINVAL;
193 }
194 K_SPINLOCK(&data->lock) {
195 data->run_mode = STEPPER_RUN_MODE_POSITION;
196 data->step_count = micro_steps;
197 update_direction_from_step_count(dev);
198 (void)k_work_reschedule(&data->stepper_dwork, K_NO_WAIT);
199 }
200 return 0;
201 }
202
gpio_stepper_set_reference_position(const struct device * dev,int32_t position)203 static int gpio_stepper_set_reference_position(const struct device *dev, int32_t position)
204 {
205 struct gpio_stepper_data *data = dev->data;
206
207 K_SPINLOCK(&data->lock) {
208 data->actual_position = position;
209 }
210 return 0;
211 }
212
gpio_stepper_get_actual_position(const struct device * dev,int32_t * position)213 static int gpio_stepper_get_actual_position(const struct device *dev, int32_t *position)
214 {
215 struct gpio_stepper_data *data = dev->data;
216
217 K_SPINLOCK(&data->lock) {
218 *position = data->actual_position;
219 }
220 return 0;
221 }
222
gpio_stepper_move_to(const struct device * dev,int32_t micro_steps)223 static int gpio_stepper_move_to(const struct device *dev, int32_t micro_steps)
224 {
225 struct gpio_stepper_data *data = dev->data;
226
227 if (!data->is_enabled) {
228 LOG_ERR("Stepper motor is not enabled");
229 return -ECANCELED;
230 }
231
232 if (data->delay_in_ns == 0) {
233 LOG_ERR("Step interval not set or invalid step interval set");
234 return -EINVAL;
235 }
236 K_SPINLOCK(&data->lock) {
237 data->run_mode = STEPPER_RUN_MODE_POSITION;
238 data->step_count = micro_steps - data->actual_position;
239 update_direction_from_step_count(dev);
240 (void)k_work_reschedule(&data->stepper_dwork, K_NO_WAIT);
241 }
242 return 0;
243 }
244
gpio_stepper_is_moving(const struct device * dev,bool * is_moving)245 static int gpio_stepper_is_moving(const struct device *dev, bool *is_moving)
246 {
247 struct gpio_stepper_data *data = dev->data;
248
249 *is_moving = k_work_delayable_is_pending(&data->stepper_dwork);
250 LOG_DBG("Motor is %s moving", *is_moving ? "" : "not");
251 return 0;
252 }
253
gpio_stepper_set_microstep_interval(const struct device * dev,uint64_t microstep_interval_ns)254 static int gpio_stepper_set_microstep_interval(const struct device *dev,
255 uint64_t microstep_interval_ns)
256 {
257 struct gpio_stepper_data *data = dev->data;
258
259 if (microstep_interval_ns == 0) {
260 LOG_ERR("Step interval is invalid.");
261 return -EINVAL;
262 }
263
264 K_SPINLOCK(&data->lock) {
265 data->delay_in_ns = microstep_interval_ns;
266 }
267 LOG_DBG("Setting Motor step interval to %llu", microstep_interval_ns);
268 return 0;
269 }
270
gpio_stepper_run(const struct device * dev,const enum stepper_direction direction)271 static int gpio_stepper_run(const struct device *dev, const enum stepper_direction direction)
272 {
273 struct gpio_stepper_data *data = dev->data;
274
275 if (!data->is_enabled) {
276 LOG_ERR("Stepper motor is not enabled");
277 return -ECANCELED;
278 }
279
280 K_SPINLOCK(&data->lock) {
281 data->run_mode = STEPPER_RUN_MODE_VELOCITY;
282 data->direction = direction;
283 (void)k_work_reschedule(&data->stepper_dwork, K_NO_WAIT);
284 }
285 return 0;
286 }
287
gpio_stepper_set_micro_step_res(const struct device * dev,enum stepper_micro_step_resolution micro_step_res)288 static int gpio_stepper_set_micro_step_res(const struct device *dev,
289 enum stepper_micro_step_resolution micro_step_res)
290 {
291 struct gpio_stepper_data *data = dev->data;
292 int err = 0;
293
294 K_SPINLOCK(&data->lock) {
295 switch (micro_step_res) {
296 case STEPPER_MICRO_STEP_1:
297 case STEPPER_MICRO_STEP_2:
298 data->step_gap = MAX_MICRO_STEP_RES >> (micro_step_res - 1);
299 break;
300 default:
301 LOG_ERR("Unsupported micro step resolution %d", micro_step_res);
302 err = -ENOTSUP;
303 }
304 }
305 return err;
306 }
307
gpio_stepper_get_micro_step_res(const struct device * dev,enum stepper_micro_step_resolution * micro_step_res)308 static int gpio_stepper_get_micro_step_res(const struct device *dev,
309 enum stepper_micro_step_resolution *micro_step_res)
310 {
311 struct gpio_stepper_data *data = dev->data;
312 *micro_step_res = MAX_MICRO_STEP_RES >> (data->step_gap - 1);
313 return 0;
314 }
315
gpio_stepper_set_event_callback(const struct device * dev,stepper_event_callback_t callback,void * user_data)316 static int gpio_stepper_set_event_callback(const struct device *dev,
317 stepper_event_callback_t callback, void *user_data)
318 {
319 struct gpio_stepper_data *data = dev->data;
320
321 K_SPINLOCK(&data->lock) {
322 data->callback = callback;
323 }
324 data->event_cb_user_data = user_data;
325 return 0;
326 }
327
gpio_stepper_enable(const struct device * dev,bool enable)328 static int gpio_stepper_enable(const struct device *dev, bool enable)
329 {
330 struct gpio_stepper_data *data = dev->data;
331 int err;
332
333 if (data->is_enabled == enable) {
334 return 0;
335 }
336
337 K_SPINLOCK(&data->lock) {
338 data->is_enabled = enable;
339
340 if (enable) {
341 err = energize_coils(dev, true);
342 } else {
343 (void)k_work_cancel_delayable(&data->stepper_dwork);
344 err = energize_coils(dev, false);
345 }
346 }
347 return err;
348 }
349
gpio_stepper_stop(const struct device * dev)350 static int gpio_stepper_stop(const struct device *dev)
351 {
352 struct gpio_stepper_data *data = dev->data;
353 int err;
354
355 K_SPINLOCK(&data->lock) {
356 (void)k_work_cancel_delayable(&data->stepper_dwork);
357 err = energize_coils(dev, true);
358
359 if (data->callback && !err) {
360 data->callback(data->dev, STEPPER_EVENT_STOPPED, data->event_cb_user_data);
361 }
362 }
363 return err;
364 }
365
gpio_stepper_init(const struct device * dev)366 static int gpio_stepper_init(const struct device *dev)
367 {
368 struct gpio_stepper_data *data = dev->data;
369 const struct gpio_stepper_config *config = dev->config;
370
371 data->dev = dev;
372 LOG_DBG("Initializing %s gpio_stepper with %d pin", dev->name, NUM_CONTROL_PINS);
373 for (uint8_t n_pin = 0; n_pin < NUM_CONTROL_PINS; n_pin++) {
374 (void)gpio_pin_configure_dt(&config->control_pins[n_pin], GPIO_OUTPUT_INACTIVE);
375 }
376 k_work_init_delayable(&data->stepper_dwork, stepper_work_step_handler);
377 return 0;
378 }
379
380 static DEVICE_API(stepper, gpio_stepper_api) = {
381 .enable = gpio_stepper_enable,
382 .set_micro_step_res = gpio_stepper_set_micro_step_res,
383 .get_micro_step_res = gpio_stepper_get_micro_step_res,
384 .set_reference_position = gpio_stepper_set_reference_position,
385 .get_actual_position = gpio_stepper_get_actual_position,
386 .set_event_callback = gpio_stepper_set_event_callback,
387 .set_microstep_interval = gpio_stepper_set_microstep_interval,
388 .move_by = gpio_stepper_move_by,
389 .move_to = gpio_stepper_move_to,
390 .run = gpio_stepper_run,
391 .stop = gpio_stepper_stop,
392 .is_moving = gpio_stepper_is_moving,
393 };
394
395 #define GPIO_STEPPER_DEFINE(inst) \
396 static const struct gpio_dt_spec gpio_stepper_motor_control_pins_##inst[] = { \
397 DT_INST_FOREACH_PROP_ELEM_SEP(inst, gpios, GPIO_DT_SPEC_GET_BY_IDX, (,)), \
398 }; \
399 BUILD_ASSERT(ARRAY_SIZE(gpio_stepper_motor_control_pins_##inst) == 4, \
400 "gpio_stepper_controller driver currently supports only 4 wire configuration"); \
401 static const struct gpio_stepper_config gpio_stepper_config_##inst = { \
402 .invert_direction = DT_INST_PROP(inst, invert_direction), \
403 .control_pins = gpio_stepper_motor_control_pins_##inst}; \
404 static struct gpio_stepper_data gpio_stepper_data_##inst = { \
405 .step_gap = MAX_MICRO_STEP_RES >> (DT_INST_PROP(inst, micro_step_res) - 1), \
406 }; \
407 BUILD_ASSERT(DT_INST_PROP(inst, micro_step_res) <= STEPPER_MICRO_STEP_2, \
408 "gpio_stepper_controller driver supports up to 2 micro steps"); \
409 DEVICE_DT_INST_DEFINE(inst, gpio_stepper_init, NULL, &gpio_stepper_data_##inst, \
410 &gpio_stepper_config_##inst, POST_KERNEL, \
411 CONFIG_STEPPER_INIT_PRIORITY, &gpio_stepper_api);
412
413 DT_INST_FOREACH_STATUS_OKAY(GPIO_STEPPER_DEFINE)
414
