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 <benpicco@googlemail.com>

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,149 @@
  * @defgroup    drivers_servo Servo Motor Driver
  * @ingroup     drivers_actuators
  * @brief       High-level driver for servo motors
+ *
+ * Usage
+ * =====
+ *
+ * Add the module `servo` to make use of the driver. In addition, extend
+ * @ref servo_pwm_params with one entry for each PWM peripheral that is used
+ * and one entry to @ref servo_params for each servo that is used. E.g. if
+ * three servos are connected to different channels of the same PWM peripheral,
+ * @ref servo_pwm_params will contain one element and @ref servo_params will
+ * contain three elements.
+ *
+ * By default, the module `auto_init_servo` will be used as well to initialize
+ * all PWM peripherals for use with the configured servos. If @ref drivers_saul
+ * is used, the module `auto_init_saul_servo` is used to automatically register
+ * all servos into the SAUL registry. Note that @ref servo_saul_entries has to
+ * contain the same number of elements as @ref servo_params, so that each
+ * configured servo can actually be registered.
+ *
+ * The test application in `tests/driver_servo` contains an example
+ * configuration in the file `params.c` that can serve as starting point for
+ * users.
+ *
  * @{
  *
  * @file
  * @brief       High-level driver for easy handling of servo motors
  *
  * @author      Hauke Petersen <hauke.petersen@fu-berlin.de>
  * @author      Joakim Nohlgård <joakim.nohlgard@eistec.se>
+ * @author      Marian Buschsieweke <marian.buschsieweke@ovgu.de>
  */
 
 #ifndef SERVO_H
 #define SERVO_H
 
+#include <stddef.h>
+#include <stdint.h>
+
+#include "macros/math.h"
 #include "periph/pwm.h"
+#include "saul.h"
+#include "saul_reg.h"
+#include "time_units.h"
 
 #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
+ *
+ * Since multiple servos can (and likely are) connected to the same PWM
+ * peripheral, this has to be initialized only once. Great care has to be
+ * taken that all servos connected to the same PWM peripheral expect the same
+ * PWM configuration.
  */
 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   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
  */
-int servo_init(servo_t *dev, pwm_t pwm, int pwm_channel, unsigned int min, unsigned int max);
+#define SERVO_DUTY(pwm_freq, pwm_res, duration) \
+    DIV_ROUND(1ULL * (duration) * (pwm_freq) * (pwm_res), US_PER_SEC)
 
 /**
- * @brief   Set the servo motor to a specified position
+ * @brief   Configuration parameters for a servo
+ */
+typedef struct {
+    /***
+     * @brief   Specification of the PWM device the servo is connected to
+     * */
+    const servo_pwm_params_t *pwm;
+    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 */
+    uint8_t pwm_chan;   /**< PWM channel of @p pwm the servo is connected to */
+} servo_params_t;
+
+/**
+ * @brief   Servo device state
+ */
+typedef struct {
+    const servo_params_t *params;   /**< Parameters of this servo */
+    /**
+     * @brief PWM duty cycle 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_duty;
+    /**
+     * @brief PWM duty cycle 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_duty;
+} servo_t;
+
+/**
+ * @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.
+ * @params[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.
+ * @pre     The PWM device @p pwm_params refers to has been initialized using
+ *          @ref servo_init_pwm
+ *
+ * @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(const 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 <marian.buschsieweke@ovgu.de>
+ *
+ * @}
+ */
+
+#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

@@ -335,4 +335,8 @@ void saul_init_devs(void)
         extern void auto_init_veml6070(void);
         auto_init_veml6070();
     }
+    if (IS_USED(MODULE_SERVO)) {
+        extern void auto_init_servo(void);
+        auto_init_servo();
+    }
 }

drivers/servo/Makefile

@@ -1,3 +1 @@
-MODULE = servo
-
-include $(RIOTBASE)/Makefile.base
+include $(RIOTMAKE)/driver_with_saul.mk

drivers/servo/Makefile.include

@@ -0,0 +1,2 @@
+USEMODULE_INCLUDES_servo := $(LAST_MAKEFILEDIR)/include
+USEMODULE_INCLUDES += $(USEMODULE_INCLUDES_servo)