From 9859deffda738606077bcb10e49b22bb825de082 Mon Sep 17 00:00:00 2001 From: Jilay Pandya Date: Tue, 8 Apr 2025 09:03:14 +0200 Subject: [PATCH 1/2] 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 --- drivers/stepper/CMakeLists.txt | 4 + drivers/stepper/Kconfig | 1 + drivers/stepper/gpio_stepper_controller.c | 156 +++++++++- drivers/stepper/ramp/CMakeLists.txt | 6 + drivers/stepper/ramp/Kconfig | 18 ++ drivers/stepper/ramp/ramp.h | 173 +++++++++++ drivers/stepper/ramp/ramp_trapezoidal.c | 280 ++++++++++++++++++ .../step_dir/step_dir_stepper_common.h | 2 +- drivers/stepper/stepper_common.h | 21 ++ dts/bindings/stepper/ramp-parameters.yaml | 42 +++ dts/bindings/stepper/zephyr,gpio-stepper.yaml | 2 +- include/zephyr/drivers/stepper.h | 59 +++- include/zephyr/dt-bindings/stepper/ramp.h | 14 + samples/drivers/stepper/generic/Kconfig | 12 + .../generic/boards/nucleo_g071rb.overlay | 7 + samples/drivers/stepper/generic/prj.conf | 2 + samples/drivers/stepper/generic/src/main.c | 6 + 17 files changed, 782 insertions(+), 23 deletions(-) create mode 100644 drivers/stepper/ramp/CMakeLists.txt create mode 100644 drivers/stepper/ramp/Kconfig create mode 100644 drivers/stepper/ramp/ramp.h create mode 100644 drivers/stepper/ramp/ramp_trapezoidal.c create mode 100644 drivers/stepper/stepper_common.h create mode 100644 dts/bindings/stepper/ramp-parameters.yaml create mode 100644 include/zephyr/dt-bindings/stepper/ramp.h diff --git a/drivers/stepper/CMakeLists.txt b/drivers/stepper/CMakeLists.txt index 1cda468fac48..f2426db50efe 100644 --- a/drivers/stepper/CMakeLists.txt +++ b/drivers/stepper/CMakeLists.txt @@ -7,6 +7,10 @@ zephyr_syscall_header(${ZEPHYR_BASE}/include/zephyr/drivers/stepper.h) add_subdirectory_ifdef(CONFIG_STEPPER_ADI_TMC adi_tmc) add_subdirectory_ifdef(CONFIG_STEPPER_ALLEGRO allegro) add_subdirectory_ifdef(CONFIG_STEPPER_TI ti) +# zephyr-keep-sorted-stop + +# zephyr-keep-sorted-start +add_subdirectory_ifdef(CONFIG_STEPPER_RAMP ramp) add_subdirectory_ifdef(CONFIG_STEP_DIR_STEPPER step_dir) # zephyr-keep-sorted-stop diff --git a/drivers/stepper/Kconfig b/drivers/stepper/Kconfig index 075c9130e33f..2d0b6e749e77 100644 --- a/drivers/stepper/Kconfig +++ b/drivers/stepper/Kconfig @@ -26,6 +26,7 @@ config STEPPER_SHELL comment "Stepper Driver Common" +rsource "ramp/Kconfig" rsource "step_dir/Kconfig" comment "Stepper Drivers" diff --git a/drivers/stepper/gpio_stepper_controller.c b/drivers/stepper/gpio_stepper_controller.c index 331d2413ec1d..08d4b0abd3d3 100644 --- a/drivers/stepper/gpio_stepper_controller.c +++ b/drivers/stepper/gpio_stepper_controller.c @@ -1,6 +1,6 @@ /* * SPDX-FileCopyrightText: Copyright (c) 2024 Carl Zeiss Meditec AG - * SPDX-FileCopyrightText: Copyright (c) 2024 Jilay Sandeep Pandya + * SPDX-FileCopyrightText: Copyright (c) 2025 Jilay Sandeep Pandya * SPDX-License-Identifier: Apache-2.0 */ @@ -10,8 +10,14 @@ #include #include #include +#include "stepper_common.h" #include +#ifdef CONFIG_STEPPER_RAMP +#include +#include "ramp/ramp.h" +#endif + #include LOG_MODULE_REGISTER(gpio_stepper_motor_controller, CONFIG_STEPPER_LOG_LEVEL); @@ -26,6 +32,10 @@ static const uint8_t struct gpio_stepper_config { const struct gpio_dt_spec *control_pins; bool invert_direction; +#ifdef CONFIG_STEPPER_RAMP + const struct stepper_ramp_api *ramp_api; + const struct stepper_ramp_config ramp_config; +#endif }; struct gpio_stepper_data { @@ -42,6 +52,9 @@ struct gpio_stepper_data { bool is_enabled; stepper_event_callback_t callback; void *event_cb_user_data; +#ifdef CONFIG_STEPPER_RAMP + struct stepper_ramp_common ramp_common; +#endif }; static int stepper_motor_set_coil_charge(const struct device *dev) @@ -98,6 +111,22 @@ static void update_coil_charge(const struct device *dev) const struct gpio_stepper_config *config = dev->config; struct gpio_stepper_data *data = dev->data; +#ifdef CONFIG_STEPPER_RAMP + if (data->ramp_common.ramp_runtime_data.current_ramp_state == + STEPPER_RAMP_STATE_PRE_DECELERATION) { + if (data->direction == STEPPER_DIRECTION_NEGATIVE) { + config->invert_direction ? decrement_coil_charge(dev) + : increment_coil_charge(dev); + data->actual_position++; + } else if (data->direction == STEPPER_DIRECTION_POSITIVE) { + config->invert_direction ? increment_coil_charge(dev) + : decrement_coil_charge(dev); + data->actual_position--; + } + return; + } +#endif + if (data->direction == STEPPER_DIRECTION_POSITIVE) { config->invert_direction ? decrement_coil_charge(dev) : increment_coil_charge(dev); data->actual_position++; @@ -111,6 +140,19 @@ static void update_remaining_steps(const struct device *dev) { struct gpio_stepper_data *data = dev->data; +#ifdef CONFIG_STEPPER_RAMP + + if (data->ramp_common.ramp_runtime_data.current_ramp_state == + STEPPER_RAMP_STATE_PRE_DECELERATION) { + if (data->step_count > 0) { + data->step_count++; + } else { + data->step_count--; + } + return; + } + +#endif if (data->step_count > 0) { data->step_count--; } else if (data->step_count < 0) { @@ -118,9 +160,10 @@ static void update_remaining_steps(const struct device *dev) } } -static void update_direction_from_step_count(const struct device *dev) +static bool update_direction_from_step_count(const struct device *dev) { struct gpio_stepper_data *data = dev->data; + enum stepper_direction direction = data->direction; if (data->step_count > 0) { data->direction = STEPPER_DIRECTION_POSITIVE; @@ -129,6 +172,10 @@ static void update_direction_from_step_count(const struct device *dev) } else { LOG_ERR("Step count is zero"); } + if (data->direction != direction) { + return true; + } + return false; } static void position_mode_task(const struct device *dev) @@ -139,6 +186,12 @@ static void position_mode_task(const struct device *dev) (void)stepper_motor_set_coil_charge(dev); update_coil_charge(dev); if (data->step_count) { +#ifdef CONFIG_STEPPER_RAMP + const struct gpio_stepper_config *config = dev->config; + + data->delay_in_ns = config->ramp_api->get_next_step_interval( + &data->ramp_common, data->delay_in_ns, STEPPER_RUN_MODE_POSITION); +#endif (void)k_work_reschedule(&data->stepper_dwork, K_NSEC(data->delay_in_ns)); } else { if (data->callback) { @@ -155,6 +208,14 @@ static void velocity_mode_task(const struct device *dev) (void)stepper_motor_set_coil_charge(dev); update_coil_charge(dev); + +#ifdef CONFIG_STEPPER_RAMP + const struct gpio_stepper_config *config = dev->config; + + data->delay_in_ns = config->ramp_api->get_next_step_interval( + &data->ramp_common, data->delay_in_ns, STEPPER_RUN_MODE_VELOCITY); +#endif + (void)k_work_reschedule(&data->stepper_dwork, K_NSEC(data->delay_in_ns)); } @@ -179,6 +240,23 @@ static void stepper_work_step_handler(struct k_work *work) } } +static bool is_step_timing_valid(const struct device *dev) +{ + struct gpio_stepper_data *data = dev->data; +#ifdef CONFIG_STEPPER_RAMP + + if (data->ramp_common.ramp_profile.max_velocity > 0) { + return true; + } + return false; +#else + if (data->delay_in_ns > 0) { + return true; + } + return false; +#endif +} + static int gpio_stepper_move_by(const struct device *dev, int32_t micro_steps) { struct gpio_stepper_data *data = dev->data; @@ -188,14 +266,36 @@ static int gpio_stepper_move_by(const struct device *dev, int32_t micro_steps) return -ECANCELED; } - if (data->delay_in_ns == 0) { - LOG_ERR("Step interval not set or invalid step interval set"); + if (!is_step_timing_valid(dev)) { return -EINVAL; } + + if (micro_steps == 0) { + LOG_WRN("Step count is zero"); + return 0; + } + K_SPINLOCK(&data->lock) { data->run_mode = STEPPER_RUN_MODE_POSITION; data->step_count = micro_steps; - update_direction_from_step_count(dev); + bool is_dir_changed = update_direction_from_step_count(dev); +#ifdef CONFIG_STEPPER_RAMP + const struct gpio_stepper_config *config = dev->config; + uint32_t steps_to_move = abs(micro_steps); + bool is_stepper_moving = k_work_delayable_is_pending(&data->stepper_dwork); + + data->ramp_common.ramp_runtime_data.is_stepper_moving = is_stepper_moving; + data->ramp_common.ramp_runtime_data.is_stepper_dir_changed = is_dir_changed; + config->ramp_api->reset_ramp_runtime_data(&config->ramp_config, &data->ramp_common, + steps_to_move); + if (!is_stepper_moving) { + data->delay_in_ns = config->ramp_api->calculate_start_interval( + data->ramp_common.ramp_profile.acceleration); + } + config->ramp_api->recalculate_ramp(&data->ramp_common, steps_to_move); +#else + ARG_UNUSED(is_dir_changed); +#endif (void)k_work_reschedule(&data->stepper_dwork, K_NO_WAIT); } return 0; @@ -258,6 +358,23 @@ static int gpio_stepper_set_microstep_interval(const struct device *dev, return 0; } +#ifdef CONFIG_STEPPER_RAMP + +static int gpio_stepper_set_ramp_profile(const struct device *dev, + const struct stepper_ramp_profile *const ramp_profile) +{ + struct gpio_stepper_data *data = dev->data; + + K_SPINLOCK(&data->lock) { + data->ramp_common.ramp_profile.acceleration = ramp_profile->acceleration; + data->ramp_common.ramp_profile.max_velocity = ramp_profile->max_velocity; + data->ramp_common.ramp_profile.deceleration = ramp_profile->deceleration; + } + return 0; +} + +#endif + static int gpio_stepper_run(const struct device *dev, const enum stepper_direction direction) { struct gpio_stepper_data *data = dev->data; @@ -269,7 +386,23 @@ static int gpio_stepper_run(const struct device *dev, const enum stepper_directi K_SPINLOCK(&data->lock) { data->run_mode = STEPPER_RUN_MODE_VELOCITY; + bool is_dir_changed = direction != data->direction; data->direction = direction; +#ifdef CONFIG_STEPPER_RAMP + const struct gpio_stepper_config *config = dev->config; + const bool is_stepper_moving = k_work_delayable_is_pending(&data->stepper_dwork); + + data->ramp_common.ramp_runtime_data.is_stepper_moving = is_stepper_moving; + data->ramp_common.ramp_runtime_data.is_stepper_dir_changed = is_dir_changed; + config->ramp_api->reset_ramp_runtime_data(&config->ramp_config, &data->ramp_common, + UINT32_MAX); + if (!is_stepper_moving) { + data->delay_in_ns = config->ramp_api->calculate_start_interval( + data->ramp_common.ramp_profile.acceleration); + } +#else + ARG_UNUSED(is_dir_changed); +#endif (void)k_work_reschedule(&data->stepper_dwork, K_NO_WAIT); } return 0; @@ -393,6 +526,7 @@ static DEVICE_API(stepper, gpio_stepper_api) = { .run = gpio_stepper_run, .stop = gpio_stepper_stop, .is_moving = gpio_stepper_is_moving, + IF_ENABLED(CONFIG_STEPPER_RAMP, (.set_ramp_profile = gpio_stepper_set_ramp_profile,)) }; #define GPIO_STEPPER_DEFINE(inst) \ @@ -400,13 +534,19 @@ static DEVICE_API(stepper, gpio_stepper_api) = { DT_INST_FOREACH_PROP_ELEM_SEP(inst, gpios, GPIO_DT_SPEC_GET_BY_IDX, (,)), \ }; \ BUILD_ASSERT(ARRAY_SIZE(gpio_stepper_motor_control_pins_##inst) == 4, \ - "gpio_stepper_controller driver currently supports only 4 wire configuration"); \ + "gpio_stepper_controller driver currently supports only 4 wire configuration"); \ static const struct gpio_stepper_config gpio_stepper_config_##inst = { \ .invert_direction = DT_INST_PROP(inst, invert_direction), \ - .control_pins = gpio_stepper_motor_control_pins_##inst}; \ + .control_pins = gpio_stepper_motor_control_pins_##inst, \ + IF_ENABLED(CONFIG_STEPPER_RAMP, \ + (.ramp_config.pre_deceleration_steps = DT_INST_PROP(inst, pre_decel_steps), \ + .ramp_api = RAMP_DT_SPEC_GET_API(inst))) }; \ static struct gpio_stepper_data gpio_stepper_data_##inst = { \ .step_gap = MAX_MICRO_STEP_RES >> (DT_INST_PROP(inst, micro_step_res) - 1), \ - }; \ + IF_ENABLED(CONFIG_STEPPER_RAMP, ( \ + .ramp_common.ramp_profile.acceleration = DT_INST_PROP(inst, acceleration), \ + .ramp_common.ramp_profile.deceleration = DT_INST_PROP(inst, deceleration), \ + .ramp_common.ramp_profile.max_velocity = DT_INST_PROP(inst, max_speed),)) }; \ BUILD_ASSERT(DT_INST_PROP(inst, micro_step_res) <= STEPPER_MICRO_STEP_2, \ "gpio_stepper_controller driver supports up to 2 micro steps"); \ DEVICE_DT_INST_DEFINE(inst, gpio_stepper_init, NULL, &gpio_stepper_data_##inst, \ diff --git a/drivers/stepper/ramp/CMakeLists.txt b/drivers/stepper/ramp/CMakeLists.txt new file mode 100644 index 000000000000..bc0c6dfb8986 --- /dev/null +++ b/drivers/stepper/ramp/CMakeLists.txt @@ -0,0 +1,6 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 Jilay Sandeep Pandya +# SPDX-License-Identifier: Apache-2.0 + +zephyr_library() + +zephyr_library_sources_ifdef(CONFIG_STEPPER_RAMP_TRAPEZOIDAL ramp_trapezoidal.c) diff --git a/drivers/stepper/ramp/Kconfig b/drivers/stepper/ramp/Kconfig new file mode 100644 index 000000000000..14b7e86164e2 --- /dev/null +++ b/drivers/stepper/ramp/Kconfig @@ -0,0 +1,18 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 Jilay Sandeep Pandya +# SPDX-License-Identifier: Apache-2.0 + +menuconfig STEPPER_RAMP + bool "Ramp control" + help + Enable ramp control for stepper drivers. + +if STEPPER_RAMP + +config STEPPER_RAMP_TRAPEZOIDAL + bool "Trapezoidal ramp control" + help + Enable trapezoidal ramp control for stepper drivers. + The trapezoidal ramp control is implemented as a state machine + and can be used with any stepper driver. + +endif diff --git a/drivers/stepper/ramp/ramp.h b/drivers/stepper/ramp/ramp.h new file mode 100644 index 000000000000..1875f9291fbb --- /dev/null +++ b/drivers/stepper/ramp/ramp.h @@ -0,0 +1,173 @@ +/* + * SPDX-FileCopyrightText: Copyright (c) 2025 Jilay Sandeep Pandya + * SPDX-License-Identifier: Apache-2.0 + */ + +/** + * @file ramp.h + * @brief stepper motor ramping algorithm definitions + * + * This header defines the data structures and APIs for stepper motor velocity + * ramping, allowing acceleration and deceleration profiles (like trapezoidal). + * It provides the foundation for implementing different ramping algorithms + * that can be selected based on application requirements. + */ + +#ifndef ZEPHYR_DRIVERS_STEPPER_RAMP_H_ +#define ZEPHYR_DRIVERS_STEPPER_RAMP_H_ + +#include +#include +#include "../stepper_common.h" + +/** + * @brief Distance profile for stepper motor movement + * + * Defines the distances (in microsteps) for each phase of movement: + * acceleration, constant speed, and deceleration. + */ +struct stepper_ramp_distance_profile { + /** Distance covered during acceleration phase (µsteps) */ + uint32_t acceleration; + /** Distance covered during constant speed phase (µsteps) */ + uint32_t const_speed; + /** Distance covered during deceleration phase (µsteps) */ + uint32_t deceleration; +}; + +/** + * @brief Ramp controller state machine states + * + * Defines the possible states of the ramp controller during operation. + */ +enum stepper_ramp_state { + /** Motor is not moving */ + STEPPER_RAMP_STATE_NOT_MOVING, + /** Motor is accelerating */ + STEPPER_RAMP_STATE_ACCELERATION, + /** Motor is running at constant speed */ + STEPPER_RAMP_STATE_CONSTANT_SPEED, + /** Motor is decelerating */ + STEPPER_RAMP_STATE_DECELERATION, + /** Motor is prematurely decelerating before changing direction */ + STEPPER_RAMP_STATE_PRE_DECELERATION, +}; + +/** + * @brief Runtime data for the ramp controller + * + * Contains the current state and position information needed by + * the ramp controller during operation. + */ +struct stepper_ramp_runtime_data { + /** Actual Ramp position in µsteps */ + uint32_t ramp_actual_position; + /** Target position in µsteps */ + uint32_t ramp_target_position; + /** Steps to be used while forced deceleration */ + uint32_t pre_deceleration_steps; + /** Minimum step interval (in ns) threshold for stopping ramping */ + uint64_t ramp_stop_step_interval_threshold_in_ns; + /** Flag indicating if the stepper direction has changed */ + bool is_stepper_dir_changed; + /** Flag indicating if the stepper is currently moving */ + bool is_stepper_moving; + /** Current state of the ramp controller */ + enum stepper_ramp_state current_ramp_state; +}; + +/** + * @brief Configuration parameters for the ramp controller + * + * Contains const configuration values used by the ramp controller. + */ +struct stepper_ramp_config { + const uint32_t pre_deceleration_steps; +}; + +/** + * @brief Combined data structure for ramp controller + * + * Aggregates all data needed for ramp operation including runtime data, + * distance profile, and ramp profile parameters. + */ +struct stepper_ramp_common { + /** Runtime state data */ + struct stepper_ramp_runtime_data ramp_runtime_data; + /** Distance profile information */ + struct stepper_ramp_distance_profile ramp_distance_profile; + /** Ramp profile parameters (acceleration, max velocity) */ + struct stepper_ramp_profile ramp_profile; +}; + +/** + * @brief Reset the ramp controller data + * + * @param config Pointer to ramp configuration + * @param ramp_common Pointer to common ramp data + * @param is_stepper_dir_changed Flag indicating if direction changed + * @param is_stepper_moving Flag indicating if stepper is currently moving + * @param steps_to_move Number of steps to move + */ +typedef void (*stepper_ramp_reset_ramp_runtime_data_t)(const struct stepper_ramp_config *config, + struct stepper_ramp_common *ramp_common, + const uint32_t steps_to_move); + +/** + * @brief Calculate the next step interval + * + * Calculates the time interval to the next step based on the current + * state of the ramp controller. + * + * @param ramp_common Pointer to common ramp data + * @param current_step_interval_in_ns Current step interval in nanoseconds + * @param run_mode Current run mode (move to target or continuous run) + * @return Next step interval in nanoseconds + */ +typedef uint64_t (*stepper_ramp_get_next_step_interval_t)( + struct stepper_ramp_common *ramp_common, const uint64_t current_step_interval_in_ns, + enum stepper_run_mode run_mode); + +/** + * @brief Recalculate the ramp profile + * + * Updates the ramp distance profile based on the total steps to move. + * + * @param ramp_common Pointer to common ramp data + * @param total_steps_to_move Total steps to move + */ +typedef void (*stepper_ramp_recalculate_ramp_t)(struct stepper_ramp_common *ramp_common, + uint32_t total_steps_to_move); + +/** + * @brief Calculate the starting step interval + * + * Determines the initial step interval based on acceleration parameters. + * + * @param acceleration Acceleration value in steps/s² + * @return Initial step interval in nanoseconds + */ +typedef uint64_t (*stepper_calculate_start_interval_t)(const uint32_t acceleration); + +/** + * @brief API structure for ramp implementations + * + * Collection of function pointers that define the interface for + * specific ramp algorithm implementations. + */ +struct stepper_ramp_api { + stepper_ramp_reset_ramp_runtime_data_t reset_ramp_runtime_data; + stepper_calculate_start_interval_t calculate_start_interval; + stepper_ramp_recalculate_ramp_t recalculate_ramp; + stepper_ramp_get_next_step_interval_t get_next_step_interval; +}; + +#define RAMP_DT_SPEC_GET_API(node_id) \ + (COND_CODE_1(DT_ENUM_HAS_VALUE(DT_DRV_INST(node_id), \ + ramp_type, STEPPER_RAMP_TYPE_TRAPEZOIDAL), (&trapezoidal_ramp_api), (NULL))) + +#ifdef CONFIG_STEPPER_RAMP_TRAPEZOIDAL +extern const struct stepper_ramp_api trapezoidal_ramp_api; +#endif + +#endif diff --git a/drivers/stepper/ramp/ramp_trapezoidal.c b/drivers/stepper/ramp/ramp_trapezoidal.c new file mode 100644 index 000000000000..215a6fcac2f5 --- /dev/null +++ b/drivers/stepper/ramp/ramp_trapezoidal.c @@ -0,0 +1,280 @@ +/* + * SPDX-FileCopyrightText: Copyright (c) 2025 Jilay Sandeep Pandya + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include "ramp.h" + +#include +LOG_MODULE_REGISTER(ramp_control, CONFIG_STEPPER_LOG_LEVEL); + +static bool is_dir_changed_while_in_motion(const bool is_stepper_moving, + const bool is_stepper_dir_changed) +{ + if (is_stepper_moving && is_stepper_dir_changed) { + LOG_DBG("Direction changed while in motion"); + return true; + } + return false; +} + +static uint64_t calc_approx_step_interval(const uint64_t current_step_interval, + const uint32_t ramp_position, + enum stepper_ramp_state state) +{ + uint64_t new_step_interval = current_step_interval; + + switch (state) { + case STEPPER_RAMP_STATE_ACCELERATION: + new_step_interval = current_step_interval - + (2 * current_step_interval) / (4 * ramp_position + 1); + break; + case STEPPER_RAMP_STATE_PRE_DECELERATION: + case STEPPER_RAMP_STATE_DECELERATION: + new_step_interval = current_step_interval + + (2 * current_step_interval) / (4 * ramp_position + 1); + break; + default: + break; + } + return new_step_interval; +} + +/** + * @brief Simple recursive implementation of an integer square root + * + * copied from samples/kernel/metairq_dispatch/src/main.c 0b90fd5 + */ +static uint32_t isqrt(uint64_t n) +{ + if (n <= 1) { + return (uint32_t)n; + } + + uint64_t x = n; + uint64_t y = (x + 1) / 2; + + while (y < x) { + x = y; + y = (x + n / x) / 2; + } + return (uint32_t)x; +} + +static uint64_t trapezpoidal_calculate_start_interval(uint32_t acceleration) +{ + if (acceleration == 0) { + LOG_ERR("Error: Acceleration cannot be zero"); + return UINT64_MAX; + } + + /* the value of (2 * factor * factor) may not overflow uint64_t but at the same time be as + * large as possible to ensure maximal possible precision of isqrt + */ + const uint64_t factor = 3037000499ULL; + + /* Calculate the start velocity based on the acceleration + * + * Using the formula: t = f * sqrt(2 * d / a) + * where f = counter frequency, d = 1 step, a = acceleration + * + * This value will be used in approximation as described in AVR446 section 2.3.1 + * The approximation introduces an error which has to be corrected by multiplying first + * interval by factor of 0.676 The resulting formula is: + * + * start_interval = f * sqrt(2 / acceleration) * 0.676 + * + * Since division of integer 2 by acceleration is problematic without usage of floating + * points,the formula is rewritten as: + * + * start_interval = f * sqrt(2 * factor * factor / acceleration) / factor + */ + const uint64_t step_interval_in_ns = NSEC_PER_SEC * 676ULL / 1000ULL * + isqrt(2ULL * factor * factor / acceleration) / factor; + LOG_DBG("Start Interval in ns: %llu", step_interval_in_ns); + return step_interval_in_ns; +} + +static void trapezoidal_recalculate_ramp(struct stepper_ramp_common *ramp_common, + const uint32_t total_steps_to_move) +{ + struct stepper_ramp_profile *ramp_profile = &ramp_common->ramp_profile; + struct stepper_ramp_distance_profile *distance_profile = + &ramp_common->ramp_distance_profile; + distance_profile->acceleration = (ramp_profile->max_velocity * ramp_profile->max_velocity) / + (ramp_profile->acceleration * 2); + + distance_profile->deceleration = (ramp_profile->max_velocity * ramp_profile->max_velocity) / + (ramp_profile->deceleration * 2); + + if ((distance_profile->acceleration + distance_profile->deceleration) > + total_steps_to_move) { + LOG_DBG("Total distance to move is less than the sum of acceleration and " + "deceleration distances"); + distance_profile->const_speed = 0; + distance_profile->deceleration = + (total_steps_to_move * ramp_profile->acceleration) / + (ramp_profile->deceleration + ramp_profile->acceleration); + distance_profile->acceleration = + total_steps_to_move - distance_profile->deceleration; + } else { + distance_profile->const_speed = total_steps_to_move - + distance_profile->acceleration - + distance_profile->deceleration; + } + LOG_DBG("Distance Profile: acceleration=%d const_speed=%d deceleration=%d for steps=%d", + distance_profile->acceleration, distance_profile->const_speed, + distance_profile->deceleration, total_steps_to_move); +} + +static void increment_ramp_position(struct stepper_ramp_runtime_data *ramp_data) +{ + if (ramp_data->ramp_actual_position < ramp_data->ramp_target_position) { + ramp_data->ramp_actual_position++; + } +} + +static uint32_t get_remaining_ramp_steps(const struct stepper_ramp_runtime_data *ramp_data) +{ + return ramp_data->ramp_target_position - ramp_data->ramp_actual_position; +} + +static uint64_t trapezoidal_get_next_step_interval(struct stepper_ramp_common *ramp_common, + const uint64_t current_step_interval_in_ns, + enum stepper_run_mode run_mode) +{ + struct stepper_ramp_runtime_data *ramp_data = &ramp_common->ramp_runtime_data; + struct stepper_ramp_profile *ramp_profile = &ramp_common->ramp_profile; + struct stepper_ramp_distance_profile *distance_profile = + &ramp_common->ramp_distance_profile; + const uint32_t max_velocity = ramp_profile->max_velocity; + uint64_t new_step_interval_in_ns = current_step_interval_in_ns; + + if (current_step_interval_in_ns < NSEC_PER_SEC / max_velocity && + ramp_data->current_ramp_state != STEPPER_RAMP_STATE_DECELERATION) { + LOG_DBG("Moving to constant speed"); + ramp_data->current_ramp_state = STEPPER_RAMP_STATE_CONSTANT_SPEED; + } + + if (get_remaining_ramp_steps(ramp_data) <= distance_profile->deceleration && + ramp_data->current_ramp_state != STEPPER_RAMP_STATE_DECELERATION && + run_mode == STEPPER_RUN_MODE_POSITION) { + LOG_DBG("Moving to deceleration"); + ramp_data->current_ramp_state = STEPPER_RAMP_STATE_DECELERATION; + } + + switch (ramp_data->current_ramp_state) { + case STEPPER_RAMP_STATE_ACCELERATION: + increment_ramp_position(ramp_data); + new_step_interval_in_ns = calc_approx_step_interval(current_step_interval_in_ns, + ramp_data->ramp_actual_position, + ramp_data->current_ramp_state); + break; + + case STEPPER_RAMP_STATE_CONSTANT_SPEED: + increment_ramp_position(ramp_data); + new_step_interval_in_ns = NSEC_PER_SEC / max_velocity; + break; + + /** + * Guard to decelerate till the ramp stop step interval threshold is reached. + */ + case STEPPER_RAMP_STATE_DECELERATION: + increment_ramp_position(ramp_data); + if ((current_step_interval_in_ns <= + ramp_data->ramp_stop_step_interval_threshold_in_ns)) { + new_step_interval_in_ns = calc_approx_step_interval( + current_step_interval_in_ns, distance_profile->deceleration--, + ramp_data->current_ramp_state); + } + break; + + /** + * In case of forced deceleration, the step interval is calculated based on the + * number of steps left to be moved. The step interval is increased by a factor of + * 2/(4*n+1) where n is the number of steps left to be moved. + */ + case STEPPER_RAMP_STATE_PRE_DECELERATION: + if ((current_step_interval_in_ns > + ramp_data->ramp_stop_step_interval_threshold_in_ns)) { + uint64_t start_velocity = + trapezpoidal_calculate_start_interval(ramp_profile->acceleration); + LOG_DBG("Step Interval in ns: %llu", start_velocity); + LOG_DBG("Forced deceleration completed"); + ramp_data->current_ramp_state = STEPPER_RAMP_STATE_ACCELERATION; + trapezoidal_recalculate_ramp(ramp_common, ramp_data->ramp_target_position); + return start_velocity; + } + + if (ramp_data->pre_deceleration_steps > 0) { + ramp_data->pre_deceleration_steps--; + } + + /* Increasing ramp target position as this amount of steps would have to be + * traversed back + */ + ramp_data->ramp_target_position++; + + new_step_interval_in_ns = calc_approx_step_interval( + current_step_interval_in_ns, ramp_data->pre_deceleration_steps, + ramp_data->current_ramp_state); + break; + default: + new_step_interval_in_ns = current_step_interval_in_ns; + break; + } + return new_step_interval_in_ns; +} + +static void trapezoidal_reset_ramp_runtime_data(const struct stepper_ramp_config *config, + struct stepper_ramp_common *ramp_common_data, + const uint32_t steps_to_move) +{ + struct stepper_ramp_runtime_data *data = &ramp_common_data->ramp_runtime_data; + struct stepper_ramp_profile *ramp_profile = &ramp_common_data->ramp_profile; + + if (is_dir_changed_while_in_motion( + ramp_common_data->ramp_runtime_data.is_stepper_moving, + ramp_common_data->ramp_runtime_data.is_stepper_dir_changed)) { + data->current_ramp_state = STEPPER_RAMP_STATE_PRE_DECELERATION; + } else if (!ramp_common_data->ramp_runtime_data.is_stepper_moving) { + data->current_ramp_state = STEPPER_RAMP_STATE_NOT_MOVING; + } + LOG_DBG("Resetting ramp data for ramp state %d", data->current_ramp_state); + + switch (data->current_ramp_state) { + case STEPPER_RAMP_STATE_PRE_DECELERATION: + data->pre_deceleration_steps = config->pre_deceleration_steps; + data->ramp_actual_position = 0; + data->ramp_target_position = steps_to_move; + break; + + case STEPPER_RAMP_STATE_NOT_MOVING: + ramp_common_data->ramp_runtime_data.ramp_stop_step_interval_threshold_in_ns = + trapezpoidal_calculate_start_interval(ramp_profile->acceleration); + data->current_ramp_state = STEPPER_RAMP_STATE_ACCELERATION; + data->ramp_actual_position = 0; + data->ramp_target_position = steps_to_move; + break; + + case STEPPER_RAMP_STATE_DECELERATION: + data->current_ramp_state = STEPPER_RAMP_STATE_ACCELERATION; + data->ramp_actual_position = get_remaining_ramp_steps(data); + data->ramp_target_position = data->ramp_actual_position + steps_to_move; + break; + case STEPPER_RAMP_STATE_ACCELERATION: + case STEPPER_RAMP_STATE_CONSTANT_SPEED: + data->ramp_target_position = data->ramp_actual_position + steps_to_move; + break; + default: + break; + } +} + +const struct stepper_ramp_api trapezoidal_ramp_api = { + .reset_ramp_runtime_data = trapezoidal_reset_ramp_runtime_data, + .calculate_start_interval = trapezpoidal_calculate_start_interval, + .recalculate_ramp = trapezoidal_recalculate_ramp, + .get_next_step_interval = trapezoidal_get_next_step_interval, +}; diff --git a/drivers/stepper/step_dir/step_dir_stepper_common.h b/drivers/stepper/step_dir/step_dir_stepper_common.h index b345cf5d68a5..f8ed59bf88be 100644 --- a/drivers/stepper/step_dir/step_dir_stepper_common.h +++ b/drivers/stepper/step_dir/step_dir_stepper_common.h @@ -18,7 +18,7 @@ #include #include #include - +#include "../stepper_common.h" #include "step_dir_stepper_timing_source.h" /** diff --git a/drivers/stepper/stepper_common.h b/drivers/stepper/stepper_common.h new file mode 100644 index 000000000000..799a164297e4 --- /dev/null +++ b/drivers/stepper/stepper_common.h @@ -0,0 +1,21 @@ +/* + * SPDX-FileCopyrightText: Copyright (c) 2025 Jilay Sandeep Pandya + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_INCLUDE_DRIVERS_STEPPER_COMMON_H_ +#define ZEPHYR_INCLUDE_DRIVERS_STEPPER_COMMON_H_ + +/** + * @brief Stepper Motor run mode options + */ +enum stepper_run_mode { + /** Hold Mode */ + STEPPER_RUN_MODE_HOLD = 0, + /** Position Mode*/ + STEPPER_RUN_MODE_POSITION = 1, + /** Velocity Mode */ + STEPPER_RUN_MODE_VELOCITY = 2, +}; + +#endif /* ZEPHYR_INCLUDE_DRIVERS_STEPPER_STEPPER_COMMON_H_ */ diff --git a/dts/bindings/stepper/ramp-parameters.yaml b/dts/bindings/stepper/ramp-parameters.yaml new file mode 100644 index 000000000000..7c6f5666b0e5 --- /dev/null +++ b/dts/bindings/stepper/ramp-parameters.yaml @@ -0,0 +1,42 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 Jilay Sandeep Pandya +# SPDX-License-Identifier: Apache-2.0 + +description: | + Collection of ramp parameters for stepper motors. + +properties: + acceleration: + type: int + default: 0 + description: | + Maximum acceleration in micro-steps per second squared. + + max-speed: + type: int + default: 0 + description: | + Maximum speed in micro-steps per second. + + deceleration: + type: int + default: 0 + description: | + Maximum deceleration in micro-steps per second squared. + + pre-decel-steps: + type: int + default: 0 + description: | + Maximum Number of steps over which to decelerate while changing motor-direction + during active ramping, Higher number corresponds to longer deceleration, hence + smoother change in direction of motion + + ramp-type: + type: int + description: | + Type of ramping to be used. The options are: + 1: Linear ramping - trapezoidal ramping with constant acceleration and deceleration + Further ramping types can be added in + default: 1 + enum: + - 1 diff --git a/dts/bindings/stepper/zephyr,gpio-stepper.yaml b/dts/bindings/stepper/zephyr,gpio-stepper.yaml index 95477d213998..681418bafe0c 100644 --- a/dts/bindings/stepper/zephyr,gpio-stepper.yaml +++ b/dts/bindings/stepper/zephyr,gpio-stepper.yaml @@ -17,7 +17,7 @@ description: | compatible: "zephyr,gpio-stepper" -include: stepper-controller.yaml +include: [stepper-controller.yaml, ramp-parameters.yaml] properties: gpios: diff --git a/include/zephyr/drivers/stepper.h b/include/zephyr/drivers/stepper.h index 16632e2579ff..d07bcb8aa07b 100644 --- a/include/zephyr/drivers/stepper.h +++ b/include/zephyr/drivers/stepper.h @@ -1,6 +1,6 @@ /* * SPDX-FileCopyrightText: Copyright (c) 2024 Carl Zeiss Meditec AG - * SPDX-FileCopyrightText: Copyright (c) 2024 Jilay Sandeep Pandya + * SPDX-FileCopyrightText: Copyright (c) 2025 Jilay Sandeep Pandya * SPDX-License-Identifier: Apache-2.0 */ @@ -76,18 +76,6 @@ enum stepper_direction { STEPPER_DIRECTION_POSITIVE = 1, }; -/** - * @brief Stepper Motor run mode options - */ -enum stepper_run_mode { - /** Hold Mode */ - STEPPER_RUN_MODE_HOLD = 0, - /** Position Mode*/ - STEPPER_RUN_MODE_POSITION = 1, - /** Velocity Mode */ - STEPPER_RUN_MODE_VELOCITY = 2, -}; - /** * @brief Stepper Events */ @@ -106,6 +94,18 @@ enum stepper_event { STEPPER_EVENT_FAULT_DETECTED = 5, }; +/** + * @brief Stepper ramp profile + */ +struct stepper_ramp_profile { + /** Ramp acceleration in micro-steps per second squared */ + uint16_t acceleration; + /** Ramp deceleration in micro-steps per second squared */ + uint16_t deceleration; + /** Ramp maximum velocity in micro-steps per second */ + uint32_t max_velocity; +}; + /** * @cond INTERNAL_HIDDEN * @@ -176,6 +176,13 @@ typedef int (*stepper_set_event_callback_t)(const struct device *dev, */ typedef int (*stepper_set_microstep_interval_t)(const struct device *dev, const uint64_t microstep_interval_ns); +/** + * @brief Set the ramp profile. + * + * @see stepper_set_ramp_profile() for details. + */ +typedef int (*stepper_set_ramp_profile_t)(const struct device *dev, + const struct stepper_ramp_profile *ramp_profile); /** * @brief Move the stepper relatively by a given number of micro-steps. * @@ -223,6 +230,7 @@ __subsystem struct stepper_driver_api { stepper_get_actual_position_t get_actual_position; stepper_set_event_callback_t set_event_callback; stepper_set_microstep_interval_t set_microstep_interval; + stepper_set_ramp_profile_t set_ramp_profile; stepper_move_by_t move_by; stepper_move_to_t move_to; stepper_run_t run; @@ -420,6 +428,31 @@ static inline int z_impl_stepper_set_microstep_interval(const struct device *dev return api->set_microstep_interval(dev, microstep_interval_ns); } +/** + * @brief Set the ramp profile + * + * @param dev pointer to the stepper driver instance + * @param ramp_profile pointer to the ramp profile structure + * + * @retval -EIO General input / output error + * @retval -EINVAL If the requested ramp profile is not supported + * @retval -ENOSYS If not implemented by device driver + * @retval 0 Success + */ +__syscall int stepper_set_ramp_profile(const struct device *dev, + const struct stepper_ramp_profile *ramp_profile); + +static inline int z_impl_stepper_set_ramp_profile(const struct device *dev, + const struct stepper_ramp_profile *ramp_profile) +{ + const struct stepper_driver_api *api = (const struct stepper_driver_api *)dev->api; + + if (api->set_ramp_profile == NULL) { + return -ENOSYS; + } + return api->set_ramp_profile(dev, ramp_profile); +} + /** * @brief Set the micro-steps to be moved from the current position i.e. relative movement * diff --git a/include/zephyr/dt-bindings/stepper/ramp.h b/include/zephyr/dt-bindings/stepper/ramp.h new file mode 100644 index 000000000000..68e9a281a384 --- /dev/null +++ b/include/zephyr/dt-bindings/stepper/ramp.h @@ -0,0 +1,14 @@ +/* + * SPDX-FileCopyrightText: Copyright (c) 2025 Jilay Sandeep Pandya + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_INCLUDE_DT_BINDINGS_STEPPER_RAMP_H_ +#define ZEPHYR_INCLUDE_DT_BINDINGS_STEPPER_RAMP_H_ + +/**************************************** + * PLEASE KEEP THIS LIST INCREMENTAL * + ***************************************/ +#define STEPPER_RAMP_TYPE_TRAPEZOIDAL 1 + +#endif /* ZEPHYR_INCLUDE_DT_BINDINGS_STEPPER_RAMP_H_ */ diff --git a/samples/drivers/stepper/generic/Kconfig b/samples/drivers/stepper/generic/Kconfig index a9259ff3b8a6..26b4d792470c 100644 --- a/samples/drivers/stepper/generic/Kconfig +++ b/samples/drivers/stepper/generic/Kconfig @@ -15,6 +15,18 @@ config PING_PONG_N_REV int "Change direction every N revolutions" default 1 +config RAMP_ACCELERATION + int "Ramp acceleration micro-step/s²" + default 100 + +config RAMP_MAX_VELOCITY + int "Ramp max velocity micro-steps/s" + default 1000 + +config RAMP_DECELERATION + int "Ramp deceleration micro-step/s²" + default 100 + config MONITOR_THREAD_TIMEOUT_MS int "Monitor thread timeout (ms)" default 1000 diff --git a/samples/drivers/stepper/generic/boards/nucleo_g071rb.overlay b/samples/drivers/stepper/generic/boards/nucleo_g071rb.overlay index e392a028cb32..5aaeee858297 100644 --- a/samples/drivers/stepper/generic/boards/nucleo_g071rb.overlay +++ b/samples/drivers/stepper/generic/boards/nucleo_g071rb.overlay @@ -1,3 +1,5 @@ +#include + / { aliases { stepper = &gpio_stepper; @@ -9,6 +11,11 @@ compatible = "zephyr,gpio-stepper"; status = "okay"; micro-step-res = <2>; + acceleration = <500>; + max-speed = <2000>; + deceleration = <500>; + pre-decel-steps = <500>; + ramp-type = ; gpios = <&gpioa 9 GPIO_ACTIVE_HIGH>, /* D8 */ <&gpioc 7 GPIO_ACTIVE_HIGH>, /* D9 */ <&gpiob 0 GPIO_ACTIVE_HIGH>, /* D10 */ diff --git a/samples/drivers/stepper/generic/prj.conf b/samples/drivers/stepper/generic/prj.conf index 7b579deb99fb..1efb722be881 100644 --- a/samples/drivers/stepper/generic/prj.conf +++ b/samples/drivers/stepper/generic/prj.conf @@ -1,3 +1,5 @@ CONFIG_STEPPER=y CONFIG_LOG=y CONFIG_INPUT=y +CONFIG_STEPPER_RAMP=y +CONFIG_STEPPER_RAMP_TRAPEZOIDAL=y diff --git a/samples/drivers/stepper/generic/src/main.c b/samples/drivers/stepper/generic/src/main.c index f1323e50850a..941e3de37915 100644 --- a/samples/drivers/stepper/generic/src/main.c +++ b/samples/drivers/stepper/generic/src/main.c @@ -74,6 +74,12 @@ int main(void) stepper_set_event_callback(stepper, stepper_callback, NULL); stepper_set_reference_position(stepper, 0); stepper_set_microstep_interval(stepper, CONFIG_STEP_INTERVAL_NS); + struct stepper_ramp_profile ramp_profile = { + .acceleration = CONFIG_RAMP_ACCELERATION, + .max_velocity = CONFIG_RAMP_MAX_VELOCITY, + .deceleration = CONFIG_RAMP_DECELERATION, + }; + stepper_set_ramp_profile(stepper, &ramp_profile); for (;;) { k_sem_take(&stepper_generic_sem, K_FOREVER); From 314c9e91931abe2eda22c9591f789cd9f8b2a054 Mon Sep 17 00:00:00 2001 From: Andre Stefanov Date: Thu, 24 Apr 2025 20:22:07 +0200 Subject: [PATCH 2/2] tests: added unit tests for stepper ramp api testing following criteria: - distance profile - first acceleration interval - next step intervals Signed-off-by: Andre Stefanov --- tests/drivers/stepper/ramp/CMakeLists.txt | 8 + tests/drivers/stepper/ramp/prj.conf | 15 ++ tests/drivers/stepper/ramp/src/main.c | 223 ++++++++++++++++++++++ tests/drivers/stepper/ramp/testcase.yaml | 13 ++ 4 files changed, 259 insertions(+) create mode 100644 tests/drivers/stepper/ramp/CMakeLists.txt create mode 100644 tests/drivers/stepper/ramp/prj.conf create mode 100644 tests/drivers/stepper/ramp/src/main.c create mode 100644 tests/drivers/stepper/ramp/testcase.yaml diff --git a/tests/drivers/stepper/ramp/CMakeLists.txt b/tests/drivers/stepper/ramp/CMakeLists.txt new file mode 100644 index 000000000000..a314b1d8b405 --- /dev/null +++ b/tests/drivers/stepper/ramp/CMakeLists.txt @@ -0,0 +1,8 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 Andre Stefanov +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20.0) +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(ramp) + +target_sources(app PRIVATE src/main.c) diff --git a/tests/drivers/stepper/ramp/prj.conf b/tests/drivers/stepper/ramp/prj.conf new file mode 100644 index 000000000000..b782839b94bb --- /dev/null +++ b/tests/drivers/stepper/ramp/prj.conf @@ -0,0 +1,15 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 Andre Stefanov +# SPDX-License-Identifier: Apache-2.0 + +CONFIG_ZTEST=y +CONFIG_TEST=y +CONFIG_TEST_USERSPACE=y +CONFIG_LOG=y +CONFIG_STEPPER_LOG_LEVEL_DBG=y +CONFIG_STEPPER=y +CONFIG_STEPPER_RAMP=y +CONFIG_STEPPER_RAMP_TRAPEZOIDAL=y +CONFIG_POLL=y + +CONFIG_MAIN_STACK_SIZE=8192 +CONFIG_FPU=y diff --git a/tests/drivers/stepper/ramp/src/main.c b/tests/drivers/stepper/ramp/src/main.c new file mode 100644 index 000000000000..027493bdbd34 --- /dev/null +++ b/tests/drivers/stepper/ramp/src/main.c @@ -0,0 +1,223 @@ +/* + * SPDX-FileCopyrightText: Copyright (c) 2025 Andre Stefanov + * SPDX-FileCopyrightText: Copyright (c) 2025 Jilay Sandeep Pandya + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include + +#include "../../../drivers/stepper/ramp/ramp.h" + +#include +LOG_MODULE_REGISTER(ramp, CONFIG_STEPPER_LOG_LEVEL); + +#define RAMP_PROFILE(accel, velocity, decel) \ + { \ + .acceleration = accel, \ + .max_velocity = velocity, \ + .deceleration = decel, \ + } + +#define DISTANCE_PROFILE(accel, const_velocity, decel) \ + { \ + .acceleration = accel, \ + .const_speed = const_velocity, \ + .deceleration = decel, \ + } + +#define RAMP_RUNTIME_DATA(actual_pos, target_pos, pre_decel_steps, threshold, state) \ + (struct stepper_ramp_runtime_data) \ + { \ + .ramp_actual_position = actual_pos, .ramp_target_position = target_pos, \ + .pre_deceleration_steps = pre_decel_steps, \ + .ramp_stop_step_interval_threshold_in_ns = threshold, .current_ramp_state = state, \ + } + +#define RAMP_TEST_PARAMS(accel, velocity, decel, pre_decel, steps) \ + (struct test_params) \ + { \ + .acceleration = accel, .max_velocity = velocity, .deceleration = decel, \ + .pre_deceleration_steps = pre_decel, .steps_to_move = steps, \ + } + +struct test_params { + uint16_t acceleration; + uint32_t max_velocity; + uint16_t deceleration; + uint32_t pre_deceleration_steps; + uint32_t steps_to_move; +}; + +struct reset_ramp_data_expectation { + struct stepper_ramp_distance_profile distance_profile; +}; + +struct get_next_step_interval_expectation { + uint64_t *intervals; + uint32_t count; +}; + +static void test_reset_ramp_data(const struct test_params params, + const struct reset_ramp_data_expectation expectation) +{ + struct stepper_ramp_common common = { + .ramp_profile = + RAMP_PROFILE(params.acceleration, params.max_velocity, params.deceleration), + .ramp_distance_profile = DISTANCE_PROFILE(0, 0, 0), + }; + + struct stepper_ramp_config config = { + .pre_deceleration_steps = params.pre_deceleration_steps, + }; + + trapezoidal_ramp_api.reset_ramp_runtime_data(&config, &common, params.steps_to_move); + + trapezoidal_ramp_api.recalculate_ramp(&common, params.steps_to_move); + + zexpect_equal(common.ramp_distance_profile.acceleration + + common.ramp_distance_profile.const_speed + + common.ramp_distance_profile.deceleration, + params.steps_to_move, "Expected total steps %d but got %d", + params.steps_to_move, + common.ramp_distance_profile.acceleration + + common.ramp_distance_profile.const_speed + + common.ramp_distance_profile.deceleration); + + zexpect_equal(common.ramp_distance_profile.acceleration, + expectation.distance_profile.acceleration, + "Expected acceleration %d but got %d", + expectation.distance_profile.acceleration, + common.ramp_distance_profile.acceleration); + + zexpect_equal( + common.ramp_distance_profile.const_speed, expectation.distance_profile.const_speed, + "Expected const_speed %d but got %d", expectation.distance_profile.const_speed, + common.ramp_distance_profile.const_speed); + + zexpect_equal(common.ramp_distance_profile.deceleration, + expectation.distance_profile.deceleration, + "Expected deceleration %d but got %d", + expectation.distance_profile.deceleration, + common.ramp_distance_profile.deceleration); +} + +ZTEST_SUITE(ramp, NULL, NULL, NULL, NULL, NULL); + +ZTEST(ramp, test_ramp_distance_profile) +{ + /** motion of 0 steps should not result in steps in any ramp phase */ + LOG_DBG("Test zero steps"); + test_reset_ramp_data(RAMP_TEST_PARAMS(1000, 10000, 1000, 0, 0), + (struct reset_ramp_data_expectation){ + .distance_profile = DISTANCE_PROFILE(0, 0, 0), + }); + + /** motion of 1 step should result in acceleration of 1 step if the requested velocity is + * higher than the start velocity of the ramp. + */ + LOG_DBG("Test 1 fast step"); + test_reset_ramp_data(RAMP_TEST_PARAMS(1000, 10000, 1000, 0, 1), + (struct reset_ramp_data_expectation){ + .distance_profile = DISTANCE_PROFILE(1, 0, 0), + }); + + /** motion of 1 step should result in a single constant speed step if the requested + * velocityis lower than the start velocity of the ramp. + */ + LOG_DBG("Test 1 slow step"); + test_reset_ramp_data(RAMP_TEST_PARAMS(1000, 1, 1000, 0, 1), + (struct reset_ramp_data_expectation){ + .distance_profile = DISTANCE_PROFILE(0, 1, 0), + }); + + LOG_DBG("Test 2 steps"); + test_reset_ramp_data(RAMP_TEST_PARAMS(1000, 10000, 1000, 0, 2), + (struct reset_ramp_data_expectation){ + .distance_profile = DISTANCE_PROFILE(1, 0, 1), + }); + + LOG_DBG("Test 3 steps"); + test_reset_ramp_data(RAMP_TEST_PARAMS(1000, 10000, 1000, 0, 3), + (struct reset_ramp_data_expectation){ + .distance_profile = DISTANCE_PROFILE(2, 0, 1), + }); + + LOG_DBG("Test 100000 steps"); + test_reset_ramp_data(RAMP_TEST_PARAMS(1000, 10000, 1000, 0, 1000), + (struct reset_ramp_data_expectation){ + .distance_profile = DISTANCE_PROFILE(500, 0, 500), + }); + + LOG_DBG("Test 100001 steps"); + test_reset_ramp_data(RAMP_TEST_PARAMS(1000, 10000, 1000, 0, 1001), + (struct reset_ramp_data_expectation){ + .distance_profile = DISTANCE_PROFILE(501, 0, 500), + }); + + LOG_DBG("Test 200000 steps"); + test_reset_ramp_data(RAMP_TEST_PARAMS(1000, 10000, 1000, 0, 110000), + (struct reset_ramp_data_expectation){ + .distance_profile = DISTANCE_PROFILE(50000, 10000, 50000), + }); +} + +ZTEST(ramp, test_first_interval) +{ + uint32_t test_accelerations[] = {1, 100, 1000, UINT32_MAX}; + + for (size_t i = 0; i < ARRAY_SIZE(test_accelerations); i++) { + LOG_DBG("Test acceleration %u steps/s/s", test_accelerations[i]); + const uint64_t start_interval = + trapezoidal_ramp_api.calculate_start_interval(test_accelerations[i]); + LOG_DBG("Start interval in ns: %llu", start_interval); + + /** see AVR446 section 2.3.1, exact calculations of the inter-step delay */ + const double ideal_start_interval = + NSEC_PER_SEC * sqrt(2.0 / test_accelerations[i]) * 0.676; + LOG_DBG("Ideal start interval in ns: %f", ideal_start_interval); + + const double ratio = fabs((double)start_interval / ideal_start_interval); + + zassert_within(ratio, 1.0, 0.01); + } + + LOG_DBG("Test acceleration %u steps/s/s", 0); + const uint64_t invalid_interval = trapezoidal_ramp_api.calculate_start_interval(0); + + zassert_equal(invalid_interval, UINT64_MAX); +} + +ZTEST(ramp, test_get_next_step_interval) +{ + struct stepper_ramp_common common = { + .ramp_profile = RAMP_PROFILE(1000, 10000, 1000), + .ramp_distance_profile = DISTANCE_PROFILE(0, 0, 0), + .ramp_runtime_data = RAMP_RUNTIME_DATA(0, 0, 0, 0, STEPPER_RAMP_STATE_NOT_MOVING), + }; + + const struct stepper_ramp_config config = { + .pre_deceleration_steps = 0, + }; + + trapezoidal_ramp_api.reset_ramp_runtime_data(&config, &common, 1000); + trapezoidal_ramp_api.recalculate_ramp(&common, 1000); + + uint64_t intervals[500] = {0}; + + intervals[0] = trapezoidal_ramp_api.calculate_start_interval(1000); + + for (uint32_t i = 1; i < 500; i++) { + intervals[i] = trapezoidal_ramp_api.get_next_step_interval( + &common, intervals[i - 1], STEPPER_RUN_MODE_POSITION); + } + + for (uint32_t i = 1; i < 500; i++) { + LOG_DBG("Compare %llu vs %llu", intervals[i - 1], intervals[i]); + zexpect(intervals[i - 1] > intervals[i], + "Acceleration intervals have to decrease over time"); + } + + zexpect(NSEC_PER_SEC / 1000 < intervals[499], + "Last ramp interval has to be smaller than the requested velocity"); +} diff --git a/tests/drivers/stepper/ramp/testcase.yaml b/tests/drivers/stepper/ramp/testcase.yaml new file mode 100644 index 000000000000..49d6a654542e --- /dev/null +++ b/tests/drivers/stepper/ramp/testcase.yaml @@ -0,0 +1,13 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 Andre Stefanov +# SPDX-License-Identifier: Apache-2.0 + +common: + tags: + - drivers + - stepper + - ramp +tests: + drivers.stepper.ramp: + platform_allow: + - qemu_x86_64/atom + - native_sim/native/64