Skip to content

Commit 1014b4c

Browse files
jilaypandyajilaypandya
committed
drivers: stepper: introduce trapezoidal ramp control
Introduce rudimentary trapezoidal ramp control using following paper: https://www.boost.org/doc/libs/1_81_0/libs/safe_numerics/example/stepper-motor.pdf Signed-off-by: Jilay Pandya <[email protected]>
1 parent 6463c68 commit 1014b4c

File tree

18 files changed

+794
-23
lines changed

18 files changed

+794
-23
lines changed

‎drivers/stepper/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,10 @@ zephyr_syscall_header(${ZEPHYR_BASE}/include/zephyr/drivers/stepper.h)
77
add_subdirectory_ifdef(CONFIG_STEPPER_ADI_TMC adi_tmc)
88
add_subdirectory_ifdef(CONFIG_STEPPER_ALLEGRO allegro)
99
add_subdirectory_ifdef(CONFIG_STEPPER_TI ti)
10+
# zephyr-keep-sorted-stop
11+
12+
# zephyr-keep-sorted-start
13+
add_subdirectory_ifdef(CONFIG_STEPPER_RAMP ramp)
1014
add_subdirectory_ifdef(CONFIG_STEP_DIR_STEPPER step_dir)
1115
# zephyr-keep-sorted-stop
1216

‎drivers/stepper/Kconfig‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@ config STEPPER_SHELL
2626

2727
comment "Stepper Driver Common"
2828

29+
rsource "ramp/Kconfig"
2930
rsource "step_dir/Kconfig"
3031

3132
comment "Stepper Drivers"

‎drivers/stepper/gpio_stepper_controller.c‎

Lines changed: 150 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
/*
22
* SPDX-FileCopyrightText: Copyright (c) 2024 Carl Zeiss Meditec AG
3-
* SPDX-FileCopyrightText: Copyright (c) 2024 Jilay Sandeep Pandya
3+
* SPDX-FileCopyrightText: Copyright (c) 2025 Jilay Sandeep Pandya
44
* SPDX-License-Identifier: Apache-2.0
55
*/
66

@@ -10,8 +10,14 @@
1010
#include <zephyr/kernel.h>
1111
#include <zephyr/sys_clock.h>
1212
#include <zephyr/drivers/stepper.h>
13+
#include "stepper_common.h"
1314
#include <zephyr/sys/__assert.h>
1415

16+
#ifdef CONFIG_STEPPER_RAMP
17+
#include <stdlib.h>
18+
#include "ramp/ramp.h"
19+
#endif
20+
1521
#include <zephyr/logging/log.h>
1622
LOG_MODULE_REGISTER(gpio_stepper_motor_controller, CONFIG_STEPPER_LOG_LEVEL);
1723

@@ -26,6 +32,10 @@ static const uint8_t
2632
struct gpio_stepper_config {
2733
const struct gpio_dt_spec *control_pins;
2834
bool invert_direction;
35+
#ifdef CONFIG_STEPPER_RAMP
36+
const struct stepper_ramp_api *ramp_api;
37+
const struct stepper_ramp_config ramp_config;
38+
#endif
2939
};
3040

3141
struct gpio_stepper_data {
@@ -42,6 +52,9 @@ struct gpio_stepper_data {
4252
bool is_enabled;
4353
stepper_event_callback_t callback;
4454
void *event_cb_user_data;
55+
#ifdef CONFIG_STEPPER_RAMP
56+
struct stepper_ramp_common ramp_common;
57+
#endif
4558
};
4659

4760
static int stepper_motor_set_coil_charge(const struct device *dev)
@@ -98,6 +111,24 @@ static void update_coil_charge(const struct device *dev)
98111
const struct gpio_stepper_config *config = dev->config;
99112
struct gpio_stepper_data *data = dev->data;
100113

114+
#ifdef CONFIG_STEPPER_RAMP
115+
if (data->ramp_common.ramp_runtime_data.current_ramp_state ==
116+
STEPPER_RAMP_STATE_PRE_DECELERATION) {
117+
if (data->direction == STEPPER_DIRECTION_NEGATIVE) {
118+
config->invert_direction ? decrement_coil_charge(dev)
119+
: increment_coil_charge(dev);
120+
data->actual_position++;
121+
return;
122+
}
123+
if (data->direction == STEPPER_DIRECTION_POSITIVE) {
124+
config->invert_direction ? increment_coil_charge(dev)
125+
: decrement_coil_charge(dev);
126+
data->actual_position--;
127+
return;
128+
}
129+
}
130+
#endif
131+
101132
if (data->direction == STEPPER_DIRECTION_POSITIVE) {
102133
config->invert_direction ? decrement_coil_charge(dev) : increment_coil_charge(dev);
103134
data->actual_position++;
@@ -111,16 +142,30 @@ static void update_remaining_steps(const struct device *dev)
111142
{
112143
struct gpio_stepper_data *data = dev->data;
113144

145+
#ifdef CONFIG_STEPPER_RAMP
146+
147+
if (data->ramp_common.ramp_runtime_data.current_ramp_state ==
148+
STEPPER_RAMP_STATE_PRE_DECELERATION) {
149+
if (data->step_count > 0) {
150+
data->step_count++;
151+
} else {
152+
data->step_count--;
153+
}
154+
return;
155+
}
156+
157+
#endif
114158
if (data->step_count > 0) {
115159
data->step_count--;
116160
} else if (data->step_count < 0) {
117161
data->step_count++;
118162
}
119163
}
120164

121-
static void update_direction_from_step_count(const struct device *dev)
165+
static bool update_direction_from_step_count(const struct device *dev)
122166
{
123167
struct gpio_stepper_data *data = dev->data;
168+
enum stepper_direction direction = data->direction;
124169

125170
if (data->step_count > 0) {
126171
data->direction = STEPPER_DIRECTION_POSITIVE;
@@ -129,6 +174,10 @@ static void update_direction_from_step_count(const struct device *dev)
129174
} else {
130175
LOG_ERR("Step count is zero");
131176
}
177+
if (data->direction != direction) {
178+
return true;
179+
}
180+
return false;
132181
}
133182

134183
static void position_mode_task(const struct device *dev)
@@ -139,6 +188,12 @@ static void position_mode_task(const struct device *dev)
139188
(void)stepper_motor_set_coil_charge(dev);
140189
update_coil_charge(dev);
141190
if (data->step_count) {
191+
#ifdef CONFIG_STEPPER_RAMP
192+
const struct gpio_stepper_config *config = dev->config;
193+
194+
data->delay_in_ns = config->ramp_api->get_next_step_interval(
195+
&data->ramp_common, data->delay_in_ns, STEPPER_RUN_MODE_POSITION);
196+
#endif
142197
(void)k_work_reschedule(&data->stepper_dwork, K_NSEC(data->delay_in_ns));
143198
} else {
144199
if (data->callback) {
@@ -155,6 +210,14 @@ static void velocity_mode_task(const struct device *dev)
155210

156211
(void)stepper_motor_set_coil_charge(dev);
157212
update_coil_charge(dev);
213+
214+
#ifdef CONFIG_STEPPER_RAMP
215+
const struct gpio_stepper_config *config = dev->config;
216+
217+
data->delay_in_ns = config->ramp_api->get_next_step_interval(
218+
&data->ramp_common, data->delay_in_ns, STEPPER_RUN_MODE_VELOCITY);
219+
#endif
220+
158221
(void)k_work_reschedule(&data->stepper_dwork, K_NSEC(data->delay_in_ns));
159222
}
160223

@@ -179,6 +242,23 @@ static void stepper_work_step_handler(struct k_work *work)
179242
}
180243
}
181244

245+
static bool is_step_timing_valid(const struct device *dev)
246+
{
247+
struct gpio_stepper_data *data = dev->data;
248+
#ifdef CONFIG_STEPPER_RAMP
249+
250+
if (data->ramp_common.ramp_profile.max_velocity > 0) {
251+
return true;
252+
}
253+
return false;
254+
#else
255+
if (data->delay_in_ns > 0) {
256+
return true;
257+
}
258+
return false;
259+
#endif
260+
}
261+
182262
static int gpio_stepper_move_by(const struct device *dev, int32_t micro_steps)
183263
{
184264
struct gpio_stepper_data *data = dev->data;
@@ -188,14 +268,36 @@ static int gpio_stepper_move_by(const struct device *dev, int32_t micro_steps)
188268
return -ECANCELED;
189269
}
190270

191-
if (data->delay_in_ns == 0) {
192-
LOG_ERR("Step interval not set or invalid step interval set");
271+
if (!is_step_timing_valid(dev)) {
193272
return -EINVAL;
194273
}
274+
275+
if (micro_steps == 0) {
276+
LOG_WRN("Step count is zero");
277+
return 0;
278+
}
279+
195280
K_SPINLOCK(&data->lock) {
196281
data->run_mode = STEPPER_RUN_MODE_POSITION;
197282
data->step_count = micro_steps;
198-
update_direction_from_step_count(dev);
283+
bool is_dir_changed = update_direction_from_step_count(dev);
284+
#ifdef CONFIG_STEPPER_RAMP
285+
const struct gpio_stepper_config *config = dev->config;
286+
uint32_t steps_to_move = abs(micro_steps);
287+
bool is_stepper_moving = k_work_delayable_is_pending(&data->stepper_dwork);
288+
289+
data->ramp_common.ramp_runtime_data.is_stepper_moving = is_stepper_moving;
290+
data->ramp_common.ramp_runtime_data.is_stepper_dir_changed = is_dir_changed;
291+
config->ramp_api->reset_ramp_runtime_data(&config->ramp_config, &data->ramp_common,
292+
steps_to_move);
293+
if (!is_stepper_moving) {
294+
data->delay_in_ns = config->ramp_api->calculate_start_interval(
295+
data->ramp_common.ramp_profile.acceleration);
296+
}
297+
config->ramp_api->recalculate_ramp(&data->ramp_common, steps_to_move);
298+
#else
299+
ARG_UNUSED(is_dir_changed);
300+
#endif
199301
(void)k_work_reschedule(&data->stepper_dwork, K_NO_WAIT);
200302
}
201303
return 0;
@@ -258,6 +360,23 @@ static int gpio_stepper_set_microstep_interval(const struct device *dev,
258360
return 0;
259361
}
260362

363+
#ifdef CONFIG_STEPPER_RAMP
364+
365+
static int gpio_stepper_set_ramp_profile(const struct device *dev,
366+
const struct stepper_ramp_profile *const ramp_profile)
367+
{
368+
struct gpio_stepper_data *data = dev->data;
369+
370+
K_SPINLOCK(&data->lock) {
371+
data->ramp_common.ramp_profile.acceleration = ramp_profile->acceleration;
372+
data->ramp_common.ramp_profile.max_velocity = ramp_profile->max_velocity;
373+
data->ramp_common.ramp_profile.deceleration = ramp_profile->deceleration;
374+
}
375+
return 0;
376+
}
377+
378+
#endif
379+
261380
static int gpio_stepper_run(const struct device *dev, const enum stepper_direction direction)
262381
{
263382
struct gpio_stepper_data *data = dev->data;
@@ -269,7 +388,23 @@ static int gpio_stepper_run(const struct device *dev, const enum stepper_directi
269388

270389
K_SPINLOCK(&data->lock) {
271390
data->run_mode = STEPPER_RUN_MODE_VELOCITY;
391+
bool is_dir_changed = direction != data->direction;
272392
data->direction = direction;
393+
#ifdef CONFIG_STEPPER_RAMP
394+
const struct gpio_stepper_config *config = dev->config;
395+
const bool is_stepper_moving = k_work_delayable_is_pending(&data->stepper_dwork);
396+
397+
data->ramp_common.ramp_runtime_data.is_stepper_moving = is_stepper_moving;
398+
data->ramp_common.ramp_runtime_data.is_stepper_dir_changed = is_dir_changed;
399+
config->ramp_api->reset_ramp_runtime_data(&config->ramp_config, &data->ramp_common,
400+
UINT32_MAX);
401+
if (!is_stepper_moving) {
402+
data->delay_in_ns = config->ramp_api->calculate_start_interval(
403+
data->ramp_common.ramp_profile.acceleration);
404+
}
405+
#else
406+
ARG_UNUSED(is_dir_changed);
407+
#endif
273408
(void)k_work_reschedule(&data->stepper_dwork, K_NO_WAIT);
274409
}
275410
return 0;
@@ -393,20 +528,27 @@ static DEVICE_API(stepper, gpio_stepper_api) = {
393528
.run = gpio_stepper_run,
394529
.stop = gpio_stepper_stop,
395530
.is_moving = gpio_stepper_is_moving,
531+
IF_ENABLED(CONFIG_STEPPER_RAMP, (.set_ramp_profile = gpio_stepper_set_ramp_profile,))
396532
};
397533

398534
#define GPIO_STEPPER_DEFINE(inst) \
399535
static const struct gpio_dt_spec gpio_stepper_motor_control_pins_##inst[] = { \
400536
DT_INST_FOREACH_PROP_ELEM_SEP(inst, gpios, GPIO_DT_SPEC_GET_BY_IDX, (,)), \
401537
}; \
402538
BUILD_ASSERT(ARRAY_SIZE(gpio_stepper_motor_control_pins_##inst) == 4, \
403-
"gpio_stepper_controller driver currently supports only 4 wire configuration"); \
539+
"gpio_stepper_controller driver currently supports only 4 wire configuration"); \
404540
static const struct gpio_stepper_config gpio_stepper_config_##inst = { \
405541
.invert_direction = DT_INST_PROP(inst, invert_direction), \
406-
.control_pins = gpio_stepper_motor_control_pins_##inst}; \
542+
.control_pins = gpio_stepper_motor_control_pins_##inst, \
543+
IF_ENABLED(CONFIG_STEPPER_RAMP, \
544+
(.ramp_config.pre_deceleration_steps = DT_INST_PROP(inst, pre_decel_steps), \
545+
.ramp_api = RAMP_DT_SPEC_GET_API(inst))) }; \
407546
static struct gpio_stepper_data gpio_stepper_data_##inst = { \
408547
.step_gap = MAX_MICRO_STEP_RES >> (DT_INST_PROP(inst, micro_step_res) - 1), \
409-
}; \
548+
IF_ENABLED(CONFIG_STEPPER_RAMP, ( \
549+
.ramp_common.ramp_profile.acceleration = DT_INST_PROP(inst, acceleration), \
550+
.ramp_common.ramp_profile.deceleration = DT_INST_PROP(inst, deceleration), \
551+
.ramp_common.ramp_profile.max_velocity = DT_INST_PROP(inst, max_speed),)) }; \
410552
BUILD_ASSERT(DT_INST_PROP(inst, micro_step_res) <= STEPPER_MICRO_STEP_2, \
411553
"gpio_stepper_controller driver supports up to 2 micro steps"); \
412554
DEVICE_DT_INST_DEFINE(inst, gpio_stepper_init, NULL, &gpio_stepper_data_##inst, \

‎drivers/stepper/ramp/CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
# SPDX-FileCopyrightText: Copyright (c) 2025 Jilay Sandeep Pandya
2+
# SPDX-License-Identifier: Apache-2.0
3+
4+
zephyr_library()
5+
6+
zephyr_library_sources_ifdef(CONFIG_STEPPER_RAMP_TRAPEZOIDAL ramp_trapezoidal.c)

‎drivers/stepper/ramp/Kconfig‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
# SPDX-FileCopyrightText: Copyright (c) 2025 Jilay Sandeep Pandya
2+
# SPDX-License-Identifier: Apache-2.0
3+
4+
menuconfig STEPPER_RAMP
5+
bool "Ramp control"
6+
help
7+
Enable ramp control for stepper drivers.
8+
9+
if STEPPER_RAMP
10+
11+
config STEPPER_RAMP_TRAPEZOIDAL
12+
bool "Trapezoidal ramp control"
13+
help
14+
Enable trapezoidal ramp control for stepper drivers.
15+
The trapezoidal ramp control is implemented as a state machine
16+
and can be used with any stepper driver.
17+
18+
endif

0 commit comments

Comments
 (0)