From 071fc182bcf4e2e69591a781805b63f8e251f3f6 Mon Sep 17 00:00:00 2001 From: Richard Hirst Date: Sun, 1 Sep 2013 14:58:14 +0100 Subject: [PATCH] Add --idle-timeout and make install features for servod --- ServoBlaster/README.txt | 52 +++- ServoBlaster/{ => kernel}/Kbuild | 0 ServoBlaster/{ => kernel}/Makefile | 18 +- ServoBlaster/{ => kernel}/servoblaster.c | 12 + ServoBlaster/{ => kernel}/servoblaster.h | 0 ServoBlaster/{ => kernel}/servoblaster.ko | Bin .../udev_scripts/20-servoblaster.rules | 0 .../{ => kernel}/udev_scripts/servoblaster | 0 ServoBlaster/servodemo.c | 4 - ServoBlaster/user/Makefile | 24 ++ ServoBlaster/user/init-script | 41 +++ ServoBlaster/{ => user}/servod.c | 269 +++++++++++++++--- 12 files changed, 350 insertions(+), 70 deletions(-) rename ServoBlaster/{ => kernel}/Kbuild (100%) rename ServoBlaster/{ => kernel}/Makefile (79%) rename ServoBlaster/{ => kernel}/servoblaster.c (98%) rename ServoBlaster/{ => kernel}/servoblaster.h (100%) rename ServoBlaster/{ => kernel}/servoblaster.ko (100%) rename ServoBlaster/{ => kernel}/udev_scripts/20-servoblaster.rules (100%) rename ServoBlaster/{ => kernel}/udev_scripts/servoblaster (100%) delete mode 100644 ServoBlaster/servodemo.c create mode 100644 ServoBlaster/user/Makefile create mode 100755 ServoBlaster/user/init-script rename ServoBlaster/{ => user}/servod.c (68%) diff --git a/ServoBlaster/README.txt b/ServoBlaster/README.txt index 7cd76f6..f4865d2 100644 --- a/ServoBlaster/README.txt +++ b/ServoBlaster/README.txt @@ -32,12 +32,14 @@ to P1 header pins as follows: 0 4 P1-7 1 17 P1-11 2 18 P1-12 - 3 21 P1-13 + 3 21/27 P1-13 4 22 P1-15 5 23 P1-16 6 24 P1-18 7 25 P1-22 +P1-13 is connected to either GPIO-21 or GPIO-27, depending on board revision. + When the driver is first loaded the GPIO pins are configure to be outputs, and their pulse widths are set to 0. This is so that servos don't jump to some arbitrary position when you load the driver. Once you know where you want your @@ -95,12 +97,15 @@ enough to crash the Pi! -There are two implementions of ServoBlaster; a kernel module based one, and -a user space daemon. The kernel module based one is the original, and is -more mature. The user space daemon implementation is much more convenient to -use but is less well tested and does not have all the features of the kernel -based one. I would recommend you try the user space implementation first, as -it is likely to be easier to get going. +There are two implementions of ServoBlaster; a kernel module based one, and a +user space daemon. The kernel module based one is the original, but is more of +a pain to build because you need a matching kernel build. The user space +daemon implementation is much more convenient to use but does not have all the +features of the kernel based one. I would recommend you try the user space +implementation first, as it is likely to be easier to get going. + +The kernel module implementation is in the subdirectory 'kernel', while the +user space implementation can be found in subdirectory 'user'. Details specific to each implementation are provided in separate sections below. @@ -135,18 +140,37 @@ $ sudo ./servod --pcm Using hardware: PCM ... -Features not currently supported in the user space implementation: +Some people have requested that a servo output turns off automatically if no +new pulse width has been requested recently, and I've had two reports of +servos overheating when driven for long periods of time. To support this +request, servod implements an idle timeout which can be specified at +module load time. The value is specified in milliseconds, so if you want +to drive your servos for 2 seconds following each new width request you would +do this: -- does not support the timeout= option to turn off servo outputs after - some specified delay. You must set a servo width to 0 to turn off an - output, if you want to. -- you cannot read /dev/servoblaster to see the current servo settings +$ sudo ./servod --idle-timeout=2000 + +Typical small servos take a few 100 milliseconds to rotate from one extreme +to the other, so for small values of idle_timeout you might find the control +pulse is turned off before your servo has reached the required position. +idle_timeout defaults to 0, which disables the feature. + +If you want servod to start automatically when the system boots, then you +can install it along with a startup script as follows: + +$ sudo make install + +You may wish to edit /etc/init.d/servoblaster to change the parameters that are +specified in that script (e.g. the idle-timeout, which is set to 2 seconds). The kernel space implementation ------------------------------- +Please note that the user space implementation is the preferred one to use and +the kernel implementation (servoblaster.ko) has been depreciated. + Upon reading /dev/servoblaster, the device file provides feedback as to what position each servo is currently set. For example, after starting the driver and sending the command "3=120", you would see: @@ -176,6 +200,10 @@ on the wiki (http://elinux.org/RPi_Kernel_Compilation) to compile the kernel, then edit the servoblaster Makefile to point at your kernel tree, then build servoblaster. +As the mapping of GPIO to P1 header pins changed between Rev 1 and Rev 2 +boards, you will need to modify servoblaster.c approriately for your board. +Please uncomment the define for REV_1 or REV_2 as appropriate. + It is not currently possible to make the kernel implementation use the PCM hardware rather than the PWM hardware, therefore it will interfere with 3.5mm jack audio output. diff --git a/ServoBlaster/Kbuild b/ServoBlaster/kernel/Kbuild similarity index 100% rename from ServoBlaster/Kbuild rename to ServoBlaster/kernel/Kbuild diff --git a/ServoBlaster/Makefile b/ServoBlaster/kernel/Makefile similarity index 79% rename from ServoBlaster/Makefile rename to ServoBlaster/kernel/Makefile index 4f5ba1b..83dea8c 100644 --- a/ServoBlaster/Makefile +++ b/ServoBlaster/kernel/Makefile @@ -3,20 +3,14 @@ INSTALL_PATH := /lib/modules/$(shell /bin/uname -r)/kernel/drivers/misc/servobla #CROSS_OPTS := CROSS_COMPILE=/usr/bin/arm-linux-gnueabi- ARCH=arm CROSS_OPTS := -.PHONY: all install install_autostart -all: servod servoblaster.ko +.PHONY: all install install_autostart uninstall +all: servoblaster.ko servoblaster.ko: servoblaster.c servoblaster.h @[ -d ${KERNEL_TREE} ] || { echo "Edit Makefile to set KERNEL_TREE to point at your kernel"; exit 1; } @[ -e ${KERNEL_TREE}/Module.symvers ] || { echo "KERNEL_TREE/Module.symvers does not exist, you need to configure and compile your kernel"; exit 1; } make -C ${KERNEL_TREE} ${CROSS_OPTS} M=$(PWD) modules -servodemo: servodemo.c servoblaster.h - gcc -Wall -g -O2 -o servodemo servodemo.c -Wl,--export-dynamic `pkg-config --cflags gtk+-3.0 gmodule-export-2.0` `pkg-config --libs gtk+-3.0 gmodule-export-2.0` - -servod: servod.c - gcc -Wall -g -O2 -o servod servod.c - install: servoblaster.ko @sudo cp $(PWD)/udev_scripts/servoblaster /lib/udev @sudo cp $(PWD)/udev_scripts/20-servoblaster.rules /etc/udev/rules.d @@ -34,7 +28,13 @@ install_autostart: install @echo " modprobe servoblaster" @echo " modprobe -r servoblaster" +uninstall: + @modprobe -r servoblaster + @sudo rm -f /lib/udev/servoblaster + @sudo rm -f /etc/udev/rules.d/20-servoblaster.rules + @sudo rm -f $(INSTALL_PATH)/servoblaster.ko + @sudo depmod -a + clean: make -C ${KERNEL_TREE} ${CROSS_OPTS} M=$(PWD) clean - rm -f servod servodemo diff --git a/ServoBlaster/servoblaster.c b/ServoBlaster/kernel/servoblaster.c similarity index 98% rename from ServoBlaster/servoblaster.c rename to ServoBlaster/kernel/servoblaster.c index d98ad0b..3c6295e 100644 --- a/ServoBlaster/servoblaster.c +++ b/ServoBlaster/kernel/servoblaster.c @@ -107,6 +107,12 @@ static struct file_operations fops = .compat_ioctl = dev_ioctl, }; +// Define REV_1 or REV_2 depending on whcih rev of Pi you have. Alternatively +// just don't try to use P1-13. + +//#define REV_1 +//#define REV_2 + // Map servo channels to GPIO pins static uint8_t servo2gpio[] = { 4, // P1-7 @@ -116,7 +122,13 @@ static uint8_t servo2gpio[] = { #else 18, // P1-12 #endif +#if defined(REV_1) 21, // P1-13 +#elif defined(REV_2) + 27, // P1-13 +#else + #error "You must define REV_1 or REV_2" +#endif 22, // P1-15 23, // P1-16 24, // P1-18 diff --git a/ServoBlaster/servoblaster.h b/ServoBlaster/kernel/servoblaster.h similarity index 100% rename from ServoBlaster/servoblaster.h rename to ServoBlaster/kernel/servoblaster.h diff --git a/ServoBlaster/servoblaster.ko b/ServoBlaster/kernel/servoblaster.ko similarity index 100% rename from ServoBlaster/servoblaster.ko rename to ServoBlaster/kernel/servoblaster.ko diff --git a/ServoBlaster/udev_scripts/20-servoblaster.rules b/ServoBlaster/kernel/udev_scripts/20-servoblaster.rules similarity index 100% rename from ServoBlaster/udev_scripts/20-servoblaster.rules rename to ServoBlaster/kernel/udev_scripts/20-servoblaster.rules diff --git a/ServoBlaster/udev_scripts/servoblaster b/ServoBlaster/kernel/udev_scripts/servoblaster similarity index 100% rename from ServoBlaster/udev_scripts/servoblaster rename to ServoBlaster/kernel/udev_scripts/servoblaster diff --git a/ServoBlaster/servodemo.c b/ServoBlaster/servodemo.c deleted file mode 100644 index 5019e79..0000000 --- a/ServoBlaster/servodemo.c +++ /dev/null @@ -1,4 +0,0 @@ -int main(int argc, char **argv) -{ - return 0; -} diff --git a/ServoBlaster/user/Makefile b/ServoBlaster/user/Makefile new file mode 100644 index 0000000..85eb9ce --- /dev/null +++ b/ServoBlaster/user/Makefile @@ -0,0 +1,24 @@ + +.PHONY: all install uninstall +all: servod + +servod: servod.c + gcc -Wall -g -O2 -o servod servod.c + +install: servod + [ "`id -u`" = "0" ] || { echo "Must be run as root"; exit 1; } + cp -f servod /usr/local/sbin + cp -f init-script /etc/init.d/servoblaster + update-rc.d servoblaster defaults 92 08 + /etc/init.d/servoblaster start + +uninstall: + [ "`id -u`" = "0" ] || { echo "Must be run as root"; exit 1; } + [ -e /etc/init.d/servoblaster ] && /etc/init.d/servoblaster stop || : + update-rc.d servoblaster remove + rm -f /usr/local/sbin/servod + rm -f /etc/init.d/servoblaster + +clean: + rm -f servod + diff --git a/ServoBlaster/user/init-script b/ServoBlaster/user/init-script new file mode 100755 index 0000000..269b053 --- /dev/null +++ b/ServoBlaster/user/init-script @@ -0,0 +1,41 @@ +#!/bin/sh +### BEGIN INIT INFO +# Provides: servoblaster +# Required-Start: hostname $local_fs +# Required-Stop: +# Should-Start: +# Default-Start: 2 3 4 5 +# Default-Stop: 0 1 6 +# Short-Description: Start/stop servod. +# Description: This script starts/stops servod. +### END INIT INFO + +PATH=/sbin:/usr/sbin:/bin:/usr/bin +. /lib/init/vars.sh + +OPTS="--idle-timeout=2000" + +res=0 + +case "$1" in + start) + /usr/local/sbin/servod $OPTS >/dev/null + ;; + restart|reload|force-reload) + killall servod + /usr/local/sbin/servod $OPTS >/dev/null + ;; + stop) + killall servod + ;; + status) + [ -e /dev/servoblaster ] || res=4 + ;; + *) + echo "Usage: servoblaster [start|stop|status]" >&2 + res=3 + ;; +esac + +exit $res + diff --git a/ServoBlaster/servod.c b/ServoBlaster/user/servod.c similarity index 68% rename from ServoBlaster/servod.c rename to ServoBlaster/user/servod.c index d22ed0f..6645e74 100644 --- a/ServoBlaster/servod.c +++ b/ServoBlaster/user/servod.c @@ -34,8 +34,15 @@ #include #include #include +#include -static uint8_t servo2gpio[] = { +/* Define which GPIOs/P1-pins to use slightly differently for Rev 1 and + * Rev 2 boards, as P1-13 is different. On Rev 1 GPIO 27 was used for + * camera control and GPIO-21 was routed to P1-13, but on Rev 2 it is the + * other way round. + */ + +static uint8_t servo2gpio_rev1[] = { 4, // P1-7 17, // P1-11 18, // P1-12 @@ -46,7 +53,19 @@ static uint8_t servo2gpio[] = { 25, // P1-22 }; -#define NUM_SERVOS (sizeof(servo2gpio)/sizeof(servo2gpio[0])) +static uint8_t servo2gpio_rev2[] = { + 4, // P1-7 + 17, // P1-11 + 18, // P1-12 + 27, // P1-13 + 22, // P1-15 + 23, // P1-16 + 24, // P1-18 + 25, // P1-22 +}; + +static uint8_t *servo2gpio; +static int num_servos; #define DEVFILE "/dev/servoblaster" @@ -63,7 +82,7 @@ static uint8_t servo2gpio[] = { #define CYCLE_TIME_US 20000 #define SAMPLE_US 10 -#define SERVO_TIME_US (CYCLE_TIME_US/NUM_SERVOS) +#define SERVO_TIME_US (CYCLE_TIME_US/num_servos) #define SERVO_SAMPLES (SERVO_TIME_US/SAMPLE_US) #define SERVO_MIN 0 #define SERVO_MAX (SERVO_SAMPLES - 1) @@ -165,27 +184,12 @@ static volatile uint32_t *gpio_reg; static int delay_hw = DELAY_VIA_PWM; +static struct timeval *servo_kill_time; + +static int idle_timeout = 0; + static void set_servo(int servo, int width); -static void -gpio_set_mode(uint32_t pin, uint32_t mode) -{ - uint32_t fsel = gpio_reg[GPIO_FSEL0 + pin/10]; - - fsel &= ~(7 << ((pin % 10) * 3)); - fsel |= mode << ((pin % 10) * 3); - gpio_reg[GPIO_FSEL0 + pin/10] = fsel; -} - -static void -gpio_set(int pin, int level) -{ - if (level) - gpio_reg[GPIO_SET0] = 1 << pin; - else - gpio_reg[GPIO_CLR0] = 1 << pin; -} - static void udelay(int us) { @@ -200,7 +204,7 @@ terminate(int dummy) int i; if (dma_reg && virtbase) { - for (i = 0; i < NUM_SERVOS; i++) + for (i = 0; i < num_servos; i++) set_servo(i, 0); udelay(CYCLE_TIME_US); dma_reg[DMA_CS] = DMA_RESET; @@ -221,6 +225,78 @@ fatal(char *fmt, ...) terminate(0); } +static void +init_idle_timers(void) +{ + servo_kill_time = calloc(num_servos, sizeof(struct timeval)); + if (!servo_kill_time) + fatal("servod: calloc() failed\n"); +} + +static void +update_idle_time(int servo) +{ + if (idle_timeout == 0) + return; + + gettimeofday(servo_kill_time + servo, NULL); + servo_kill_time[servo].tv_sec += idle_timeout / 1000; + servo_kill_time[servo].tv_usec += (idle_timeout % 1000) * 1000; + while (servo_kill_time[servo].tv_usec >= 1000000) { + servo_kill_time[servo].tv_usec -= 1000000; + servo_kill_time[servo].tv_sec++; + } +} + +static void +get_next_idle_timeout(struct timeval *tv) +{ + int i; + struct timeval now; + struct timeval min = { 60, 0 }; + long this_diff, min_diff; + + gettimeofday(&now, NULL); + for (i = 0; i < num_servos; i++) { + if (servo_kill_time[i].tv_sec == 0) + continue; + else if (servo_kill_time[i].tv_sec < now.tv_sec || + (servo_kill_time[i].tv_sec == now.tv_sec && + servo_kill_time[i].tv_usec <= now.tv_usec)) { + servo_kill_time[i].tv_sec = 0; + set_servo(i, 0); + } else { + this_diff = (servo_kill_time[i].tv_sec - now.tv_sec) * 1000000 + + servo_kill_time[i].tv_usec - now.tv_usec; + min_diff = min.tv_sec * 1000000 + min.tv_usec; + if (this_diff < min_diff) { + min.tv_sec = this_diff / 1000000; + min.tv_usec = this_diff % 1000000; + } + } + } + *tv = min; +} + +static void +gpio_set_mode(uint32_t pin, uint32_t mode) +{ + uint32_t fsel = gpio_reg[GPIO_FSEL0 + pin/10]; + + fsel &= ~(7 << ((pin % 10) * 3)); + fsel |= mode << ((pin % 10) * 3); + gpio_reg[GPIO_FSEL0 + pin/10] = fsel; +} + +static void +gpio_set(int pin, int level) +{ + if (level) + gpio_reg[GPIO_SET0] = 1 << pin; + else + gpio_reg[GPIO_CLR0] = 1 << pin; +} + static uint32_t mem_virt_to_phys(void *virt) { @@ -265,6 +341,7 @@ set_servo(int servo, int width) dp[i] = 0; dp[0] = mask; cbp->dst = phys_gpset0; + update_idle_time(servo); } } @@ -335,7 +412,7 @@ init_ctrl_data(void) phys_fifo_addr = (PCM_BASE | 0x7e000000) + 0x04; memset(ctl->sample, 0, sizeof(ctl->sample)); - for (servo = 0 ; servo < NUM_SERVOS; servo++) { + for (servo = 0 ; servo < num_servos; servo++) { for (i = 0; i < SERVO_SAMPLES; i++) ctl->sample[servo * SERVO_SAMPLES + i] = 1 << servo2gpio[servo]; } @@ -425,49 +502,151 @@ init_hardware(void) static void go_go_go(void) { - FILE *fp; + int fd; + struct timeval tv; + static char line[128]; + int nchars = 0; + char nl; - if ((fp = fopen(DEVFILE, "r+")) == NULL) + if ((fd = open(DEVFILE, O_RDWR|O_NONBLOCK)) == -1) fatal("servod: Failed to open %s: %m\n", DEVFILE); - char *lineptr = NULL, nl; - size_t linelen; - for (;;) { int n, width, servo; + fd_set ifds; - if ((n = getline(&lineptr, &linelen, fp)) < 0) + FD_ZERO(&ifds); + FD_SET(fd, &ifds); + get_next_idle_timeout(&tv); + if ((n = select(fd+1, &ifds, NULL, NULL, &tv)) != 1) continue; - //fprintf(stderr, "[%d]%s", n, lineptr); - n = sscanf(lineptr, "%d=%d%c", &servo, &width, &nl); - if (n !=3 || nl != '\n') { - fprintf(stderr, "Bad input: %s", lineptr); - } else if (servo < 0 || servo >= NUM_SERVOS) { - fprintf(stderr, "Invalid servo number %d\n", servo); - } else if (width < SERVO_MIN || width > SERVO_MAX) { - fprintf(stderr, "Invalid width %d\n", width); - } else { - set_servo(servo, width); + while (read(fd, line+nchars, 1) == 1) { + if (line[nchars] == '\n') { + line[++nchars] = '\0'; + nchars = 0; + n = sscanf(line, "%d=%d%c", &servo, &width, &nl); + if (n !=3 || nl != '\n') { + fprintf(stderr, "Bad input: %s", line); + } else if (servo < 0 || servo >= num_servos) { + fprintf(stderr, "Invalid servo number %d\n", servo); + } else if (width < SERVO_MIN || width > SERVO_MAX) { + fprintf(stderr, "Invalid width %d\n", width); + } else { + set_servo(servo, width); + } + } else { + if (++nchars >= 126) { + fprintf(stderr, "Input too long\n"); + nchars = 0; + } + } } } } +/* Determining the board revision is a lot more complicated than it should be + * (see comments in wiringPi for details). We will just look at the last two + * digits of the Revision string and treat '00' and '01' as errors, '02' and + * '03' as rev 1, and any other hex value as rev 2. + */ +static int +board_rev(void) +{ + char buf[128]; + char *ptr, *end, *res; + static int rev = 0; + FILE *fp; + + if (rev) + return rev; + + fp = fopen("/proc/cpuinfo", "r"); + + if (!fp) + fatal("Unable to open /proc/cpuinfo: %m\n"); + + while ((res = fgets(buf, 128, fp))) { + if (!strncmp(buf, "Revision", 8)) + break; + } + fclose(fp); + + if (!res) + fatal("servod: No 'Revision' record in /proc/cpuinfo\n"); + + ptr = buf + strlen(buf) - 3; + rev = strtol(ptr, &end, 16); + if (end != ptr + 2) + fatal("servod: Failed to parse Revision string\n"); + if (rev < 1) + fatal("servod: Invalid board Revision\n"); + else if (rev < 4) + rev = 1; + else + rev = 2; + + return rev; +} + int main(int argc, char **argv) { int i; - // Very crude... - if (argc == 2 && !strcmp(argv[1], "--pcm")) - delay_hw = DELAY_VIA_PCM; + while (1) { + int c; + int option_index; + static struct option long_options[] = { + { "pcm", no_argument, 0, 'p' }, + { "idle-timeout", required_argument, 0, 't' }, + { "idle_timeout", required_argument, 0, 't' }, + { "help", no_argument, 0, 'h' }, + { 0, 0, 0, 0 } + }; + + c = getopt_long(argc, argv, "hpt:", long_options, &option_index); + if (c == -1) { + break; + } else if (c == 'p') { + delay_hw = DELAY_VIA_PCM; + } else if (c == 't') { + idle_timeout = atoi(optarg); + if (idle_timeout < 10 || idle_timeout > 3600000) + fatal("Invalid idle_timeout specified\n"); + } else if (c == 'h') { + printf("\nUsage: %s [--pcm] [--idle-timeout=N]\n\n" + "Where:\n" + " --pcm tells servod to use PCM rather than PWM hardware\n" + " to implement delays\n" + " --idle-timeout=N tells servod to stop sending servo pulses for a\n" + " given output N milliseconds after the last update\n\n", + argv[0]); + exit(0); + } else { + fatal("Invalid parameter\n"); + } + } + + /* Select the correct pin mapping based on board rev */ + if (board_rev() == 1) { + servo2gpio = servo2gpio_rev1; + num_servos = sizeof(servo2gpio_rev1); + } else { + servo2gpio = servo2gpio_rev2; + num_servos = sizeof(servo2gpio_rev2); + } + + printf("Board revision: %5d\n", board_rev()); printf("Using hardware: %5s\n", delay_hw == DELAY_VIA_PWM ? "PWM" : "PCM"); - printf("Number of servos: %5d\n", NUM_SERVOS); + printf("Idle timeout (ms): %5d\n", idle_timeout); + printf("Number of servos: %5d\n", num_servos); printf("Servo cycle time: %5dus\n", CYCLE_TIME_US); printf("Pulse width units: %5dus\n", SAMPLE_US); printf("Maximum width value: %5d (%dus)\n", SERVO_MAX, SERVO_MAX * SAMPLE_US); + init_idle_timers(); setup_sighandlers(); dma_reg = map_peripheral(DMA_BASE, DMA_LEN); @@ -486,7 +665,7 @@ main(int argc, char **argv) make_pagemap(); - for (i = 0; i < NUM_SERVOS; i++) { + for (i = 0; i < num_servos; i++) { gpio_set(servo2gpio[i], 0); gpio_set_mode(servo2gpio[i], GPIO_MODE_OUT); }