From 463e2e721ab3c5ab317ffe82c40fdebec6b14c22 Mon Sep 17 00:00:00 2001 From: Marian Buschsieweke Date: Tue, 2 Aug 2022 22:13:45 +0200 Subject: [PATCH] drivers/servo: reimplement with high level interface The previous servo driver didn't provide any benefit over using PWM directly, as users controlled the servo in terms of PWM duty cycles. This changes the interface to provide a high level interface that abstracts the gory PWM details. In addition, a SAUL layer and auto-initialization is provided. Co-authored-by: benpicco --- drivers/include/servo.h | 230 +++++++++++++++---- drivers/saul/init_devs/auto_init_servo.c | 58 +++++ drivers/saul/init_devs/init.c | 4 + drivers/servo/Kconfig | 32 ++- drivers/servo/Makefile | 2 +- drivers/servo/Makefile.dep | 23 +- drivers/servo/Makefile.include | 2 + drivers/servo/include/servo_params.h | 273 +++++++++++++++++++++++ drivers/servo/pwm.c | 100 +++++++++ drivers/servo/saul.c | 70 ++++++ drivers/servo/servo.c | 107 --------- drivers/servo/timer.c | 160 +++++++++++++ makefiles/pseudomodules.inc.mk | 15 ++ tests/driver_servo/Makefile | 19 +- tests/driver_servo/app.config.test | 5 +- tests/driver_servo/main.c | 59 +---- 16 files changed, 949 insertions(+), 210 deletions(-) create mode 100644 drivers/saul/init_devs/auto_init_servo.c create mode 100644 drivers/servo/Makefile.include create mode 100644 drivers/servo/include/servo_params.h create mode 100644 drivers/servo/pwm.c create mode 100644 drivers/servo/saul.c delete mode 100644 drivers/servo/servo.c create mode 100644 drivers/servo/timer.c diff --git a/drivers/include/servo.h b/drivers/include/servo.h index d74b89cb6b24d..26b84add825d4 100644 --- a/drivers/include/servo.h +++ b/drivers/include/servo.h @@ -1,6 +1,7 @@ /* * Copyright (C) 2014 Freie Universität Berlin * Copyright (C) 2015 Eistec AB + * Copyright (C) 2022 Otto-von-Guericke-Universität Magdeburg * * This file is subject to the terms and conditions of the GNU Lesser General * Public License v2.1. See the file LICENSE in the top level directory for more @@ -11,6 +12,23 @@ * @defgroup drivers_servo Servo Motor Driver * @ingroup drivers_actuators * @brief High-level driver for servo motors + * + * Usage + * ===== + * + * Select a flavor of the driver, e.g. `USEMODULE += servo_pwm` for + * @ref drivers_servo_pwm or `USEMODULE += servo_timer` for + * @ref drivers_servo_timer to use. Typically, the PWM implementation is the + * preferred one, but some MCU (e.g. nRF52xxx) cannot configure the PWM + * peripheral to run anywhere close to the 50 Hz to 100 Hz required. + * + * In addition, you many need to extend or adapt @ref servo_params and, + * depending on the selected implementation, @ref servo_pwm_params or + * @ref servo_timer_params to match your hardware configuration. + * + * The test application in `tests/driver_servo` can serve as starting point for + * users. + * * @{ * * @file @@ -18,75 +36,197 @@ * * @author Hauke Petersen * @author Joakim Nohlgård + * @author Marian Buschsieweke */ #ifndef SERVO_H #define SERVO_H +#include +#include + #include "periph/pwm.h" +#include "periph/timer.h" +#include "saul.h" +#include "saul_reg.h" +#include "time_units.h" + +#ifndef SERVO_TIMER_MAX_CHAN +/** + * @brief In case the `servo_timer` backend is used to driver the servo, + * this is the highest channel number usable by the driver + * + * @note To drive *n* servos, *n* + 1 timer channels are required. Hence, + * this must be at least 2 + * + * Trimming this down safes a small bit of RAM: Storage for one pointer is + * wasted on every servo that could be controlled by a timer but is not + * actually used. + */ +#define SERVO_TIMER_MAX_CHAN 4 +#endif #ifdef __cplusplus extern "C" { #endif /** - * @brief Descriptor struct for a servo + * @brief The SAUL adaption driver for servos + */ +extern const saul_driver_t servo_saul_driver; + +/** + * @brief PWM configuration parameters for a servos + * + * Only used with */ typedef struct { - pwm_t device; /**< the PWM device driving the servo */ - int channel; /**< the channel the servo is connected to */ - unsigned int min; /**< minimum pulse width, in us */ - unsigned int max; /**< maximum pulse width, in us */ - unsigned int scale_nom; /**< timing scale factor, to adjust for an inexact PWM frequency, nominator */ - unsigned int scale_den; /**< timing scale factor, to adjust for an inexact PWM frequency, denominator */ -} servo_t; + uint16_t res; /**< PWM resolution to use */ + uint16_t freq; /**< PWM frequency to use */ + pwm_t pwm; /**< PWM dev the servo is connected to */ +} servo_pwm_params_t; /** - * @brief Initialize a servo motor by assigning it a PWM device and channel - * - * Digital servos are controlled by regular pulses sent to them. The width - * of a pulse determines the position of the servo. A pulse width of 1.5ms - * puts the servo in the center position, a pulse width of about 1.0ms and - * about 2.0ms put the servo to the maximum angles. These values can however - * differ slightly from servo to servo, so the min and max values are - * parameterized in the init function. - * - * The servo is initialized with default PWM values: - * - frequency: 100Hz (10ms interval) - * - resolution: 10000 (1000 steps per ms) - * - * These default values can be changed by setting SERVO_RESOLUTION and - * SERVO_FREQUENCY macros. - * Caution: When initializing a servo, the PWM device will be reconfigured to - * new frequency/resolution values. It is however fine to use multiple servos - * with the same PWM device, just on different channels. - * - * @param[out] dev struct describing the servo - * @param[in] pwm the PWM device the servo is connected to - * @param[in] pwm_channel the PWM channel the servo is connected to - * @param[in] min minimum pulse width (in the resolution range) - * @param[in] max maximum pulse width (in the resolution range) - * - * @return 0 on success - * @return <0 on error + * @brief Servo device state */ -int servo_init(servo_t *dev, pwm_t pwm, int pwm_channel, unsigned int min, unsigned int max); +typedef struct servo servo_t; /** - * @brief Set the servo motor to a specified position + * @brief Memory needed for book keeping when using @ref drivers_servo_timer + */ +typedef struct { + /** + * @brief Look up table to get from channel + * + * @note Since timer channel 0 is used to set all servo pins, we use + * `chan - 1` as idx rather than `chan` to not waste one entry. + */ + servo_t *servo_map[SERVO_TIMER_MAX_CHAN]; +} servo_timer_ctx_t; + +/** + * @brief Timer configuration parameters for a servos + */ +typedef struct { + tim_t timer; /**< Timer to use */ + uint32_t timer_freq; /**< Timer frequency to use */ + uint16_t servo_freq; /**< Servo frequency (typically 50 Hz or 100 Hz) */ + servo_timer_ctx_t *ctx; /**< Per-timer state needed for book keeping */ +} servo_timer_params_t; + +/** + * @brief Configuration parameters for a servo + */ +typedef struct { +#if defined(MODULE_SERVO_PWM) || defined(DOXYGEN) + /** + * @brief Specification of the PWM device the servo is connected to + * + * @note Only available when @ref drivers_servo_pwm is used + */ + const servo_pwm_params_t *pwm; +#endif +#if defined(MODULE_SERVO_TIMER) || defined(DOXYGEN) + /** + * @brief Specification of the timer to use + * + * @note Only available when @ref drivers_servo_timer is used + */ + const servo_timer_params_t *timer; + /** + * @brief GPIO pin the servo is connected to + * + * @note Only available when @ref drivers_servo_timer is used + */ + gpio_t servo_pin; +#endif + uint16_t min_us; /**< Duration of high phase (in µs) for min extension */ + uint16_t max_us; /**< Duration of high phase (in µs) for max extension */ +#ifdef MODULE_SERVO_PWM + /** + * @brief PWM channel to use + * + * @note Only available when @ref drivers_servo_pwm is used + */ + uint8_t pwm_chan; +#endif +#ifdef MODULE_SERVO_TIMER + /** + * @brief Timer channel to use + * + * @pre `(timer_chan > 0) && (timer_chan <= SERVO_TIMER_MAX_CHAN)` + * + * @note Only available when @ref drivers_servo_timer is used + * + * The timer channel 0 is used to set the GPIO pin of all servos + * driver by the timer, the other channels are used to clean the GPIO pin + * of the corresponding servo according to the current duty cycle. + */ + uint8_t timer_chan; +#endif +} servo_params_t; + +/** + * @brief Servo device state + */ +struct servo { + const servo_params_t *params; /**< Parameters of this servo */ + /** + * @brief Minimum PWM duty cycle / timer target matching + * @ref servo_params_t::min_us + * + * Note that the actual PWM frequency can be significantly different from + * the requested one, depending on what the hardware can generate using the + * clock source and clock dividers available. + */ + uint16_t min; + /** + * @brief Maximum PWM duty cycle / timer target matching + * @ref servo_params_t::min_us + * + * Note that the actual PWM frequency can be significantly different from + * the requested one, depending on what the hardware can generate using the + * clock source and clock dividers available. + */ + uint16_t max; +#ifdef MODULE_SERVO_TIMER + uint16_t current; /**< Current timer target */ +#endif +}; + +#if defined(MODULE_SERVO_TIMER) || DOXYGEN +/** + * @brief Default timer context + */ +extern servo_timer_ctx_t servo_timer_default_ctx; +#endif + +/** + * @brief Initialize servo * - * The position of the servo is specified in the pulse width that - * controls the servo. With default configurations, a value of 1500 - * means a pulse width of 1.5 ms, which is the center position on - * most servos. + * @param[out] dev Device handle to initialize + * @param[in] params Parameters defining the PWM configuration * - * In case pos is larger/smaller then the max/min values, pos will be set to - * these values. + * @retval 0 Success + * @retval <0 Failure (as negative errno code to indicate cause) + */ +int servo_init(servo_t *dev, const servo_params_t *params); + +/** + * @brief Set the servo motor to a specified position + * + * The position of the servo is specified in the fraction of maximum extension, + * with 0 being the lowest extension (e.g. on an 180° servo it would be at -90°) + * and `UINT8_MAX` being the highest extension (e.g. +90° on that 180° servo). * * @param[in] dev the servo to set - * @param[in] pos the position to set the servo (in the resolution range) + * @param[in] pos the extension to set + * + * Note: 8 bit of resolution may seem low, but is indeed more than high enough + * for any practical PWM based servo. For higher precision, stepper motors would + * be required. */ -void servo_set(const servo_t *dev, unsigned int pos); +void servo_set(servo_t *dev, uint8_t pos); #ifdef __cplusplus } diff --git a/drivers/saul/init_devs/auto_init_servo.c b/drivers/saul/init_devs/auto_init_servo.c new file mode 100644 index 0000000000000..a53a741bc0be8 --- /dev/null +++ b/drivers/saul/init_devs/auto_init_servo.c @@ -0,0 +1,58 @@ +/* + * Copyright (C) 2022 Otto-von-Guericke-Universität Magdeburg + * + * This file is subject to the terms and conditions of the GNU Lesser + * General Public License v2.1. See the file LICENSE in the top level + * directory for more details. + */ + +/** + * @ingroup drivers_saul + * @ingroup sys_auto_init_saul + * @{ + * + * @file + * @brief Auto initialization for servo motors + * + * @author Marian Buschsieweke + * + * @} + */ + +#include +#include + +#include "assert.h" +#include "kernel_defines.h" +#include "log.h" +#include "phydat.h" +#include "saul.h" +#include "saul/periph.h" +#include "saul_reg.h" +#include "servo.h" +#include "servo_params.h" + +#define SERVO_NUMOF ARRAY_SIZE(servo_params) + +static servo_t servos[SERVO_NUMOF]; +static saul_reg_t saul_entries[SERVO_NUMOF]; + +void auto_init_servo(void) +{ + for (unsigned i = 0; i < SERVO_NUMOF; i++) { + LOG_DEBUG("[servo] auto-init servo #%u\n", i); + int retval = servo_init(&servos[i], &servo_params[i]); + if (retval != 0) { + LOG_WARNING("[servo] auto-init of servo #%u failed: %d\n", + i, retval); + continue; + } + saul_reg_t *e = &saul_entries[i]; + + e->dev = &servos[i]; + e->name = servo_saul_info[i].name; + e->driver = &servo_saul_driver; + + saul_reg_add(e); + } +} diff --git a/drivers/saul/init_devs/init.c b/drivers/saul/init_devs/init.c index 55b26e96aded7..6853178912352 100644 --- a/drivers/saul/init_devs/init.c +++ b/drivers/saul/init_devs/init.c @@ -339,4 +339,8 @@ void saul_init_devs(void) extern void auto_init_vl6180x(void); auto_init_vl6180x(); } + if (IS_USED(MODULE_SERVO)) { + extern void auto_init_servo(void); + auto_init_servo(); + } } diff --git a/drivers/servo/Kconfig b/drivers/servo/Kconfig index 9cff354f93e58..744c7de1b5469 100644 --- a/drivers/servo/Kconfig +++ b/drivers/servo/Kconfig @@ -6,7 +6,35 @@ # config MODULE_SERVO - bool "Servo Motor driver" - depends on HAS_PERIPH_PWM + bool "Servo motor driver" + depends on TEST_KCONFIG + select MODULE_SERVO_SAUL if MODULE_SAUL_DEFAULT + +config MODULE_SERVO_SAUL + bool "SAUL integration of the servo motor driver" depends on TEST_KCONFIG + +choice SERVO_DRIVER_BACKEND + bool "Servo motor driver backend" + depends on MODULE_SERVO + default MODULE_SERVO_PWM if HAS_PERIPH_PWM + default MODULE_SERVO_TIMER if !HAS_PERIPH_PWM && HAS_PERIPH_TIMER_PERIODIC + +config MODULE_SERVO_PWM + bool "periph_pwm based Servo Motor driver backend (preferred)" + depends on HAS_PERIPH_PWM + # PWM prescaler on nRF5x MCUs cannot generate a 50 Hz signal + depends on !HAS_CPU_NRF51 + depends on !HAS_CPU_NRF52 + depends on !HAS_CPU_NRF9160 select MODULE_PERIPH_PWM + select SERVO_DRIVER_BACKEND + +config MODULE_SERVO_TIMER + bool "periph_timer based Servo Motor driver backend" + depends on HAS_PERIPH_TIMER + depends on HAS_PERIPH_TIMER_PERIODIC + select MODULE_PERIPH_TIMER + select MODULE_PERIPH_TIMER_PERIODIC + select SERVO_DRIVER_BACKEND +endchoice diff --git a/drivers/servo/Makefile b/drivers/servo/Makefile index 2ff3f9dcb1578..cd1af2456e055 100644 --- a/drivers/servo/Makefile +++ b/drivers/servo/Makefile @@ -1,3 +1,3 @@ -MODULE = servo +SUBMODULES := 1 include $(RIOTBASE)/Makefile.base diff --git a/drivers/servo/Makefile.dep b/drivers/servo/Makefile.dep index 3e3849e830548..318c71dde80b9 100644 --- a/drivers/servo/Makefile.dep +++ b/drivers/servo/Makefile.dep @@ -1 +1,22 @@ -FEATURES_REQUIRED += periph_pwm +ifneq (,$(filter saul,$(USEMODULE))) + DEFAULT_MODULE += servo_saul +endif + +# if no servo driver implementation is chosen, we pick one +ifeq (,$(filter servo_pwm servo_timer,$(USEMODULE))) + # choose servo_pwm except for MCUs known to be incompatible + ifneq (,$(filter nrf5%, $(CPU_FAM))) + USEMODULE += servo_timer + else + USEMODULE += servo_pwm + endif +endif + +ifneq (,$(filter servo_pwm,$(USEMODULE))) + FEATURES_REQUIRED += periph_pwm +endif + +ifneq (,$(filter servo_timer,$(USEMODULE))) + FEATURES_REQUIRED += periph_timer_periodic + FEATURES_REQUIRED += periph_gpio +endif diff --git a/drivers/servo/Makefile.include b/drivers/servo/Makefile.include new file mode 100644 index 0000000000000..a28c5c5ddf69b --- /dev/null +++ b/drivers/servo/Makefile.include @@ -0,0 +1,2 @@ +USEMODULE_INCLUDES_servo := $(LAST_MAKEFILEDIR)/include +USEMODULE_INCLUDES += $(USEMODULE_INCLUDES_servo) diff --git a/drivers/servo/include/servo_params.h b/drivers/servo/include/servo_params.h new file mode 100644 index 0000000000000..03a82aa5ddd4a --- /dev/null +++ b/drivers/servo/include/servo_params.h @@ -0,0 +1,273 @@ +/* + * Copyright (C) 2022 Otto-von-Guericke-Universität Magdeburg + * + * This file is subject to the terms and conditions of the GNU Lesser + * General Public License v2.1. See the file LICENSE in the top level + * directory for more details. + */ + +/** + * @ingroup drivers_servo + * + * @{ + * @file + * @brief Default configuration for servo devices + * + * @author Marian Buschsieweke + */ + +#ifndef SERVO_PARAMS_H +#define SERVO_PARAMS_H + +#include "board.h" +#include "macros/units.h" +#include "periph/gpio.h" +#include "periph/pwm.h" +#include "periph/timer.h" +#include "saul_reg.h" +#include "servo.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @name Default servo PWM configuration + * @{ + */ +#ifndef SERVO_PWM_PARAM_DEV +/** + * @brief PWM device to use to control the servo + * + * Defaults to `PWM_DEV(0)`. + */ +#define SERVO_PWM_PARAM_DEV PWM_DEV(0) +#endif + +#ifndef SERVO_PWM_PARAM_RES +/** + * @brief PWM resolution to use to control the servo + * + * Defaults to `UINT16_MAX`. + */ +#define SERVO_PWM_PARAM_RES UINT16_MAX +#endif + +#ifndef SERVO_PWM_PARAM_FREQ +/** + * @brief PWM frequency in Hertz to use to control the servo + * + * Defaults to `50UL` Hz. + */ +#define SERVO_PWM_PARAM_FREQ 50 +#endif + +#ifndef SERVO_PWM_PARAMS +/** + * @brief PWM parameters for controlling a servo + */ +#define SERVO_PWM_PARAMS {\ + .pwm = SERVO_PWM_PARAM_DEV, \ + .freq = SERVO_PWM_PARAM_FREQ, \ + .res = SERVO_PWM_PARAM_RES, \ + } +#endif +/** @} */ + +/** + * @brief Servo PWM parameters + */ +static const servo_pwm_params_t servo_pwm_params[] = +{ + SERVO_PWM_PARAMS +}; + +/** + * @name Default servo timer configuration + * @{ + */ +#ifndef SERVO_TIMER_PARAM_DEV +/** + * @brief Timer to use to control the servo + * + * Defaults to `TIMER_DEV(1)`. + */ +#define SERVO_TIMER_PARAM_DEV TIMER_DEV(1) +#endif + +#ifndef SERVO_TIMER_PARAM_TIMER_FREQ +/** + * @brief Timer frequency to use to control the servo in Hz + * + * Defaults to 1 MHz + */ +#define SERVO_TIMER_PARAM_TIMER_FREQ MHZ(1) +#endif + +#ifndef SERVO_TIMER_PARAM_SERVO_FREQ +/** + * @brief Servo frequency in Hertz + * + * Defaults to `50UL` Hz. + */ +#define SERVO_TIMER_PARAM_SERVO_FREQ 50 +#endif + +#ifndef SERVO_TIMER_PARAM_TIMER_CTX +/** + * @brief Default servo timer context + * + * Allocated in drivers/servo/timer.c + */ +extern servo_timer_ctx_t servo_timer_default_ctx; +/** + * @brief Memory needed for book keeping + * + * Defaults to `&servo_timer_default_ctx`. One context per timer used is needed. + * E.g. when 4 servos are connected but all are controlled with the same timer + * peripheral, only one context is needed. + */ +#define SERVO_TIMER_PARAM_TIMER_CTX (&servo_timer_default_ctx) +#endif + +#ifndef SERVO_TIMER_PARAMS +/** + * @brief TIMER parameters for controlling a servo + */ +#define SERVO_TIMER_PARAMS {\ + .timer = SERVO_TIMER_PARAM_DEV, \ + .timer_freq = SERVO_TIMER_PARAM_TIMER_FREQ, \ + .servo_freq = SERVO_TIMER_PARAM_SERVO_FREQ, \ + .ctx = SERVO_TIMER_PARAM_TIMER_CTX, \ + } +#endif +/** @} */ + +/** + * @brief Servo timer parameters + */ +static const servo_timer_params_t servo_timer_params[] = +{ + SERVO_TIMER_PARAMS +}; + +/** + * @name Default servo configuration + * @{ + */ +#ifndef SERVO_PARAM_PWM_PARAMS +/** + * @brief PWM parameters + * + * Defaults to `&servo_pwm_params[0]`. + */ +#define SERVO_PARAM_PWM_PARAMS (&servo_pwm_params[0]) +#endif + +#ifndef SERVO_PARAM_TIMER_PARAMS +/** + * @brief Timer parameters + * + * Defaults to `&servo_timer_params[0]`. + */ +#define SERVO_PARAM_TIMER_PARAMS (&servo_timer_params[0]) +#endif + +#ifndef SERVO_PARAM_PWM_CHAN +/** + * @brief PWM channel to use to control the servo + * + * Defaults to `0` + */ +#define SERVO_PARAM_PWM_CHAN 0 +#endif + +#ifndef SERVO_PARAM_TIMER_CHAN +/** + * @brief Timer channel used to clear the servo pin + * + * Defaults to `1` + */ +#define SERVO_PARAM_TIMER_CHAN 1 +#endif + +#ifndef SERVO_PARAM_PIN +/** + * @brief GPIO pin the servo input is connected to + * + * @note Only used with @ref drivers_servo_timer + */ +#define SERVO_PARAM_PIN GPIO_UNDEF +#endif + +#ifndef SERVO_PARAM_MIN_US +/** + * @brief Minimum time in µs of a pulse (corresponds to minimum extension) + * + * Defaults to `900UL`. + */ +#define SERVO_PARAM_MIN_US 900UL +#endif + +#ifndef SERVO_PARAM_MAX_US +/** + * @brief Maximum time in µs of a pulse (corresponds to maximum extension) + * + * Defaults to `2100UL`. + */ +#define SERVO_PARAM_MAX_US 2100UL +#endif + +#ifndef SERVO_PARAMS +/** + * @brief Parameters for controlling a servo + */ +#ifdef MODULE_SERVO_PWM +#define SERVO_PARAMS {\ + .pwm = SERVO_PARAM_PWM_PARAMS, \ + .min_us = SERVO_PARAM_MIN_US, \ + .max_us = SERVO_PARAM_MAX_US, \ + .pwm_chan = SERVO_PARAM_PWM_CHAN, \ + } +#endif +#ifdef MODULE_SERVO_TIMER +#define SERVO_PARAMS {\ + .timer = SERVO_PARAM_TIMER_PARAMS, \ + .servo_pin = SERVO_PARAM_PIN, \ + .min_us = SERVO_PARAM_MIN_US, \ + .max_us = SERVO_PARAM_MAX_US, \ + .timer_chan = SERVO_PARAM_TIMER_CHAN, \ + } +#endif +#endif +/**@}*/ + +/** + * @brief Servo configuration + */ +static const servo_params_t servo_params[] = +{ + SERVO_PARAMS +}; + +#ifndef SERVO_SAULINFO +/** + * @brief Servo SAUL info + */ +#define SERVO_SAULINFO { .name = "servo" } +#endif + +/** + * @brief Allocate and configure entries to the SAUL registry + */ +static const saul_reg_info_t servo_saul_info[] = +{ + SERVO_SAULINFO +}; + +#ifdef __cplusplus +} +#endif + +#endif /* SERVO_PARAMS_H */ +/** @} */ diff --git a/drivers/servo/pwm.c b/drivers/servo/pwm.c new file mode 100644 index 0000000000000..17e3210b4d501 --- /dev/null +++ b/drivers/servo/pwm.c @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2014 Freie Universität Berlin + * Copyright (C) 2015 Eistec AB + * Copyright (C) 2022 Otto-von-Guericke-Universität Magdeburg + * + * This file is subject to the terms and conditions of the GNU Lesser General + * Public License v2.1. See the file LICENSE in the top level directory for more + * details. + */ + +/** + * @ingroup drivers_servo_pwm + * @{ + * + * @file + * @brief Servo motor driver implementation using PWM + * + * @author Hauke Petersen + * @author Joakim Nohlgård + * @author Marian Buschsieweke + * + * @} + */ + +#include +#include +#include + +#include "kernel_defines.h" +#include "periph/pwm.h" +#include "servo.h" +#include "test_utils/expect.h" +#include "macros/math.h" + +#define ENABLE_DEBUG 0 +#include "debug.h" + +/** + * @brief Calculate the duty cycle corresponding to the given duration of the + * "on duration" + * @param pwm_freq frequency of the PWM peripheral + * @param pwm_res resolution of the PWM peripheral + * @param duration duration of the "on phase" in microseconds + * + * @note Scientific rounding is used + */ +static uint16_t duty_cycle(uint32_t freq, uint16_t res, uint16_t duration_us) +{ + return DIV_ROUND((uint64_t)duration_us * freq * res, US_PER_SEC); +} + +int servo_init(servo_t *dev, const servo_params_t *params) +{ + memset(dev, 0, sizeof(*dev)); + const servo_pwm_params_t *pwm_params = params->pwm; + DEBUG("[servo] trying to initialize PWM %u with frequency %u Hz " + "and resolution %u\n", + (unsigned)pwm_params->pwm, (unsigned)pwm_params->freq, + (unsigned)pwm_params->res); + + /* Note: This may initialize the PWM dev over and over again if multiple + * servos are connected to the same PWM. But other than wasting CPU + * cycles, this does no harm. And it greatly simplifies the API, so + * we willfully accept this inefficiency here. + */ + uint32_t freq = pwm_init(pwm_params->pwm, PWM_LEFT, pwm_params->freq, + pwm_params->res); + DEBUG("[servo] initialized PWM %u with frequency %" PRIu32 " Hz\n", + (unsigned)pwm_params->pwm, freq); + + /* assert successful initialization with frequency roughly matching + * requested frequency. A 50 Hz MG90S servo controlled by a 100 Hz PWM + * worked just fine for me, so we are really lax here and accept + * everything in the range of [0.5f; 2f] */ + assert((freq != 0) + && (freq >= pwm_params->freq - pwm_params->freq / 2U) + && (freq <= pwm_params->freq * 2)); + + if (!freq) { + return -EIO; + } + + dev->params = params; + dev->min = duty_cycle(freq, pwm_params->res, params->min_us); + dev->max = duty_cycle(freq, pwm_params->res, params->max_us); + + return 0; +} + +void servo_set(servo_t *dev, uint8_t pos) +{ + const servo_params_t *par = dev->params; + uint32_t duty = dev->max - dev->min; + duty *= pos; + duty >>= 8; + duty += dev->min; + DEBUG("[servo] setting %p to %u (%u / 255)\n", + (void *)dev, (unsigned)duty, (unsigned)pos); + pwm_set(par->pwm->pwm, par->pwm_chan, duty); +} diff --git a/drivers/servo/saul.c b/drivers/servo/saul.c new file mode 100644 index 0000000000000..8ab1d25fd3419 --- /dev/null +++ b/drivers/servo/saul.c @@ -0,0 +1,70 @@ +/* + * Copyright (C) 2022 Otto-von-Guericke-Universität Magdeburg + * + * This file is subject to the terms and conditions of the GNU Lesser + * General Public License v2.1. See the file LICENSE in the top level + * directory for more details. + */ + +/** + * @ingroup drivers_saul + * @{ + * + * @file + * @brief SAUL wrapper for servo motors + * + * @author Marian Buschsieweke + * + * @} + */ + +#include +#include + +#include "assert.h" +#include "phydat.h" +#include "saul.h" +#include "saul/periph.h" +#include "saul_reg.h" +#include "servo.h" + +static int write(const void *dev, const phydat_t *state) +{ + servo_t *s = (void *)dev; + int32_t num = state->val[0]; + switch (state->unit) { + case UNIT_PERCENT: + num *= 100; + num >>= 8; + break; + case UNIT_PERMILL: + num *= 1000; + num >>= 8; + break; + case UNIT_BOOL: + num = (num) ? 255 : 0; + break; + case UNIT_NONE: + case UNIT_UNDEF: + break; + default: + return -EINVAL; + } + + if (num > UINT8_MAX) { + num = UINT8_MAX; + } + + if (num < 0) { + num = 0; + } + + servo_set(s, num); + return 1; +} + +const saul_driver_t servo_saul_driver = { + .read = saul_read_notsup, + .write = write, + .type = SAUL_ACT_SERVO, +}; diff --git a/drivers/servo/servo.c b/drivers/servo/servo.c deleted file mode 100644 index eac4dcbdef7d4..0000000000000 --- a/drivers/servo/servo.c +++ /dev/null @@ -1,107 +0,0 @@ -/* - * Copyright (C) 2014 Freie Universität Berlin - * Copyright (C) 2015 Eistec AB - * - * This file is subject to the terms and conditions of the GNU Lesser General - * Public License v2.1. See the file LICENSE in the top level directory for more - * details. - */ - -/** - * @ingroup drivers_servo - * @{ - * - * @file - * @brief Servo motor driver implementation - * - * @author Hauke Petersen - * @author Joakim Nohlgård - * - * @} - */ - -#include "servo.h" -#include "periph/pwm.h" -#include "timex.h" /* for US_PER_SEC */ - -#define ENABLE_DEBUG 0 -#include "debug.h" - -#ifndef SERVO_FREQUENCY -#define SERVO_FREQUENCY (100U) -#endif - -#ifndef SERVO_RESOLUTION -#define SERVO_RESOLUTION (US_PER_SEC / SERVO_FREQUENCY) -#endif - -int servo_init(servo_t *dev, pwm_t pwm, int pwm_channel, unsigned int min, unsigned int max) -{ - int actual_frequency; - - actual_frequency = pwm_init(pwm, PWM_LEFT, SERVO_FREQUENCY, SERVO_RESOLUTION); - - DEBUG("servo: requested %d hz, got %d hz\n", SERVO_FREQUENCY, actual_frequency); - - if (actual_frequency < 0) { - /* PWM error */ - return -1; - } - dev->device = pwm; - dev->channel = pwm_channel; - dev->min = min; - dev->max = max; - - /* Compute scaling fractional */ - /* - * The PWM pulse width can be written as: - * - * t = k / (f * r) - * - * where t is the pulse high time, k is the value set in the PWM peripheral, - * f is the frequency, and r is the resolution of the PWM module. - * - * define t0 as the desired pulse width: - * - * t0 = k0 / (f0 * r) - * - * where f0 is the requested frequency, k0 is the requested number of ticks. - * Introducing f1 as the closest achievable frequency and k1 as the set tick - * value yields: - * - * t1 = k1 / (f1 * r) - * - * setting t1 = t0 and substituting k1 = k0 * s yields: - * - * k0 / (f0 * r) = k0 * s / (f1 * r) - * - * solve for s: - * - * s = f1 / f0 - * - * where s is the optimal scale factor to translate from requested position - * to actual hardware ticks. - */ - dev->scale_nom = actual_frequency; - dev->scale_den = SERVO_FREQUENCY; - - return 0; -} - -void servo_set(const servo_t *dev, unsigned int pos) -{ - unsigned int raw_value; - if (pos > dev->max) { - pos = dev->max; - } - else if (pos < dev->min) { - pos = dev->min; - } - - /* rescale value to match PWM peripheral configuration */ - raw_value = (pos * dev->scale_nom) / dev->scale_den; - - DEBUG("servo_set: pos %d -> raw %d\n", pos, raw_value); - - pwm_set(dev->device, dev->channel, raw_value); -} diff --git a/drivers/servo/timer.c b/drivers/servo/timer.c new file mode 100644 index 0000000000000..c47f31396ee06 --- /dev/null +++ b/drivers/servo/timer.c @@ -0,0 +1,160 @@ +/* + * Copyright (C) 2022 Otto-von-Guericke-Universität Magdeburg + * + * This file is subject to the terms and conditions of the GNU Lesser General + * Public License v2.1. See the file LICENSE in the top level directory for more + * details. + */ + +/** + * @ingroup drivers_servo_timer + * @{ + * + * @file + * @brief Servo motor driver implementation using periph_timer_periodic + * + * @author Marian Buschsieweke + * + * @} + */ + +#include +#include +#include + +#include "atomic_utils.h" +#include "irq.h" +#include "kernel_defines.h" +#include "macros/math.h" +#include "periph/gpio.h" +#include "periph/timer.h" +#include "servo.h" +#include "test_utils/expect.h" +#include "time_units.h" + +#define ENABLE_DEBUG 0 +#include "debug.h" + +servo_timer_ctx_t servo_timer_default_ctx; + +static unsigned ticks_from_us(uint64_t duration, uint64_t freq) +{ + return DIV_ROUND(duration * freq, US_PER_SEC); +} + +/* + * Timer channel 0 is always used for the rising flank of all servos driven + * by the same timer. The channels 1 till n are used for the falling flanks of + * servos 0 till n-1. E.g. as shown in this diagram: + * + * Servo_0 ______/""""\___________ + * Servo_1 ______/""""""""\_______ + * ... + * Servo_n ______/""""""""""""\___ + * + * ^ ^ ^ ^ + * | | | | + * timer chan 0 -+ | | | + * timer chan 1 ------+ | | + * timer chan 2 ----------+ | + * ... | + * timer chan n+1 ------------+ + * + * Channel 0 is set to the period of one PWM control cycle and due to flag + * `TIM_FLAG_RESET_ON_MATCH` will end the period and stat the new period. As + * a result, n+1 channels are needed to control n servos. + */ +static void timer_cb(void *arg, int chan) +{ + servo_timer_ctx_t *ctx = arg; + if (chan == 0) { + /* end of period, set the control pin of all controlled servos */ + for (unsigned i = 0; i < ARRAY_SIZE(ctx->servo_map); i++) { + servo_t *servo = ctx->servo_map[i]; + if (servo) { + gpio_set(servo->params->servo_pin); + } + } + } + else { + /* end of duty cycle of a servo, clear the control pin of the servo + * for which the timer fired */ + assert((unsigned)chan <= ARRAY_SIZE(ctx->servo_map)); + servo_t *servo = ctx->servo_map[chan - 1]; + assert(servo); + gpio_clear(servo->params->servo_pin); + } +} + +int servo_init(servo_t *dev, const servo_params_t *params) +{ + memset(dev, 0, sizeof(*dev)); + assert(params->servo_pin != GPIO_UNDEF); + assert((params->timer_chan > 0) + && (params->timer_chan <= SERVO_TIMER_MAX_CHAN)); + DEBUG("[servo] init %p for GPIO pin %x\n", (void *)dev, + (unsigned)params->servo_pin); + + const servo_timer_params_t *timer_params = params->timer; + + gpio_init(params->servo_pin, GPIO_OUT); + + /* Note: This may initialize the timer dev over and over again if multiple + * servos are connected to the same timer . But other than wasting CPU + * cycles, this does no harm. And it greatly simplifies the API, so + * we willfully accept this inefficiency here. + */ + int retval = timer_init(timer_params->timer, timer_params->timer_freq, + timer_cb, timer_params->ctx); + DEBUG("[servo] timer_init(0x%x, %" PRIu32", timer_cb, ctx)) returned %i\n", + (unsigned)timer_params->timer, timer_params->timer_freq, retval); + + assert(retval == 0); + if (retval != 0) { + return -EINVAL; + } + + uint32_t servo_period_us = US_PER_SEC / timer_params->servo_freq; + unsigned ticks = ticks_from_us(servo_period_us, timer_params->timer_freq); + retval = timer_set_periodic(timer_params->timer, 0, ticks, + TIM_FLAG_RESET_ON_MATCH); + DEBUG("[servo] timer_set_periodic(0x%x, 0, %u) returned %i\n", + (unsigned)timer_params->timer, ticks, retval); + + assert(retval == 0); + if (retval != 0) { + return -ENOTSUP; + } + + dev->params = params; + dev->min = ticks_from_us(params->min_us, timer_params->timer_freq); + dev->max = ticks_from_us(params->max_us, timer_params->timer_freq); + + unsigned irq_state = irq_disable(); + timer_params->ctx->servo_map[params->timer_chan - 1] = dev; + irq_restore(irq_state); + + servo_set(dev, 127); + + return 0; +} + +void servo_set(servo_t *dev, uint8_t pos) +{ + uint32_t target = dev->max - dev->min; + target *= pos; + target >>= 8; + target += dev->min; + DEBUG("[servo] setting %p to %u (%u / 255)\n", + (void *)dev, (unsigned)target, (unsigned)pos); + + /* Update duty cycle */ + const servo_params_t *params = dev->params; + tim_t tim = params->timer->timer; + int retval = timer_set_periodic(tim, params->timer_chan, target, 0); + + assert(retval == 0); + DEBUG("[servo] timer_set_periodic(0x%x, %u, %u) returned %i\n", + (unsigned)tim, (unsigned)params->timer_chan, + (unsigned)dev->min, retval); +} diff --git a/makefiles/pseudomodules.inc.mk b/makefiles/pseudomodules.inc.mk index 82b82b812538e..6854028bf24f3 100644 --- a/makefiles/pseudomodules.inc.mk +++ b/makefiles/pseudomodules.inc.mk @@ -384,6 +384,21 @@ PSEUDOMODULES += semtech_loramac_rx PSEUDOMODULES += senml_cbor PSEUDOMODULES += senml_phydat PSEUDOMODULES += senml_saul +## @defgroup drivers_servo_pwm PWM based servo driver +## @ingroup drivers_servo +## @{ +PSEUDOMODULES += servo_pwm +## @} +## @defgroup drivers_servo_timer periph_timer_periodic based servo driver +## @ingroup drivers_servo +## @{ +PSEUDOMODULES += servo_timer +## @} +## @defgroup drivers_servo_saul SAUL integration of the servo driver +## @ingroup drivers_servo +## @{ +PSEUDOMODULES += servo_saul +## @} ## @defgroup pseudomodule_sha1sum sha1sum ## @ingroup sys_shell_commands ## @{ diff --git a/tests/driver_servo/Makefile b/tests/driver_servo/Makefile index db4089c281983..928d1b12f524d 100644 --- a/tests/driver_servo/Makefile +++ b/tests/driver_servo/Makefile @@ -1,6 +1,23 @@ include ../Makefile.tests_common -USEMODULE += xtimer USEMODULE += servo +USEMODULE += shell +USEMODULE += shell_cmds_default +USEMODULE += saul_default include $(RIOTBASE)/Makefile.include + +SERVO_PIN ?= GPIO_PIN(0, 0) + +TIMER ?= TIMER_DEV(0) +TIMER_FREQ ?= CLOCK_CORECLOCK/8 + +ifneq (,$(filter atmega_common,$(USEMODULE))) + # The ATmega PWM driver has no support for 16 bit timers (as of now). Hence, + # limit the PWM resolution to what a 8 bit timer can handle. + CFLAGS += -DSERVO_PWM_PARAM_RES=256 +endif + +CFLAGS += '-DSERVO_PARAM_PIN=$(SERVO_PIN)' +CFLAGS += '-DSERVO_TIMER_PARAM_DEV=$(TIMER)' +CFLAGS += '-DSERVO_TIMER_PARAM_FREQ=$(TIMER_FREQ)' diff --git a/tests/driver_servo/app.config.test b/tests/driver_servo/app.config.test index 95e75094386d0..b2c4c23df32af 100644 --- a/tests/driver_servo/app.config.test +++ b/tests/driver_servo/app.config.test @@ -1,4 +1,7 @@ # this file enables modules defined in Kconfig. Do not use this file for # application configuration. This is only needed during migration. +CONFIG_MODULE_SAUL=y +CONFIG_MODULE_SAUL_DEFAULT=y CONFIG_MODULE_SERVO=y -CONFIG_MODULE_XTIMER=y +CONFIG_MODULE_SHELL=y +CONFIG_MODULE_SHELL_CMDS_DEFAULT=y diff --git a/tests/driver_servo/main.c b/tests/driver_servo/main.c index a93d9fab44b2b..588d45002ac3d 100644 --- a/tests/driver_servo/main.c +++ b/tests/driver_servo/main.c @@ -1,5 +1,6 @@ /* * Copyright (C) 2015 Eistec AB + * Copyright (C) 2022 Otto-von-Guericke-Universität Magdeburg * * This file is subject to the terms and conditions of the GNU Lesser General * Public License v2.1. See the file LICENSE in the top level directory for more @@ -13,69 +14,23 @@ * @file * @brief Test for servo driver * - * This test initializes the given servo device and moves it between - * 1.000 -- 2.000 ms, roughly -/+ 90 degrees from the middle position if the - * connected servo is a standard RC servo. - * * @author Joakim Nohlgård + * @author Marian Buschsieweke * * @} */ +#include #include -#include "cpu.h" -#include "board.h" -#include "xtimer.h" -#include "periph/pwm.h" -#include "servo.h" - -#define DEV PWM_DEV(0) -#define CHANNEL 0 - -#define SERVO_MIN (1000U) -#define SERVO_MAX (2000U) - -/* these are defined outside the limits of the servo_init min/max parameters above */ -/* we will test the clamping functionality of the servo_set function. */ -#define STEP_LOWER_BOUND (900U) -#define STEP_UPPER_BOUND (2100U) - -/* Step size that we move per WAIT us */ -#define STEP (10U) - -/* Sleep time between updates, no need to update the servo position more than - * once per cycle */ -#define WAIT (10000U) - -static servo_t servo; +#include "shell.h" int main(void) { - int res; - unsigned int pos = (STEP_LOWER_BOUND + STEP_UPPER_BOUND) / 2; - int step = STEP; - - puts("\nRIOT RC servo test"); - puts("Connect an RC servo or scope to PWM_0 channel 0 to see anything"); - - res = servo_init(&servo, DEV, CHANNEL, SERVO_MIN, SERVO_MAX); - if (res < 0) { - puts("Errors while initializing servo"); - return -1; - } - puts("Servo initialized."); - - while (1) { - servo_set(&servo, pos); - - pos += step; - if (pos <= STEP_LOWER_BOUND || pos >= STEP_UPPER_BOUND) { - step = -step; - } + puts("RIOT RC servo test"); - xtimer_usleep(WAIT); - } + char buf[SHELL_DEFAULT_BUFSIZE]; + shell_run(NULL, buf, sizeof(buf)); return 0; }