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 <[email protected]>

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,82 +12,221 @@
  * @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
  * @brief       High-level driver for easy handling of servo motors
  *
  * @author      Hauke Petersen <[email protected]>
  * @author      Joakim Nohlgård <[email protected]>
+ * @author      Marian Buschsieweke <[email protected]>
  */
 
 #ifndef SERVO_H
 #define SERVO_H
 
+#include <stddef.h>
+#include <stdint.h>
+
 #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
 }

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 <[email protected]>
+ *
+ * @}
+ */
+
+#include <string.h>
+#include <stdint.h>
+
+#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);
+    }
+}

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();
+    }
 }

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

drivers/servo/Makefile

@@ -1,3 +1,3 @@
-MODULE = servo
+SUBMODULES := 1
 
 include $(RIOTBASE)/Makefile.base