stepper: driver: introduce gpio driver
This commit introduces a basic gpio stepper driver Signed-off-by: Jilay Pandya <jilay.pandya@zeiss.com> Signed-off-by: Fabian Blatz <fabianblatz@gmail.com>
This commit is contained in:
Jilay Pandya
Carles Cufí
parent
1af0ec1d96
commit
4f36fa8010
5 changed files with 400 additions and 0 deletions
349
drivers/stepper/gpio_stepper_controller.c
Normal file
349
drivers/stepper/gpio_stepper_controller.c
Normal file
|
|
@ -0,0 +1,349 @@
|
|||
/*
|
||||
* SPDX-FileCopyrightText: Copyright (c) 2024 Carl Zeiss Meditec AG
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#define DT_DRV_COMPAT gpio_steppers
|
||||
|
||||
#include <zephyr/drivers/gpio.h>
|
||||
#include <zephyr/kernel.h>
|
||||
#include <zephyr/sys_clock.h>
|
||||
#include <zephyr/drivers/stepper.h>
|
||||
#include <zephyr/logging/log.h>
|
||||
#include <zephyr/sys/__assert.h>
|
||||
|
||||
LOG_MODULE_REGISTER(gpio_stepper_motor_controller, CONFIG_STEPPER_LOG_LEVEL);
|
||||
|
||||
#define MAX_MICRO_STEP_RES STEPPER_MICRO_STEP_2
|
||||
#define NUM_CONTROL_PINS 4
|
||||
|
||||
static const uint8_t
|
||||
half_step_lookup_table[NUM_CONTROL_PINS * MAX_MICRO_STEP_RES][NUM_CONTROL_PINS] = {
|
||||
{1, 1, 0, 0}, {0, 1, 0, 0}, {0, 1, 1, 0}, {0, 0, 1, 0},
|
||||
{0, 0, 1, 1}, {0, 0, 0, 1}, {1, 0, 0, 1}, {1, 0, 0, 0}};
|
||||
|
||||
struct gpio_stepper_motor_controller_config {
|
||||
const struct gpio_dt_spec *control_pins;
|
||||
};
|
||||
|
||||
struct gpio_stepper_motor_controller_data {
|
||||
const struct device *dev;
|
||||
struct k_spinlock lock;
|
||||
enum stepper_direction direction;
|
||||
enum stepper_run_mode run_mode;
|
||||
uint8_t step_gap;
|
||||
uint8_t coil_charge;
|
||||
struct k_work_delayable stepper_dwork;
|
||||
struct k_poll_signal *async_signal;
|
||||
int32_t actual_position;
|
||||
uint32_t delay_in_us;
|
||||
int32_t step_count;
|
||||
};
|
||||
|
||||
static int stepper_motor_set_coil_charge(const struct device *dev)
|
||||
{
|
||||
struct gpio_stepper_motor_controller_data *data = dev->data;
|
||||
const struct gpio_stepper_motor_controller_config *config = dev->config;
|
||||
|
||||
for (int i = 0; i < NUM_CONTROL_PINS; i++) {
|
||||
gpio_pin_set_dt(&config->control_pins[i],
|
||||
half_step_lookup_table[data->coil_charge][i]);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void update_coil_charge(const struct device *dev)
|
||||
{
|
||||
struct gpio_stepper_motor_controller_data *data = dev->data;
|
||||
|
||||
if (data->direction == STEPPER_DIRECTION_POSITIVE) {
|
||||
data->coil_charge = data->coil_charge == 0
|
||||
? NUM_CONTROL_PINS * MAX_MICRO_STEP_RES - data->step_gap
|
||||
: data->coil_charge - data->step_gap;
|
||||
data->actual_position++;
|
||||
} else if (data->direction == STEPPER_DIRECTION_NEGATIVE) {
|
||||
data->coil_charge =
|
||||
data->coil_charge == NUM_CONTROL_PINS * MAX_MICRO_STEP_RES - data->step_gap
|
||||
? 0
|
||||
: data->coil_charge + data->step_gap;
|
||||
data->actual_position--;
|
||||
}
|
||||
}
|
||||
|
||||
static void update_remaining_steps(struct gpio_stepper_motor_controller_data *data)
|
||||
{
|
||||
if (data->step_count > 0) {
|
||||
data->step_count--;
|
||||
k_work_reschedule(&data->stepper_dwork, K_USEC(data->delay_in_us));
|
||||
} else if (data->step_count < 0) {
|
||||
data->step_count++;
|
||||
k_work_reschedule(&data->stepper_dwork, K_USEC(data->delay_in_us));
|
||||
} else {
|
||||
if (data->async_signal) {
|
||||
k_poll_signal_raise(data->async_signal, STEPPER_SIGNAL_STEPS_COMPLETED);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void update_direction_from_step_count(struct gpio_stepper_motor_controller_data *data)
|
||||
{
|
||||
if (data->step_count > 0) {
|
||||
data->direction = STEPPER_DIRECTION_POSITIVE;
|
||||
} else if (data->step_count < 0) {
|
||||
data->direction = STEPPER_DIRECTION_NEGATIVE;
|
||||
} else {
|
||||
LOG_ERR("Step count is zero");
|
||||
}
|
||||
}
|
||||
|
||||
static void position_mode_task(const struct device *dev)
|
||||
{
|
||||
stepper_motor_set_coil_charge(dev);
|
||||
update_coil_charge(dev);
|
||||
update_remaining_steps(dev->data);
|
||||
}
|
||||
|
||||
static void velocity_mode_task(const struct device *dev)
|
||||
{
|
||||
struct gpio_stepper_motor_controller_data *data = dev->data;
|
||||
|
||||
stepper_motor_set_coil_charge(dev);
|
||||
update_coil_charge(dev);
|
||||
k_work_reschedule(&data->stepper_dwork, K_USEC(data->delay_in_us));
|
||||
}
|
||||
|
||||
static void stepper_work_step_handler(struct k_work *work)
|
||||
{
|
||||
struct k_work_delayable *dwork = k_work_delayable_from_work(work);
|
||||
struct gpio_stepper_motor_controller_data *data =
|
||||
CONTAINER_OF(dwork, struct gpio_stepper_motor_controller_data, stepper_dwork);
|
||||
|
||||
K_SPINLOCK(&data->lock) {
|
||||
switch (data->run_mode) {
|
||||
case STEPPER_POSITION_MODE:
|
||||
position_mode_task(data->dev);
|
||||
break;
|
||||
case STEPPER_VELOCITY_MODE:
|
||||
velocity_mode_task(data->dev);
|
||||
break;
|
||||
default:
|
||||
LOG_WRN("Unsupported run mode %d", data->run_mode);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static int gpio_stepper_move(const struct device *dev, int32_t micro_steps,
|
||||
struct k_poll_signal *async)
|
||||
{
|
||||
struct gpio_stepper_motor_controller_data *data = dev->data;
|
||||
|
||||
if (data->delay_in_us == 0) {
|
||||
LOG_ERR("Velocity not set or invalid velocity set");
|
||||
return -EINVAL;
|
||||
}
|
||||
K_SPINLOCK(&data->lock) {
|
||||
if (data->async_signal) {
|
||||
k_poll_signal_reset(data->async_signal);
|
||||
}
|
||||
data->async_signal = async;
|
||||
data->run_mode = STEPPER_POSITION_MODE;
|
||||
data->step_count = micro_steps;
|
||||
update_direction_from_step_count(data);
|
||||
k_work_reschedule(&data->stepper_dwork, K_NO_WAIT);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int gpio_stepper_set_actual_position(const struct device *dev, int32_t position)
|
||||
{
|
||||
struct gpio_stepper_motor_controller_data *data = dev->data;
|
||||
|
||||
K_SPINLOCK(&data->lock) {
|
||||
data->actual_position = position;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int gpio_stepper_get_actual_position(const struct device *dev, int32_t *position)
|
||||
{
|
||||
struct gpio_stepper_motor_controller_data *data = dev->data;
|
||||
|
||||
K_SPINLOCK(&data->lock) {
|
||||
*position = data->actual_position;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int gpio_stepper_set_target_position(const struct device *dev, int32_t position,
|
||||
struct k_poll_signal *async)
|
||||
{
|
||||
struct gpio_stepper_motor_controller_data *data = dev->data;
|
||||
|
||||
if (data->delay_in_us == 0) {
|
||||
LOG_ERR("Velocity not set or invalid velocity set");
|
||||
return -EINVAL;
|
||||
}
|
||||
K_SPINLOCK(&data->lock) {
|
||||
if (data->async_signal) {
|
||||
k_poll_signal_reset(data->async_signal);
|
||||
}
|
||||
data->async_signal = async;
|
||||
data->run_mode = STEPPER_POSITION_MODE;
|
||||
data->step_count = position - data->actual_position;
|
||||
update_direction_from_step_count(data);
|
||||
k_work_reschedule(&data->stepper_dwork, K_NO_WAIT);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int gpio_stepper_is_moving(const struct device *dev, bool *is_moving)
|
||||
{
|
||||
struct gpio_stepper_motor_controller_data *data = dev->data;
|
||||
|
||||
*is_moving = k_work_delayable_is_pending(&data->stepper_dwork);
|
||||
LOG_DBG("Is Motor Moving %d", *is_moving);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int gpio_stepper_set_max_velocity(const struct device *dev, uint32_t velocity)
|
||||
{
|
||||
struct gpio_stepper_motor_controller_data *data = dev->data;
|
||||
|
||||
if (velocity == 0) {
|
||||
LOG_ERR("Velocity cannot be zero");
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
if (velocity > USEC_PER_SEC) {
|
||||
LOG_ERR("Velocity cannot be greater than %d micro_steps_per_second", USEC_PER_SEC);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
K_SPINLOCK(&data->lock) {
|
||||
data->delay_in_us = USEC_PER_SEC / velocity;
|
||||
}
|
||||
LOG_DBG("Setting Motor Speed to %d", velocity);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int gpio_stepper_enable_constant_velocity_mode(const struct device *dev,
|
||||
const enum stepper_direction direction,
|
||||
const uint32_t value)
|
||||
{
|
||||
struct gpio_stepper_motor_controller_data *data = dev->data;
|
||||
|
||||
K_SPINLOCK(&data->lock) {
|
||||
data->run_mode = STEPPER_VELOCITY_MODE;
|
||||
data->direction = direction;
|
||||
if (value != 0) {
|
||||
data->delay_in_us = USEC_PER_SEC / value;
|
||||
k_work_reschedule(&data->stepper_dwork, K_NO_WAIT);
|
||||
} else {
|
||||
k_work_cancel_delayable(&data->stepper_dwork);
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int gpio_stepper_set_micro_step_res(const struct device *dev,
|
||||
enum micro_step_resolution micro_step_res)
|
||||
{
|
||||
struct gpio_stepper_motor_controller_data *data = dev->data;
|
||||
|
||||
K_SPINLOCK(&data->lock) {
|
||||
switch (micro_step_res) {
|
||||
case STEPPER_FULL_STEP:
|
||||
case STEPPER_MICRO_STEP_2:
|
||||
data->step_gap = MAX_MICRO_STEP_RES >> (micro_step_res - 1);
|
||||
break;
|
||||
default:
|
||||
LOG_ERR("Unsupported micro step resolution %d", micro_step_res);
|
||||
return -ENOTSUP;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int gpio_stepper_get_micro_step_res(const struct device *dev,
|
||||
enum micro_step_resolution *micro_step_res)
|
||||
{
|
||||
struct gpio_stepper_motor_controller_data *data = dev->data;
|
||||
*micro_step_res = MAX_MICRO_STEP_RES >> (data->step_gap - 1);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int gpio_stepper_enable(const struct device *dev, bool enable)
|
||||
{
|
||||
struct gpio_stepper_motor_controller_data *data = dev->data;
|
||||
|
||||
K_SPINLOCK(&data->lock) {
|
||||
if (enable) {
|
||||
k_work_reschedule(&data->stepper_dwork, K_NO_WAIT);
|
||||
} else {
|
||||
k_work_cancel_delayable(&data->stepper_dwork);
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int gpio_stepper_motor_controller_init(const struct device *dev)
|
||||
{
|
||||
struct gpio_stepper_motor_controller_data *data = dev->data;
|
||||
const struct gpio_stepper_motor_controller_config *config = dev->config;
|
||||
|
||||
data->dev = dev;
|
||||
LOG_DBG("Initializing %s gpio_stepper_motor_controller with %d pin", dev->name,
|
||||
NUM_CONTROL_PINS);
|
||||
for (uint8_t n_pin = 0; n_pin < NUM_CONTROL_PINS; n_pin++) {
|
||||
gpio_pin_configure_dt(&config->control_pins[n_pin], GPIO_OUTPUT_INACTIVE);
|
||||
}
|
||||
k_work_init_delayable(&data->stepper_dwork, stepper_work_step_handler);
|
||||
return 0;
|
||||
}
|
||||
|
||||
#define GPIO_STEPPER_DEVICE_DATA_DEFINE(child) \
|
||||
static struct gpio_stepper_motor_controller_data \
|
||||
gpio_stepper_motor_controller_data_##child = { \
|
||||
.step_gap = MAX_MICRO_STEP_RES >> (DT_PROP(child, micro_step_res) - 1), \
|
||||
}; \
|
||||
BUILD_ASSERT(DT_PROP(child, micro_step_res) <= STEPPER_MICRO_STEP_2, \
|
||||
"gpio_stepper_controller driver supports up to 2 micro steps");
|
||||
|
||||
#define GPIO_STEPPER_DEVICE_CONFIG_DEFINE(child) \
|
||||
static const struct gpio_dt_spec gpio_stepper_motor_control_pins_##child[] = { \
|
||||
DT_FOREACH_PROP_ELEM_SEP(child, gpios, GPIO_DT_SPEC_GET_BY_IDX, (,)), \
|
||||
}; \
|
||||
BUILD_ASSERT( \
|
||||
ARRAY_SIZE(gpio_stepper_motor_control_pins_##child) == 4, \
|
||||
"gpio_stepper_controller driver currently supports only 4 wire configuration"); \
|
||||
static const struct gpio_stepper_motor_controller_config \
|
||||
gpio_stepper_motor_controller_config_##child = { \
|
||||
.control_pins = gpio_stepper_motor_control_pins_##child};
|
||||
|
||||
#define GPIO_STEPPER_API_DEFINE(child) \
|
||||
static const struct stepper_api gpio_stepper_api_##child = { \
|
||||
.enable = gpio_stepper_enable, \
|
||||
.move = gpio_stepper_move, \
|
||||
.is_moving = gpio_stepper_is_moving, \
|
||||
.set_actual_position = gpio_stepper_set_actual_position, \
|
||||
.get_actual_position = gpio_stepper_get_actual_position, \
|
||||
.set_target_position = gpio_stepper_set_target_position, \
|
||||
.set_max_velocity = gpio_stepper_set_max_velocity, \
|
||||
.enable_constant_velocity_mode = gpio_stepper_enable_constant_velocity_mode, \
|
||||
.set_micro_step_res = gpio_stepper_set_micro_step_res, \
|
||||
.get_micro_step_res = gpio_stepper_get_micro_step_res};
|
||||
|
||||
#define GPIO_STEPPER_DEVICE_DEFINE(child) \
|
||||
DEVICE_DT_DEFINE(child, gpio_stepper_motor_controller_init, NULL, \
|
||||
&gpio_stepper_motor_controller_data_##child, \
|
||||
&gpio_stepper_motor_controller_config_##child, POST_KERNEL, \
|
||||
CONFIG_STEPPER_INIT_PRIORITY, &gpio_stepper_api_##child);
|
||||
|
||||
#define GPIO_STEPPER_CONTROLLER_DEFINE(inst) \
|
||||
DT_INST_FOREACH_CHILD(inst, GPIO_STEPPER_DEVICE_CONFIG_DEFINE); \
|
||||
DT_INST_FOREACH_CHILD(inst, GPIO_STEPPER_DEVICE_DATA_DEFINE); \
|
||||
DT_INST_FOREACH_CHILD(inst, GPIO_STEPPER_API_DEFINE); \
|
||||
DT_INST_FOREACH_CHILD(inst, GPIO_STEPPER_DEVICE_DEFINE);
|
||||
|
||||
DT_INST_FOREACH_STATUS_OKAY(GPIO_STEPPER_CONTROLLER_DEFINE)
|
||||
Loading…
Add table
Add a link
Reference in a new issue