summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorSebastian Reichel <sre@ring0.de>2012-08-11 23:02:47 +0200
committerSebastian Reichel <sre@ring0.de>2012-08-11 23:02:47 +0200
commit2b427f4bdfc71b055b7db8ffe2b9dd0ea3e4cc9a (patch)
treef67684ceafcdccd777cc3e1684ca72b63096be1b
downloadatmostripe-master.tar.bz2
initial commitHEADmaster
-rw-r--r--README.md58
-rw-r--r--client/Makefile9
-rw-r--r--client/enginecontroller.vala237
-rw-r--r--client/main.vala58
-rw-r--r--doc/atmostripe-pinout.txt1
-rw-r--r--doc/isp-plug.svg187
-rw-r--r--firmware/Makefile32
-rw-r--r--firmware/firmware.c170
8 files changed, 752 insertions, 0 deletions
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..6bdab4b
--- /dev/null
+++ b/README.md
@@ -0,0 +1,58 @@
+ATmega Engine Controller
+========================
+
+The code from firmware/ is supposed to run on a ATmega48
+sending servo signals to cheap brushless controllers as used
+in RC models. The ATmega48 gets its instructions via a serial
+line from a host system. The code in client/ is a small test
+binary for the host system.
+
+Connection
+----------------
+
+ [Engine 1]--->[BLC 1]--->PB1
+ [Engine 2]--->[BLC 2]--->PB2
+ [Engine 3]--->[BLC 3]--->PB3
+ [Engine 4]--->[BLC 4]--->PD3
+ [Engine 5]--->[BLC 5]--->PD5
+ [Engine 6]--->[BLC 6]--->PD6
+
+ [Host TX]--->PD0
+ [Host RX]--->PD1
+
+Serial Protocol
+---------------
+
+Each of the following commands will be acknowledged with
+```0x2E```, which is the ASCII value for a dot. Available
+commands are:
+
+- **no operation**
+
+ This command simply does nothing. It consists of a
+ single byte: ```0x00```
+
+- **enable**
+
+ Sending the bytes ```0xCA```, ```0xFE```, ```0xBA```, ```0xBE``` activates
+ the engine controlling. Before sending this command
+ the controller does not activate any engine.
+
+- **disable**
+
+ One can disable all engines again by sending the
+ bytes ```0xDE```, ```0xAD```, ```0xBA```, ```0xBE```. After disabling the
+ engines with this command all future engine settings
+ will be ignored.
+
+- **set speed (of a single engine)**
+
+ To set the speed of a single engine on has to send
+ two bytes: first the engine number in the system
+ (```0x01``` - ```0x06```), the the desired speed (```0x00```-```0xFF```).
+
+- **set speed of all engines simultaneously**
+
+ One can also set the speed of all engines simultaneously
+ to the same value by first sending ```0xFF``` and then sending
+ a second byte with the desired speed (```0x00```-```0xFF```).
diff --git a/client/Makefile b/client/Makefile
new file mode 100644
index 0000000..d274276
--- /dev/null
+++ b/client/Makefile
@@ -0,0 +1,9 @@
+all: engine
+
+engine: enginecontroller.vala main.vala
+ valac -o $@ --pkg posix --pkg linux $^
+
+clean:
+ rm -f engine
+
+.PHONY: all clean
diff --git a/client/enginecontroller.vala b/client/enginecontroller.vala
new file mode 100644
index 0000000..23bd572
--- /dev/null
+++ b/client/enginecontroller.vala
@@ -0,0 +1,237 @@
+public class EngineController {
+ private Posix.termios newtio;
+ private Posix.termios restoretio;
+ private int fd=-1;
+ private int byterate;
+
+ private void setup(string device, int rate, int bits, int stopbits) {
+ Posix.speed_t baudrate = Posix.B9600;
+
+ fd = Posix.open(device, Posix.O_RDWR /*| Posix.O_NONBLOCK*/);
+
+ if(fd < 0) {
+ fd = -1;
+ stderr.printf("Could not open device!\n");
+ return;
+ }
+
+ Posix.tcflush(fd, Posix.TCIOFLUSH);
+
+ Posix.tcgetattr(fd, out restoretio);
+
+ /* apply settings */
+ switch(rate) {
+ case 300:
+ baudrate = Posix.B300;
+ break;
+ case 600:
+ baudrate = Posix.B600;
+ break;
+ case 1200:
+ baudrate = Posix.B1200;
+ break;
+ case 2400:
+ baudrate = Posix.B2400;
+ break;
+ case 4800:
+ baudrate = Posix.B4800;
+ break;
+ case 9600:
+ baudrate = Posix.B9600;
+ break;
+ case 19200:
+ baudrate = Posix.B19200;
+ break;
+ case 38400:
+ baudrate = Posix.B38400;
+ break;
+ case 57600:
+ baudrate = Posix.B57600;
+ break;
+ case 115200:
+ baudrate = Posix.B115200;
+ break;
+ case 230400:
+ baudrate = Posix.B230400;
+ break;
+ default:
+ /* not supported */
+ rate = 9600;
+ break;
+ }
+
+ Posix.cfsetospeed(ref newtio, baudrate);
+ Posix.cfsetispeed(ref newtio, baudrate);
+
+ switch(bits) {
+ case 5:
+ newtio.c_cflag = (newtio.c_cflag & ~Posix.CSIZE) | Posix.CS5;
+ break;
+ case 6:
+ newtio.c_cflag = (newtio.c_cflag & ~Posix.CSIZE) | Posix.CS6;
+ break;
+ case 7:
+ newtio.c_cflag = (newtio.c_cflag & ~Posix.CSIZE) | Posix.CS7;
+ break;
+ case 8:
+ default:
+ newtio.c_cflag = (newtio.c_cflag & ~Posix.CSIZE) | Posix.CS8;
+ break;
+ }
+
+ newtio.c_cflag |= Posix.CLOCAL | Posix.CREAD;
+
+ newtio.c_cflag &= ~(Posix.PARENB | Posix.PARODD);
+
+ /* TODO: parity */
+
+ newtio.c_cflag &= ~Linux.Termios.CRTSCTS;
+
+ if(stopbits == 2)
+ newtio.c_cflag |= Posix.CSTOPB;
+ else
+ newtio.c_cflag &= ~Posix.CSTOPB;
+
+ newtio.c_iflag = Posix.IGNBRK;
+
+ newtio.c_lflag = 0;
+ newtio.c_oflag = 0;
+
+ newtio.c_cc[Posix.VTIME]=1;
+ newtio.c_cc[Posix.VMIN]=1;
+
+ newtio.c_lflag &= ~(Posix.ECHONL|Posix.NOFLSH);
+
+ int mcs=0;
+ Posix.ioctl(fd, Linux.Termios.TIOCMGET, out mcs);
+ mcs |= Linux.Termios.TIOCM_RTS;
+ Posix.ioctl(fd, Linux.Termios.TIOCMSET, out mcs);
+
+ Posix.tcsetattr(fd, Posix.TCSANOW, newtio);
+
+ this.byterate = rate/bits;
+ }
+
+ public EngineController(string device) {
+ setup(device, 9600, 8, 1);
+ }
+
+ private ssize_t read(void *buf, size_t count) {
+ if(fd >= 0)
+ return Posix.read(fd, buf, count);
+ else
+ stderr.printf("not connected!\n");
+ return 0;
+ }
+
+ private ssize_t write(void *buf, size_t count) {
+ if(fd >= 0) {
+ ssize_t size = Posix.write(fd, buf, count);
+ return size;
+ } else {
+ stderr.printf("not connected!\n");
+ return 0;
+ }
+ }
+
+ public bool disable() {
+ uint8[4] buf = new uint8[4];
+ buf[0] = 0xDE;
+ buf[1] = 0xAD;
+ buf[2] = 0xBA;
+ buf[3] = 0xBE;
+
+ if(fd < 0) {
+ stderr.printf("not connected!\n");
+ return false;
+ }
+
+ write(buf, 4);
+ read(buf, 1);
+
+ if(buf[0] == '.')
+ return true;
+ else
+ return false;
+ }
+
+ public bool enable() {
+ uint8[4] buf = new uint8[4];
+ buf[0] = 0xCA;
+ buf[1] = 0xFE;
+ buf[2] = 0xBA;
+ buf[3] = 0xBE;
+
+ if(fd < 0) {
+ stderr.printf("not connected!\n");
+ return false;
+ }
+
+ write(buf, 4);
+ read(buf, 1);
+
+ if(buf[0] == '.')
+ return true;
+ else
+ return false;
+ }
+
+ public bool no_operation() {
+ uint8[1] buf = new uint8[1];
+ buf[0] = 0x00;
+
+ if(fd < 0) {
+ stderr.printf("not connected!\n");
+ return false;
+ }
+
+ write(buf, 1);
+ read(buf, 1);
+
+ if(buf[0] == '.')
+ return true;
+ else
+ return false;
+ }
+
+ public bool set_speed(uint8 engine, uint8 speed) {
+ uint8[2] buf = new uint8[2];
+ buf[0] = engine+1;
+ buf[1] = speed;
+
+ if(fd < 0) {
+ stderr.printf("not connected!\n");
+ return false;
+ }
+
+ if(engine > 5)
+ return false;
+
+ write(buf, 2);
+ read(buf, 2);
+
+ if(buf[0] == '.')
+ return true;
+ else
+ return false;
+ }
+
+ public bool set_speed_all(uint8 speed) {
+ uint8[2] buf = new uint8[2];
+ buf[0] = 0xFF;
+ buf[1] = speed;
+
+ if(fd < 0) {
+ stderr.printf("not connected!\n");
+ return false;
+ }
+
+ write(buf, 2);
+ read(buf, 2);
+
+ if(buf[0] == '.')
+ return true;
+ else
+ return false;
+ }
+}
diff --git a/client/main.vala b/client/main.vala
new file mode 100644
index 0000000..2c676a5
--- /dev/null
+++ b/client/main.vala
@@ -0,0 +1,58 @@
+public static void main(string[] args) {
+ var engines = new EngineController("/dev/ttyAMA0");
+
+ if(args.length < 2) {
+ stderr.printf("%s <cmd>\n", args[0]);
+ stderr.printf("cmds: engine, enable, disable, nop\n");
+ return;
+ }
+
+ switch(args[1]) {
+ case "engine":
+ if(args.length < 4) {
+ stderr.printf("%s engine <engine> <speed>\n", args[0]);
+ return;
+ }
+
+ uint8 engine = (uint8) int.parse(args[2]);
+ if("%u".printf(engine) != args[2])
+ error("engine must be an integer");
+
+ uint8 speed = (uint8) int.parse(args[3]);
+ if("%u".printf(speed) != args[3])
+ error("speed must be an integer");
+
+ if(engine == 6)
+ if(engines.set_speed_all(speed))
+ stdout.printf("set speed of all engines to %u: ok\n", speed);
+ else
+ stdout.printf("set speed of all engines to %u: fail\n", speed);
+ if(engine <= 5)
+ if(engines.set_speed(engine, speed))
+ stdout.printf("set speed of engine %u to %u: ok\n", engine, speed);
+ else
+ stdout.printf("set speed of engine %u to %u: fail\n", engine, speed);
+ break;
+ case "enable":
+ if(engines.enable())
+ stdout.printf("enable motors: ok\n");
+ else
+ stdout.printf("enable motors: fail\n");
+ break;
+ case "disable":
+ if(engines.disable())
+ stdout.printf("disable motors: ok\n");
+ else
+ stdout.printf("disable motors: fail\n");
+ break;
+ case "nop":
+ if(engines.no_operation())
+ stdout.printf("no operation: ok\n");
+ else
+ stdout.printf("no operation: fail\n");
+ break;
+ default:
+ stderr.printf("cmd not supported!\n");
+ break;
+ }
+}
diff --git a/doc/atmostripe-pinout.txt b/doc/atmostripe-pinout.txt
new file mode 100644
index 0000000..6031357
--- /dev/null
+++ b/doc/atmostripe-pinout.txt
@@ -0,0 +1 @@
+Atmostripe ISP Pins: VCC, GND, RST, MOSI, MISO, SCK
diff --git a/doc/isp-plug.svg b/doc/isp-plug.svg
new file mode 100644
index 0000000..f68795d
--- /dev/null
+++ b/doc/isp-plug.svg
@@ -0,0 +1,187 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ id="svg2"
+ version="1.1"
+ inkscape:version="0.48.3.1 r9886"
+ width="240"
+ height="88"
+ sodipodi:docname="ISP 6 way pinout.png">
+ <metadata
+ id="metadata8">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ <dc:title></dc:title>
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <defs
+ id="defs6" />
+ <sodipodi:namedview
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1"
+ objecttolerance="10"
+ gridtolerance="10"
+ guidetolerance="10"
+ inkscape:pageopacity="0"
+ inkscape:pageshadow="2"
+ inkscape:window-width="1179"
+ inkscape:window-height="678"
+ id="namedview4"
+ showgrid="false"
+ inkscape:zoom="3.4648232"
+ inkscape:cx="117.8808"
+ inkscape:cy="34.798019"
+ inkscape:window-x="0"
+ inkscape:window-y="0"
+ inkscape:window-maximized="0"
+ inkscape:current-layer="svg2" />
+ <path
+ style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+ d="m 88,28 0,-24 62,0 0,81 -62,0 0,-23"
+ id="path2987"
+ inkscape:connector-curvature="0"
+ sodipodi:nodetypes="cccccc" />
+ <path
+ sodipodi:type="arc"
+ style="fill:none;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ id="path2989"
+ sodipodi:cx="124.53738"
+ sodipodi:cy="21.185625"
+ sodipodi:rx="3.0304577"
+ sodipodi:ry="3.0304577"
+ d="m 127.56783,21.185625 a 3.0304577,3.0304577 0 1 1 -6.06091,0 3.0304577,3.0304577 0 1 1 6.06091,0 z"
+ transform="translate(4.928208,3.768891)" />
+ <path
+ sodipodi:type="arc"
+ style="fill:none;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ id="path2989-4"
+ sodipodi:cx="124.53738"
+ sodipodi:cy="21.185625"
+ sodipodi:rx="3.0304577"
+ sodipodi:ry="3.0304577"
+ d="m 127.56783,21.185625 a 3.0304577,3.0304577 0 1 1 -6.06091,0 3.0304577,3.0304577 0 1 1 6.06091,0 z"
+ transform="translate(4.86284,24.364255)" />
+ <path
+ sodipodi:type="arc"
+ style="fill:none;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ id="path2989-0"
+ sodipodi:cx="124.53738"
+ sodipodi:cy="21.185625"
+ sodipodi:rx="3.0304577"
+ sodipodi:ry="3.0304577"
+ d="m 127.56783,21.185625 a 3.0304577,3.0304577 0 1 1 -6.06091,0 3.0304577,3.0304577 0 1 1 6.06091,0 z"
+ transform="translate(4.717362,44.705716)" />
+ <path
+ sodipodi:type="arc"
+ style="fill:none;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ id="path2989-0-1"
+ sodipodi:cx="124.53738"
+ sodipodi:cy="21.185625"
+ sodipodi:rx="3.0304577"
+ sodipodi:ry="3.0304577"
+ d="m 127.56783,21.185625 a 3.0304577,3.0304577 0 1 1 -6.06091,0 3.0304577,3.0304577 0 1 1 6.06091,0 z"
+ transform="translate(-15.690801,44.705716)" />
+ <path
+ sodipodi:type="arc"
+ style="fill:none;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ id="path2989-0-3"
+ sodipodi:cx="124.53738"
+ sodipodi:cy="21.185625"
+ sodipodi:rx="3.0304577"
+ sodipodi:ry="3.0304577"
+ d="m 127.56783,21.185625 a 3.0304577,3.0304577 0 1 1 -6.06091,0 3.0304577,3.0304577 0 1 1 6.06091,0 z"
+ transform="translate(-15.690801,24.297552)" />
+ <rect
+ style="fill:none;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ id="rect3052"
+ width="7.3469386"
+ height="6.734694"
+ x="105.3772"
+ y="21.605625" />
+ <text
+ xml:space="preserve"
+ style="font-size:9px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:end;line-height:125%;letter-spacing:0px;word-spacing:0px;text-anchor:end;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans Mono;-inkscape-font-specification:Bitstream Vera Sans Mono"
+ x="82.087166"
+ y="29.972973"
+ id="text3054"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ id="tspan3056"
+ x="82.087166"
+ y="29.972973"
+ style="font-size:14px;font-weight:bold;-inkscape-font-specification:Bitstream Vera Sans Mono Bold">MISO: 1</tspan></text>
+ <text
+ xml:space="preserve"
+ style="font-size:9px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:end;line-height:125%;letter-spacing:0px;word-spacing:0px;text-anchor:end;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans Mono;-inkscape-font-specification:Bitstream Vera Sans Mono"
+ x="81.870941"
+ y="52.007938"
+ id="text3054-1"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ id="tspan3056-4"
+ x="81.870941"
+ y="52.007938"
+ style="font-size:14px;font-weight:bold;-inkscape-font-specification:Bitstream Vera Sans Mono Bold">SCK: 3</tspan></text>
+ <text
+ xml:space="preserve"
+ style="font-size:9px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:end;line-height:125%;letter-spacing:0px;word-spacing:0px;text-anchor:end;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans Mono;-inkscape-font-specification:Bitstream Vera Sans Mono"
+ x="82.693886"
+ y="70.987534"
+ id="text3054-1-9"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ id="tspan3056-4-6"
+ x="82.693886"
+ y="70.987534"
+ style="font-size:14px;font-weight:bold;-inkscape-font-specification:Bitstream Vera Sans Mono Bold">RESET: 5</tspan></text>
+ <text
+ xml:space="preserve"
+ style="font-size:9px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans Mono;-inkscape-font-specification:Bitstream Vera Sans Mono"
+ x="157.418"
+ y="30.789299"
+ id="text3100"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ id="tspan3102"
+ x="157.418"
+ y="30.789299"
+ style="font-size:14px;font-weight:bold;-inkscape-font-specification:Bitstream Vera Sans Mono Bold">2: VCC</tspan></text>
+ <text
+ xml:space="preserve"
+ style="font-size:9px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans Mono;-inkscape-font-specification:Bitstream Vera Sans Mono"
+ x="158.43869"
+ y="50.171204"
+ id="text3100-9"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ id="tspan3102-8"
+ x="158.43869"
+ y="50.171204"
+ style="font-size:14px;font-weight:bold;-inkscape-font-specification:Bitstream Vera Sans Mono Bold">4: MOSI</tspan></text>
+ <text
+ xml:space="preserve"
+ style="font-size:9px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans Mono;-inkscape-font-specification:Bitstream Vera Sans Mono"
+ x="157.7283"
+ y="72.987534"
+ id="text3100-9-8"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ id="tspan3102-8-6"
+ x="157.7283"
+ y="72.987534"
+ style="font-size:14px;font-weight:bold;-inkscape-font-specification:Bitstream Vera Sans Mono Bold">6: Ground</tspan></text>
+</svg>
diff --git a/firmware/Makefile b/firmware/Makefile
new file mode 100644
index 0000000..d9b89e7
--- /dev/null
+++ b/firmware/Makefile
@@ -0,0 +1,32 @@
+SOURCES := firmware.c
+OBJECTS := $(patsubst %.c, %.o, $(SOURCES))
+CFLAGS := -O2 -std=c99 -mmcu=atmega48 -Wall -DF_CPU=16000000UL -DDEBUG=1
+LDFLAGS := -mmcu=atmega48 -lm
+
+all: fw.hex
+
+%.o: %.c
+ @echo "[CC] $<"
+ @avr-gcc -o $@ $(CFLAGS) -c $<
+
+fw.hex: fw.elf
+ @echo "[HEX] $@"
+ @avr-objcopy $^ -O ihex -R .eeprom $@
+
+fw.elf: ${OBJECTS}
+ @echo "[LD] $@"
+ @avr-gcc -o $@ $(LDFLAGS) $^
+
+fw.S: fw.elf
+ @avr-objdump -d fw.elf -h -m avr > fw.S
+
+clean:
+ @rm -f ${OBJECTS} fw.hex fw.elf fw.S
+
+backup:
+ avrdude -c buspirate -P /dev/ttyUSB0 -p m48 -U flash:r:"backup.hex":i noreset
+
+flash:
+ avrdude -c buspirate -P /dev/ttyUSB0 -p m48 -U flash:w:"fw.hex":i reset
+
+.PHONY: all clean backup flash
diff --git a/firmware/firmware.c b/firmware/firmware.c
new file mode 100644
index 0000000..9a04d41
--- /dev/null
+++ b/firmware/firmware.c
@@ -0,0 +1,170 @@
+#include <avr/io.h>
+#include <util/delay.h>
+#include <util/delay.h>
+#include <avr/interrupt.h>
+
+/* 500Hz -> 2ms */
+#define MAXPULSFREQ 500
+
+/* timer1 value for a 2ms puls */
+#define TIMER_MAXPULS F_CPU/MAXPULSFREQ
+
+/* min/max values */
+#define MINPULS 13500
+#define MAXPULS 30000
+
+/* baud rate for serial communication */
+#define BAUD 9600UL
+
+/* hardware settings for baud rate */
+#define UBRR_VAL ((F_CPU+BAUD*8)/(BAUD*16)-1)
+#define BAUD_REAL (F_CPU/(16*(UBRR_VAL+1)))
+#define BAUD_ERROR ((BAUD_REAL*1000)/BAUD)
+
+/* compile time check if baud rate is ok */
+#if ((BAUD_ERROR<990) || (BAUD_ERROR>1010))
+ #error systematic error of baud rate is > 1%
+#endif
+
+
+static uint16_t servo_pulslength[20];
+static uint8_t activated = 0;
+
+/* init IO pins */
+static inline void init_io() {
+ /* PB1 = left:red, PB2 = left:green, PB3 = left:blue */
+ DDRB = (1 << PB1) | (1 << PB2) | (1 << PB3);
+
+ /* PD3 = right:red, PD5 = right:green, PD6 = right:blue */
+ DDRD = (1 << PD3) | (1 << PD5) | (1 << PD6);
+
+ /* These pins are broken, so enable them */
+ DDRC = (1 << PC4) | (1 << PC5) | (1 << PC6);
+ PORTC = (1 << PC4) | (1 << PC5) | (1 << PC6);
+}
+
+/* INTERRUPT: timer for software pwm to control servos */
+ISR(TIMER1_OVF_vect) {
+ static uint8_t servo_indexhalf = 0;
+
+ switch(servo_indexhalf) {
+ case 0 : PORTB |= (1 << PB1); break;
+ case 1 : PORTB &= ~(1 << PB1); break;
+ case 2 : PORTB |= (1 << PB2); break;
+ case 3 : PORTB &= ~(1 << PB2); break;
+ case 4 : PORTB |= (1 << PB3); break;
+ case 5 : PORTB &= ~(1 << PB3); break;
+ case 6 : PORTD |= (1 << PD3); break;
+ case 7 : PORTD &= ~(1 << PD3); break;
+ case 8 : PORTD |= (1 << PD5); break;
+ case 9 : PORTD &= ~(1 << PD5); break;
+ case 10: PORTD |= (1 << PD6); break;
+ case 11: PORTD &= ~(1 << PD6); break;
+ }
+
+ /* set time for the next interrupt */
+ TCNT1 = servo_pulslength[servo_indexhalf];
+
+ servo_indexhalf++;
+
+ if(servo_indexhalf == 20)
+ servo_indexhalf = 0;
+}
+
+/* setup serial communication */
+static inline void init_uart() {
+ UBRR0 = UBRR_VAL;
+ UCSR0B |= (1 << TXEN0);
+ UCSR0B |= (1 << RXEN0);
+
+ /* Frame Format: Asynchron 8N1 */
+ UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
+}
+
+/* set servo speed */
+static inline void servo_set(uint8_t index, uint8_t value) {
+ if(!activated)
+ value = 0;
+
+ if (index < 6) {
+ uint16_t tmp = MINPULS+(MAXPULS-MINPULS)/256*value;
+ /* calculate hightime */
+ servo_pulslength[index<<1]=(-1)*tmp;
+ /* sum of low and hightime for one servo is 2ms */
+ servo_pulslength[(index<<1)+1] = (-1)*(TIMER_MAXPULS-tmp);
+ }
+}
+
+/* init servos, reset speed */
+static inline void init_servo() {
+ uint8_t i;
+
+ /* disable all motors */
+ for(i=0;i<6;i++)
+ servo_set(i, 0);
+
+ /* init timer */
+ TCNT1 = 0 - 16000;
+ TCCR1A = 0;
+ TCCR1B = (1 << CS10);
+ TIMSK1 |= (1 << TOIE1);
+}
+
+/* get a single byte from serial device */
+static inline uint8_t uart_getc() {
+ while (!(UCSR0A & (1<<RXC0)));
+ return UDR0;
+}
+
+/* put a single byte to serial device */
+static inline void uart_putc(uint8_t c) {
+ while (!(UCSR0A & (1<<UDRE0)));
+ UDR0 = c;
+}
+
+int main() {
+ /* initialize hardware */
+ init_io();
+ init_servo();
+ init_uart();
+ sei();
+
+ uint8_t c;
+
+ while(1) {
+ c = uart_getc();
+
+ /* set speed for single engine */
+ if(c >= 0x01 && c <= 0x06) {
+ uint8_t speed = uart_getc();
+ servo_set(c-1, speed);
+ /* set speed for all engines */
+ } else if(c == 0xFF) {
+ uint8_t speed = uart_getc();
+ for(int i=0;i<6;i++)
+ servo_set(i, speed);
+ /* activate engines */
+ } else if(c == 0xCA) {
+ if(uart_getc() != 0xFE) continue;
+ if(uart_getc() != 0xBA) continue;
+ if(uart_getc() != 0xBE) continue;
+ activated=1;
+ /* disable engines */
+ } else if(c == 0xDE) {
+ if(uart_getc() != 0xAD) continue;
+ if(uart_getc() != 0xBA) continue;
+ if(uart_getc() != 0xBE) continue;
+ activated=0;
+ for(int i=0;i<6;i++)
+ servo_set(i, 0);
+ /* ignore unknown commands */
+ } else if(c != 0x00) {
+ continue;
+ }
+
+ /* acknowledge command */
+ uart_putc('.');
+ }
+
+ return 0;
+}