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..2a142f766a4d1 100644 --- a/drivers/servo/Kconfig +++ b/drivers/servo/Kconfig @@ -6,7 +6,29 @@ # 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" + 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 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 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..62fb5a5c95553 --- /dev/null +++ b/drivers/servo/include/servo_params.h @@ -0,0 +1,267 @@ +/* + * 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 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; }