drivers/servo: Handle inexact frequencies from periph_pwm

drivers/include/servo.h

@@ -1,5 +1,6 @@
 /*
  * 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
@@ -16,6 +17,7 @@
  * @brief       High-level driver for easy handling of servo motors
  *
  * @author      Hauke Petersen <hauke.petersen@fu-berlin.de>
+ * @author      Joakim Gebart <joakim.gebart@eistec.se>
  */
 
 #ifndef SERVO_H
@@ -31,10 +33,12 @@ extern "C" {
  * @brief Descriptor struct for a servo
  */
 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 */
+    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;
 
 /**

drivers/servo/servo.c

@@ -1,5 +1,6 @@
 /*
  * 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
@@ -14,33 +15,88 @@
  * @brief       Servo motor driver implementation
  *
  * @author      Hauke Petersen <hauke.petersen@fu-berlin.de>
+ * @author      Joakim Gebart <joakim.gebart@eistec.se>
  *
  * @}
  */
 
 #include "servo.h"
 #include "periph/pwm.h"
+#include "timex.h" /* for SEC_IN_USEC */
+
+#define ENABLE_DEBUG    (0)
+#include "debug.h"
 
 #define FREQUENCY       (100U)
-#define RESOLUTION      (10000U)
+#define RESOLUTION      (SEC_IN_USEC / FREQUENCY)
 
 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(dev->device, PWM_LEFT, FREQUENCY, RESOLUTION);
+
+    DEBUG("servo: requested %d hz, got %d hz\n", FREQUENCY, actual_frequency);
+
+    if (actual_frequency < 0) {
+        /* PWM error */
+        return -1;
+    }
     dev->device = pwm;
     dev->channel = pwm_channel;
     dev->min = min;
     dev->max = max;
 
-    return pwm_init(dev->device, PWM_LEFT, FREQUENCY, RESOLUTION);
+    /* 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 = FREQUENCY;
+
+    return 0;
 }
 
 int servo_set(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;
     }
-    return pwm_set(dev->device, dev->channel, pos);
+
+    /* 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);
+
+    return pwm_set(dev->device, dev->channel, raw_value);
 }

tests/driver_servo/README.md

@@ -14,4 +14,3 @@ long. The requested frequency is 100 Hz, but due to hardware limitations it
 might not be possible to achieve the selected frequency. The pulse width
 should, however, remain the same, only the frequency of pulses (and hence the
 duty cycle) should differ.
-