mirror of
https://github.com/richardghirst/PiBits.git
synced 2024-11-28 12:24:11 +01:00
Added ServoBlaster
This commit is contained in:
Richard Hirst
parent
257ab3065a
commit
d8df9b7625
11
README.md
11
README.md
@ -1,2 +1,11 @@
|
|||||||
PiBits
|
PiBits
|
||||||
======
|
======
|
||||||
|
|
||||||
|
Here you'll find various RaspberryPi related projcts:
|
||||||
|
|
||||||
|
ServoBlaster: A kernel driver that lets you control 8 (or more) servos from your RaspberryPi.
|
||||||
|
|
||||||
|
Panalyzer: A Pi based Logic Analyzer. This can be found in a separate repository alongside this one, but is mentioned here for completeness.
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
10
ServoBlaster/Kbuild
Normal file
10
ServoBlaster/Kbuild
Normal file
@ -0,0 +1,10 @@
|
|||||||
|
obj-m = servoblaster.o
|
||||||
|
|
||||||
|
KERNEL_TREE := /home/richard/Pi/git/linux
|
||||||
|
|
||||||
|
module:
|
||||||
|
make -C ${KERNEL_TREE} ARCH=arm CROSS_COMPILE=/usr/bin/arm-linux-gnueabi- M=$(PWD) modules
|
||||||
|
|
||||||
|
clean:
|
||||||
|
make -C ${KERNEL_TREE} ARCH=arm CROSS_COMPILE=/usr/bin/arm-linux-gnueabi- M=$(PWD) clean
|
||||||
|
|
||||||
14
ServoBlaster/Makefile
Normal file
14
ServoBlaster/Makefile
Normal file
@ -0,0 +1,14 @@
|
|||||||
|
KERNEL_TREE := /home/richard/Pi/git/linux
|
||||||
|
|
||||||
|
all: servoblaster.ko servodemo
|
||||||
|
|
||||||
|
servoblaster.ko: servoblaster.c servoblaster.h
|
||||||
|
make -C ${KERNEL_TREE} ARCH=arm CROSS_COMPILE=/usr/bin/arm-linux-gnueabi- 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`
|
||||||
|
|
||||||
|
clean:
|
||||||
|
make -C ${KERNEL_TREE} ARCH=arm CROSS_COMPILE=/usr/bin/arm-linux-gnueabi- M=$(PWD) clean
|
||||||
|
rm -f servodemo
|
||||||
|
|
||||||
82
ServoBlaster/README.txt
Normal file
82
ServoBlaster/README.txt
Normal file
@ -0,0 +1,82 @@
|
|||||||
|
ServoBlaster
|
||||||
|
|
||||||
|
This is a Linux kernel driver for the RaspberryPi, which provides an interface
|
||||||
|
to drive multiple servos via the GPIO pins. You control the servo postions by
|
||||||
|
sending commands to the driver saying what pulse width a particular servo
|
||||||
|
output should use. The driver maintains that pulse width until you send a new
|
||||||
|
command requesting some other width.
|
||||||
|
|
||||||
|
Currently is it configured to drive 8 servos. Servos typically need an active
|
||||||
|
high pulse of somewhere between 0.5ms and 2.5ms, where the pulse width controls
|
||||||
|
the position of the servo. The pulse should be repeated approximately every
|
||||||
|
20ms, although pulse frequency is not critical. The pulse width is critical,
|
||||||
|
as that translates directly to the servo position.
|
||||||
|
|
||||||
|
The driver creates a device file, /dev/servoblaster, in to which you can send
|
||||||
|
commands. The command format is "<servo-number>=<sero-position>", where servo
|
||||||
|
number is a number from 0 to 7 inclusive, and servo position is the pulse width
|
||||||
|
you want in units of 10us. So, if you want to set servo 3 to a pulse width of
|
||||||
|
1.2ms you could do this at the shell prompt:
|
||||||
|
|
||||||
|
echo 3=120 > /dev/servoblaster
|
||||||
|
|
||||||
|
120 is in units os 10us, so that is 1200us, or 1.2ms.
|
||||||
|
|
||||||
|
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 postion when you load the driver. Once you know where you want your
|
||||||
|
servos positioned, write a value to /dev/servoblaster to enable the respective
|
||||||
|
output. When the driver is unloaded it attempts to shut down the outputs
|
||||||
|
cleanly, rather than cutting some pulse short and causing a servo position to
|
||||||
|
jump.
|
||||||
|
|
||||||
|
The driver allocates a timeslot of 2.5ms to each output (8 servos resulting in
|
||||||
|
a cycle time of 20ms). A servo output is set high at the start of its 2.5ms
|
||||||
|
timeslot, and set low after the appropriate delay. There is then a further
|
||||||
|
delay to take us to the end of that timeslot before the next servo output is
|
||||||
|
set high. This way there is only ever one servo output active at a time, which
|
||||||
|
helps keep the code simple.
|
||||||
|
|
||||||
|
The driver works by setting up a linked list of DMA control blocks with the
|
||||||
|
last one linked back to the first, so once initialized the DMA controller
|
||||||
|
cycles round continuously and the driver does not need to get involved except
|
||||||
|
when a pulse width needs to be changed. For a given servo there are four DMA
|
||||||
|
control blocks; the first transfers a single word to the GPIO 'set output'
|
||||||
|
register, the second transfers some number of words to the PWM FIFO to generate
|
||||||
|
the required pulse width time, the third transfers a single word to the GPIO
|
||||||
|
'clear output' register, and the fourth transfers a number of words to the PWM
|
||||||
|
FIFO to generate a delay up to the end of the 2.5ms timeslot.
|
||||||
|
|
||||||
|
While the driver does use the PWM peripheral, it only uses it to pace the DMA
|
||||||
|
transfers, so as to generate accurate delays. The PWM is set up such that it
|
||||||
|
consumes one word from the FIFO every 10us, so to generate a delay of 1.2ms the
|
||||||
|
driver sets the DMA transfer count to 480 (1200/10*4, as the FIFO is 32 bits
|
||||||
|
wide). The PWM is set to request data as soon as there is a single word free
|
||||||
|
in the FIFO, so there should be no burst transfers to upset the timing.
|
||||||
|
|
||||||
|
I used Panalyzer running on one Pi to mointor the servo outputs from a second
|
||||||
|
Pi. The pulse widths and frequencies seem very stable, even under heavy SD
|
||||||
|
card use. This is expected, because the pulse generation is effectively
|
||||||
|
handled in hardware and not influenced by interrupt latency or scheduling
|
||||||
|
effects.
|
||||||
|
|
||||||
|
Please read the driver source for more details, such as which GPIO pin maps to
|
||||||
|
which servo number. The comments at the top of servoblaster.c also explain how
|
||||||
|
to make your system create the /dev/servoblaster device node automatically when
|
||||||
|
the driver is loaded.
|
||||||
|
|
||||||
|
The driver uses DMA channel 0, and PWM channel 1. It makes no attempt to
|
||||||
|
protect against other code using those peripherals. It sets the relevant GPIO
|
||||||
|
pins to be outputs when the driver is loaded, so please ensure that you are not
|
||||||
|
driving those pins externally.
|
||||||
|
|
||||||
|
I would of course recommend some buffering between the GPIO outputs and the
|
||||||
|
servo controls, to protect the Pi. That said, I'm living dangerously and doing
|
||||||
|
without :-) If you just want to experiment with a small servo you can probably
|
||||||
|
take the 5 volts for it from the header pins on the Pi, but I find that doing
|
||||||
|
anything non-trivial with four servos connected pulls the 5 volts down far
|
||||||
|
enough to crash the Pi!
|
||||||
|
|
||||||
|
|
||||||
|
Richard Hirst <richardghirst@gmail.com> August 2012
|
||||||
|
|
||||||
37
ServoBlaster/rock.sh
Executable file
37
ServoBlaster/rock.sh
Executable file
@ -0,0 +1,37 @@
|
|||||||
|
#!/bin/bash
|
||||||
|
|
||||||
|
# I have four servos driving four 'legs' on a robot. This script makes
|
||||||
|
# the legs go though something like a walking motion. It actually just
|
||||||
|
# skids back and fore on the spot at the moment, becuase it needs knees
|
||||||
|
# and lower legs before it can really lift up its feet and take a real
|
||||||
|
# step forward. This is just a quick demo though..
|
||||||
|
|
||||||
|
ss()
|
||||||
|
{
|
||||||
|
echo $1 > /dev/servoblaster
|
||||||
|
}
|
||||||
|
|
||||||
|
p[0]=132
|
||||||
|
p[1]=164
|
||||||
|
p[2]=150
|
||||||
|
p[3]=170
|
||||||
|
|
||||||
|
step=2
|
||||||
|
cnt=0
|
||||||
|
while true; do
|
||||||
|
for i in 0 1 2 3; do
|
||||||
|
ss $i=${p[$i]}
|
||||||
|
p[$i]=$[ p[$i] + step ]
|
||||||
|
sleep 0.05
|
||||||
|
done
|
||||||
|
cnt=$[ cnt + step ]
|
||||||
|
if [ $step -gt 0 ]; then
|
||||||
|
if [ $cnt -ge 20 ]; then
|
||||||
|
step=-2
|
||||||
|
fi
|
||||||
|
else
|
||||||
|
if [ $cnt -lt -20 ]; then
|
||||||
|
step=2
|
||||||
|
fi
|
||||||
|
fi
|
||||||
|
done
|
||||||
380
ServoBlaster/servoblaster.c
Normal file
380
ServoBlaster/servoblaster.c
Normal file
@ -0,0 +1,380 @@
|
|||||||
|
/*
|
||||||
|
* servoblaster.c Multiple Servo Driver for the RaspberryPi
|
||||||
|
* Copyright (c) 2012 Richard Hirst <richardghirst@gmail.com>
|
||||||
|
*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License version 2 as
|
||||||
|
* published by the Free Software Foundation.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with this program; if not, write to the Free Software
|
||||||
|
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/*
|
||||||
|
* If you want the device node created automatically create these two
|
||||||
|
* files, and make /lib/udev/servoblaster executable (chmod +x):
|
||||||
|
*
|
||||||
|
* ============= /etc/udev/rules.d/20-servoblaster.rules =============
|
||||||
|
* SUBSYSTEM=="module", DEVPATH=="/module/servoblaster", RUN+="/lib/udev/servoblaster"
|
||||||
|
* ===================================================================
|
||||||
|
*
|
||||||
|
* ===================== /lib/udev/servoblaster ======================
|
||||||
|
* #!/bin/bash
|
||||||
|
*
|
||||||
|
* if [ "$ACTION" = "remove" ]; then
|
||||||
|
* rm -f /dev/servoblaster
|
||||||
|
* elif [ "$ACTION" = "add" ]; then
|
||||||
|
* major=$( sed -n 's/ servoblaster//p' /proc/devices )
|
||||||
|
* [ "$major" ] && mknod -m 0666 /dev/servoblaster c $major 0
|
||||||
|
* fi
|
||||||
|
*
|
||||||
|
* exit 0
|
||||||
|
* ===================================================================
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <linux/module.h>
|
||||||
|
#include <linux/string.h>
|
||||||
|
#include <linux/fs.h>
|
||||||
|
#include <linux/io.h>
|
||||||
|
#include <linux/vmalloc.h>
|
||||||
|
#include <linux/cdev.h>
|
||||||
|
#include <linux/scatterlist.h>
|
||||||
|
#include <linux/delay.h>
|
||||||
|
#include <linux/sched.h>
|
||||||
|
#include <mach/platform.h>
|
||||||
|
#include <asm/uaccess.h>
|
||||||
|
#include <mach/dma.h>
|
||||||
|
#include "servoblaster.h"
|
||||||
|
|
||||||
|
#define GPIO_LEN 0xb4
|
||||||
|
#define DMA_LEN 0x24
|
||||||
|
#define PWM_BASE (BCM2708_PERI_BASE + 0x20C000)
|
||||||
|
#define PWM_LEN 0x28
|
||||||
|
#define CLK_BASE (BCM2708_PERI_BASE + 0x101000)
|
||||||
|
#define CLK_LEN 0xA8
|
||||||
|
|
||||||
|
#define GPFSEL0 (0x00/4)
|
||||||
|
#define GPFSEL1 (0x04/4)
|
||||||
|
#define GPSET0 (0x1c/4)
|
||||||
|
#define GPCLR0 (0x28/4)
|
||||||
|
|
||||||
|
#define PWM_CTL (0x00/4)
|
||||||
|
#define PWM_DMAC (0x08/4)
|
||||||
|
#define PWM_RNG1 (0x10/4)
|
||||||
|
#define PWM_FIFO (0x18/4)
|
||||||
|
|
||||||
|
#define PWMCLK_CNTL 40
|
||||||
|
#define PWMCLK_DIV 41
|
||||||
|
|
||||||
|
#define PWMCTL_MODE1 (1<<1)
|
||||||
|
#define PWMCTL_PWEN1 (1<<0)
|
||||||
|
#define PWMCTL_CLRF (1<<6)
|
||||||
|
#define PWMCTL_USEF1 (1<<5)
|
||||||
|
|
||||||
|
#define PWMDMAC_ENAB (1<<31)
|
||||||
|
// I think this means it requests as soon as there is one free slot in the FIFO
|
||||||
|
// which is what we want as burst DMA would mess up our timing..
|
||||||
|
#define PWMDMAC_THRSHLD ((15<<8)|(15<<0))
|
||||||
|
|
||||||
|
#define DMA_CS (BCM2708_DMA_CS/4)
|
||||||
|
#define DMA_CONBLK_AD (BCM2708_DMA_ADDR/4)
|
||||||
|
#define DMA_DEBUG (BCM2708_DMA_DEBUG/4)
|
||||||
|
|
||||||
|
#define BCM2708_DMA_END (1<<1) // Why is this not in mach/dma.h ?
|
||||||
|
#define BCM2708_DMA_NO_WIDE_BURSTS (1<<26)
|
||||||
|
|
||||||
|
static int dev_open(struct inode *, struct file *);
|
||||||
|
static int dev_close(struct inode *, struct file *);
|
||||||
|
static ssize_t dev_read(struct file *, char *, size_t, loff_t *);
|
||||||
|
static ssize_t dev_write(struct file *, const char *, size_t, loff_t *);
|
||||||
|
static long dev_ioctl(struct file *, unsigned int, unsigned long);
|
||||||
|
|
||||||
|
static struct file_operations fops =
|
||||||
|
{
|
||||||
|
.open = dev_open,
|
||||||
|
.read = dev_read,
|
||||||
|
.write = dev_write,
|
||||||
|
.release = dev_close,
|
||||||
|
.unlocked_ioctl = dev_ioctl,
|
||||||
|
.compat_ioctl = dev_ioctl,
|
||||||
|
};
|
||||||
|
|
||||||
|
// Map servo channels to GPIO pins
|
||||||
|
static uint8_t servo2gpio[] = {
|
||||||
|
4, // P1-7
|
||||||
|
17, // P1-11
|
||||||
|
#ifdef PWM0_ON_GPIO18
|
||||||
|
1, // P1-5 (GPIO-18, P1-12 is currently PWM0, for debug)
|
||||||
|
#else
|
||||||
|
18, // P1-12
|
||||||
|
#endif
|
||||||
|
21, // P1-13
|
||||||
|
22, // P1-15
|
||||||
|
23, // P1-16
|
||||||
|
24, // P1-18
|
||||||
|
25, // P1-22
|
||||||
|
};
|
||||||
|
#define NUM_SERVOS (sizeof(servo2gpio)/sizeof(servo2gpio[0]))
|
||||||
|
|
||||||
|
// Structure of our control data, stored in a 4K page, and accessed by dma controller
|
||||||
|
struct ctldata_s {
|
||||||
|
struct bcm2708_dma_cb cb[NUM_SERVOS * 4]; // gpio-hi, delay, gpio-lo, delay, for each servo output
|
||||||
|
uint32_t gpiodata[NUM_SERVOS]; // set-pin, clear-pin values, per servo output
|
||||||
|
uint32_t pwmdata; // the word we write to the pwm fifo
|
||||||
|
};
|
||||||
|
|
||||||
|
static struct ctldata_s *ctl;
|
||||||
|
static unsigned long ctldatabase;
|
||||||
|
static volatile uint32_t *gpio_reg;
|
||||||
|
static volatile uint32_t *dma_reg;
|
||||||
|
static volatile uint32_t *clk_reg;
|
||||||
|
static volatile uint32_t *pwm_reg;
|
||||||
|
|
||||||
|
static dev_t devno;
|
||||||
|
static struct cdev my_cdev;
|
||||||
|
static int my_major;
|
||||||
|
|
||||||
|
// Wait until we're not processing the given servo (actually wait until
|
||||||
|
// we are not processing the low period of the previous servo, or the
|
||||||
|
// high period of this one).
|
||||||
|
static int wait_for_servo(int servo)
|
||||||
|
{
|
||||||
|
local_irq_disable();
|
||||||
|
for (;;) {
|
||||||
|
int cb = (dma_reg[DMA_CONBLK_AD] - ((uint32_t)ctl->cb & 0x7fffffff)) / sizeof(ctl->cb[0]);
|
||||||
|
|
||||||
|
if (servo > 0) {
|
||||||
|
if (cb < servo*4-2 || cb > servo*4+2)
|
||||||
|
break;
|
||||||
|
} else {
|
||||||
|
if (cb > 2 && cb < NUM_SERVOS*4-2)
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
local_irq_enable();
|
||||||
|
set_current_state(TASK_INTERRUPTIBLE);
|
||||||
|
if (schedule_timeout(1))
|
||||||
|
return -EINTR;
|
||||||
|
local_irq_disable();
|
||||||
|
}
|
||||||
|
// Return with IRQs disabled!!!
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int init_module(void)
|
||||||
|
{
|
||||||
|
int res, i, s;
|
||||||
|
|
||||||
|
res = alloc_chrdev_region(&devno, 0, 1, "servoblaster");
|
||||||
|
if (res < 0) {
|
||||||
|
printk(KERN_WARNING "ServoBlaster: Can't allocated device number\n");
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
my_major = MAJOR(devno);
|
||||||
|
cdev_init(&my_cdev, &fops);
|
||||||
|
my_cdev.owner = THIS_MODULE;
|
||||||
|
my_cdev.ops = &fops;
|
||||||
|
res = cdev_add(&my_cdev, MKDEV(my_major, 0), 1);
|
||||||
|
if (res) {
|
||||||
|
printk(KERN_WARNING "ServoBlaster: Error %d adding device\n", res);
|
||||||
|
unregister_chrdev_region(devno, 1);
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
|
||||||
|
ctldatabase = __get_free_pages(GFP_KERNEL, 0);
|
||||||
|
printk(KERN_INFO "ServoBlaster: Control page is at 0x%lx\n", ctldatabase);
|
||||||
|
if (ctldatabase == 0) {
|
||||||
|
printk(KERN_WARNING "ServoBlaster: alloc_pages failed\n");
|
||||||
|
cdev_del(&my_cdev);
|
||||||
|
unregister_chrdev_region(devno, 1);
|
||||||
|
return -EFAULT;
|
||||||
|
}
|
||||||
|
ctl = (struct ctldata_s *)ctldatabase;
|
||||||
|
gpio_reg = (uint32_t *)ioremap(GPIO_BASE, GPIO_LEN);
|
||||||
|
dma_reg = (uint32_t *)ioremap(DMA_BASE, DMA_LEN);
|
||||||
|
clk_reg = (uint32_t *)ioremap(CLK_BASE, CLK_LEN);
|
||||||
|
pwm_reg = (uint32_t *)ioremap(PWM_BASE, PWM_LEN);
|
||||||
|
|
||||||
|
memset(ctl, 0, sizeof(*ctl));
|
||||||
|
|
||||||
|
// Set all servo control pins to be outputs
|
||||||
|
for (i = 0; i < NUM_SERVOS; i++) {
|
||||||
|
int gpio = servo2gpio[i];
|
||||||
|
int fnreg = gpio/10 + GPFSEL0;
|
||||||
|
int fnshft = (gpio %10) * 3;
|
||||||
|
gpio_reg[GPCLR0] = 1 << gpio;
|
||||||
|
gpio_reg[fnreg] = (gpio_reg[fnreg] & ~(7 << fnshft)) | (1 << fnshft);
|
||||||
|
}
|
||||||
|
#ifdef PWM0_ON_GPIO18
|
||||||
|
// Set pwm0 output on gpio18
|
||||||
|
gpio_reg[GPCLR0] = 1 << 18;
|
||||||
|
gpio_reg[GPFSEL1] = (gpio_reg[GPFSEL1] & ~(7 << 8*3)) | ( 2 << 8*3);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Build the DMA CB chain
|
||||||
|
for (s = 0; s < NUM_SERVOS; s++) {
|
||||||
|
int i = s*4;
|
||||||
|
// Set gpio high
|
||||||
|
ctl->gpiodata[s] = 1 << servo2gpio[s];
|
||||||
|
ctl->cb[i].info = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP;
|
||||||
|
ctl->cb[i].src = (uint32_t)(&ctl->gpiodata[s]) & 0x7fffffff;
|
||||||
|
// We clear the GPIO here initially, so outputs go to 0 on startup
|
||||||
|
// Once someone writes to /dev/servoblaster we'll patch it to a 'set'
|
||||||
|
ctl->cb[i].dst = ((GPIO_BASE + GPCLR0*4) & 0x00ffffff) | 0x7e000000;
|
||||||
|
ctl->cb[i].length = sizeof(uint32_t);
|
||||||
|
ctl->cb[i].stride = 0;
|
||||||
|
ctl->cb[i].next = (uint32_t)(ctl->cb + i + 1) & 0x7fffffff;
|
||||||
|
// delay
|
||||||
|
i++;
|
||||||
|
ctl->cb[i].info = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP | BCM2708_DMA_D_DREQ | BCM2708_DMA_PER_MAP(5);
|
||||||
|
ctl->cb[i].src = (uint32_t)(&ctl->pwmdata) & 0x7fffffff;
|
||||||
|
ctl->cb[i].dst = ((PWM_BASE + PWM_FIFO*4) & 0x00ffffff) | 0x7e000000;
|
||||||
|
ctl->cb[i].length = sizeof(uint32_t) * 100; // default 1000us high
|
||||||
|
ctl->cb[i].stride = 0;
|
||||||
|
ctl->cb[i].next = (uint32_t)(ctl->cb + i + 1) & 0x7fffffff;
|
||||||
|
// Set gpio lo
|
||||||
|
i++;
|
||||||
|
ctl->cb[i].info = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP;
|
||||||
|
ctl->cb[i].src = (uint32_t)(&ctl->gpiodata[s]) & 0x7fffffff;
|
||||||
|
ctl->cb[i].dst = ((GPIO_BASE + GPCLR0*4) & 0x00ffffff) | 0x7e000000;
|
||||||
|
ctl->cb[i].length = sizeof(uint32_t);
|
||||||
|
ctl->cb[i].stride = 0;
|
||||||
|
ctl->cb[i].next = (uint32_t)(ctl->cb + i + 1) & 0x7fffffff;
|
||||||
|
// delay
|
||||||
|
i++;
|
||||||
|
ctl->cb[i].info = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP | BCM2708_DMA_D_DREQ | BCM2708_DMA_PER_MAP(5);
|
||||||
|
ctl->cb[i].src = (uint32_t)(&ctl->pwmdata) & 0x7fffffff;
|
||||||
|
ctl->cb[i].dst = ((PWM_BASE + PWM_FIFO*4) & 0x00ffffff) | 0x7e000000;
|
||||||
|
ctl->cb[i].length = sizeof(uint32_t) * 150; // default 1500us, to give 2.5ms per servo
|
||||||
|
ctl->cb[i].stride = 0;
|
||||||
|
ctl->cb[i].next = (uint32_t)(ctl->cb + i + 1) & 0x7fffffff;
|
||||||
|
}
|
||||||
|
// Point last cb back to first one so it loops continuously
|
||||||
|
ctl->cb[NUM_SERVOS*4-1].next = (uint32_t)(ctl->cb) & 0x7fffffff;
|
||||||
|
|
||||||
|
// Initialise PWM
|
||||||
|
pwm_reg[PWM_CTL] = 0;
|
||||||
|
udelay(10);
|
||||||
|
clk_reg[PWMCLK_DIV] = 0x5A000000 | (32<<12); // set pwm div to 32 (19.2MHz/32 = 600KHz)
|
||||||
|
clk_reg[PWMCLK_CNTL] = 0x5A000011; // Source=osc and enable
|
||||||
|
pwm_reg[PWM_RNG1] = 6; // 600KHz/6 = 10us per FIFO write
|
||||||
|
udelay(10);
|
||||||
|
ctl->pwmdata = 1; // Give a pulse of one clock width for each fifo write
|
||||||
|
pwm_reg[PWM_DMAC] = PWMDMAC_ENAB | PWMDMAC_THRSHLD;
|
||||||
|
udelay(10);
|
||||||
|
pwm_reg[PWM_CTL] = PWMCTL_CLRF;
|
||||||
|
udelay(10);
|
||||||
|
pwm_reg[PWM_CTL] = PWMCTL_USEF1 | PWMCTL_PWEN1;
|
||||||
|
udelay(10);
|
||||||
|
|
||||||
|
// Initialise the DMA
|
||||||
|
dma_reg[DMA_CS] = BCM2708_DMA_RESET;
|
||||||
|
udelay(10);
|
||||||
|
dma_reg[DMA_CS] = BCM2708_DMA_INT | BCM2708_DMA_END;
|
||||||
|
dma_reg[DMA_CONBLK_AD] = (uint32_t)(ctl->cb) & 0x7fffffff;
|
||||||
|
dma_reg[DMA_DEBUG] = 7; // clear debug error flags
|
||||||
|
dma_reg[DMA_CS] = 0x10880001; // go, mid priority, wait for outstanding writes
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void cleanup_module(void)
|
||||||
|
{
|
||||||
|
int servo;
|
||||||
|
|
||||||
|
// Take care to stop servos with outputs low, so we don't get
|
||||||
|
// spurious movement on module unload
|
||||||
|
for (servo = 0; servo < NUM_SERVOS; servo++) {
|
||||||
|
// Wait until we're not driving this servo
|
||||||
|
if (wait_for_servo(servo))
|
||||||
|
break;
|
||||||
|
// Patch the control block so it stays low
|
||||||
|
ctl->cb[servo*4+0].dst = ((GPIO_BASE + GPCLR0*4) & 0x00ffffff) | 0x7e000000;
|
||||||
|
local_irq_enable();
|
||||||
|
}
|
||||||
|
// Wait 20ms to be sure it has finished it's cycle an all outputs are low
|
||||||
|
msleep(20);
|
||||||
|
// Now we can kill everything
|
||||||
|
dma_reg[DMA_CS] = BCM2708_DMA_RESET;
|
||||||
|
pwm_reg[PWM_CTL] = 0;
|
||||||
|
udelay(10);
|
||||||
|
free_pages(ctldatabase, 0);
|
||||||
|
iounmap(gpio_reg);
|
||||||
|
iounmap(dma_reg);
|
||||||
|
iounmap(clk_reg);
|
||||||
|
iounmap(pwm_reg);
|
||||||
|
cdev_del(&my_cdev);
|
||||||
|
unregister_chrdev_region(devno, 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
static int dev_open(struct inode *inod,struct file *fil)
|
||||||
|
{
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static ssize_t dev_read(struct file *filp,char *buf,size_t count,loff_t *f_pos)
|
||||||
|
{
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static ssize_t dev_write(struct file *filp,const char *buf,size_t count,loff_t *f_pos)
|
||||||
|
{
|
||||||
|
int servo;
|
||||||
|
int cnt;
|
||||||
|
int n;
|
||||||
|
char str[32];
|
||||||
|
char dummy;
|
||||||
|
|
||||||
|
cnt = count < 32 ? count : 31;
|
||||||
|
if (copy_from_user(str, buf, cnt)) {
|
||||||
|
return -EFAULT;
|
||||||
|
}
|
||||||
|
str[cnt] = '\0';
|
||||||
|
n = sscanf(str, "%d=%d\n%c", &servo, &cnt, &dummy);
|
||||||
|
if (n != 2) {
|
||||||
|
printk(KERN_WARNING "ServoBlaster: Failed to parse command (n=%d)\n", n);
|
||||||
|
return -EINVAL;
|
||||||
|
}
|
||||||
|
if (servo < 0 || servo >= NUM_SERVOS) {
|
||||||
|
printk(KERN_WARNING "ServoBlaster: Bad servo number %d\n", servo);
|
||||||
|
return -EINVAL;
|
||||||
|
}
|
||||||
|
if (cnt < 0 || cnt > 249) {
|
||||||
|
printk(KERN_WARNING "ServoBlaster: Bad value %d\n", cnt);
|
||||||
|
return -EINVAL;
|
||||||
|
}
|
||||||
|
if (wait_for_servo(servo))
|
||||||
|
return -EINTR;
|
||||||
|
if (cnt == 0) {
|
||||||
|
ctl->cb[servo*4+0].dst = ((GPIO_BASE + GPCLR0*4) & 0x00ffffff) | 0x7e000000;
|
||||||
|
} else {
|
||||||
|
ctl->cb[servo*4+0].dst = ((GPIO_BASE + GPSET0*4) & 0x00ffffff) | 0x7e000000;
|
||||||
|
ctl->cb[servo*4+1].length = cnt * sizeof(uint32_t);
|
||||||
|
ctl->cb[servo*4+3].length = (250 - cnt) * sizeof(uint32_t);
|
||||||
|
}
|
||||||
|
local_irq_enable();
|
||||||
|
|
||||||
|
return count;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int dev_close(struct inode *inod,struct file *fil)
|
||||||
|
{
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static long dev_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
|
||||||
|
{
|
||||||
|
return -EINVAL;
|
||||||
|
}
|
||||||
|
|
||||||
|
MODULE_DESCRIPTION("ServoBlaster, Multiple Servo Driver for the RaspberryPi");
|
||||||
|
MODULE_AUTHOR("Richard Hirst <richardghirst@gmail.com>");
|
||||||
|
MODULE_LICENSE("GPL v2");
|
||||||
|
|
||||||
0
ServoBlaster/servoblaster.h
Normal file
0
ServoBlaster/servoblaster.h
Normal file
BIN
ServoBlaster/servoblaster.ko
Normal file
BIN
ServoBlaster/servoblaster.ko
Normal file
Binary file not shown.
4
ServoBlaster/servodemo.c
Normal file
4
ServoBlaster/servodemo.c
Normal file
@ -0,0 +1,4 @@
|
|||||||
|
int main(int argc, char **argv)
|
||||||
|
{
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue
Block a user