Commit 4f36fa80 authored by Jilay Pandya's avatar Jilay Pandya Committed by Carles Cufi
Browse files

stepper: driver: introduce gpio driver 



This commit introduces a basic gpio stepper driver

Signed-off-by: default avatarJilay Pandya <jilay.pandya@zeiss.com>
Signed-off-by: default avatarFabian Blatz <fabianblatz@gmail.com>
parent 1af0ec1d

drivers/stepper/CMakeLists.txt

+2 −0
Original line number Diff line number Diff line
@@ -4,3 +4,5 @@ 
zephyr_syscall_header(${ZEPHYR_BASE}/include/zephyr/drivers/stepper.h)



zephyr_library()



zephyr_library_sources_ifdef(CONFIG_GPIO_STEPPER gpio_stepper_controller.c)

drivers/stepper/Kconfig

+2 −0
Original line number Diff line number Diff line
@@ -20,4 +20,6 @@ config STEPPER_INIT_PRIORITY 


comment "Stepper Drivers"



rsource "Kconfig.gpio"



endif

drivers/stepper/Kconfig.gpio

0 → 100644
+14 −0
Original line number Diff line number Diff line 
# SPDX-FileCopyrightText: Copyright (c) 2024 Carl Zeiss Meditec AG

# SPDX-License-Identifier: Apache-2.0



menu "GPIO stepper driver"



config GPIO_STEPPER

	bool "Activate driver for gpio stepper control"

	depends on DT_HAS_GPIO_STEPPERS_ENABLED

	select POLL

	default y

	help

	  GPIO Stepper driver for stepper motor control with darlington arrays or dual H-bridge.



endmenu

drivers/stepper/gpio_stepper_controller.c

0 → 100644
+349 −0
Original line number Diff line number Diff line 
/*

 * 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)

dts/bindings/stepper/gpio-stepper-controller.yaml

0 → 100644
+33 −0
Original line number Diff line number Diff line 
# SPDX-FileCopyrightText: Copyright (c) 2024 Carl Zeiss Meditec AG

# SPDX-License-Identifier: Apache-2.0



description: |

  GPIO Stepper Controller cluster for darlington transistor arrays or dual H-bridge



  Example:

    /* Lead A is connected Lead C and Lead B is connected to Lead D*/

    stepper {

        compatible = "gpio-steppers";

        motor: motor {

            gpios = <&gpioa 9 GPIO_ACTIVE_HIGH>,  /* Lead A1/A */

                    <&gpioc 7 GPIO_ACTIVE_HIGH>,  /* Lead B1/B */

                    <&gpiob 0 GPIO_ACTIVE_HIGH>,  /* Lead A2/C */

                    <&gpioa 7 GPIO_ACTIVE_HIGH>;  /* Lead B2/D */

        };

    };



compatible: "gpio-steppers"



child-binding:

  description: GPIO Controller for stepper motor

  include:

    - name: stepper-controller.yaml

      property-allowlist:

        - micro-step-res



  properties:

    gpios:

      type: phandle-array

      required: true

      description: |

        The gpio pin array on which the stepper inputs are to be connected

CRA Git | Maintained and supported by SUSTech CRA and CCSE