diff --git a/Makefile b/Makefile
index 724b7f50..153eaf46 100644
--- a/Makefile
+++ b/Makefile
@@ -99,7 +99,7 @@ CSRC = $(PORTSRC) \
        myUSB.c \
        irq_handlers.c \
        buffer.c \
-       comm.c \
+       comm_usb.c \
        crc.c \
        digital_filter.c \
        ledpwm.c \
@@ -111,6 +111,7 @@ CSRC = $(PORTSRC) \
        terminal.c \
        conf_general.c \
        eeprom.c \
+       commands.c \
        $(HWSRC) \
        $(APPSRC)
        
diff --git a/applications/app.c b/applications/app.c
index 7c52e4c0..52b42ef0 100644
--- a/applications/app.c
+++ b/applications/app.c
@@ -13,7 +13,7 @@
 
     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.
-    */
+ */
 
 /*
  * app.c
@@ -27,19 +27,28 @@
 #include "hal.h"
 
 // Private variables
-static app_configuration app_conf;
+static app_configuration appconf;
 
 void app_init(app_configuration *conf) {
-	app_conf = *conf;
+	appconf = *conf;
 
-	switch (app_conf.app_to_use) {
+	switch (appconf.app_to_use) {
 	case APP_PPM:
-		app_ppm_configure(app_conf.app_ppm_ctrl_type, app_conf.app_ppm_pid_max_erpm);
+		app_ppm_configure(appconf.app_ppm_ctrl_type, appconf.app_ppm_pid_max_erpm, appconf.app_ppm_hyst,
+				appconf.app_ppm_timeout, appconf.app_ppm_pulse_start, appconf.app_ppm_pulse_width);
 		app_ppm_start();
 		break;
 
-	case APP_UARTCOMM:
-		app_uartcomm_configure(app_conf.app_uart_baudrate);
+	case APP_UART:
+		app_uartcomm_configure(appconf.app_uart_baudrate, appconf.app_uart_timeout);
+		app_uartcomm_start();
+		break;
+
+	case APP_PPM_UART:
+		app_ppm_configure(appconf.app_ppm_ctrl_type, appconf.app_ppm_pid_max_erpm, appconf.app_ppm_hyst,
+				appconf.app_ppm_timeout, appconf.app_ppm_pulse_start, appconf.app_ppm_pulse_width);
+		app_ppm_start();
+		app_uartcomm_configure(appconf.app_uart_baudrate, appconf.app_uart_timeout);
 		app_uartcomm_start();
 		break;
 
@@ -58,7 +67,7 @@ void app_init(app_configuration *conf) {
 }
 
 const app_configuration* app_get_configuration(void) {
-	return &app_conf;
+	return &appconf;
 }
 
 /**
@@ -69,7 +78,8 @@ const app_configuration* app_get_configuration(void) {
  * The new configuration to use.
  */
 void app_set_configuration(app_configuration *conf) {
-	app_conf = *conf;
-	app_ppm_configure(app_conf.app_ppm_ctrl_type, app_conf.app_ppm_pid_max_erpm);
-	app_uartcomm_configure(app_conf.app_uart_baudrate);
+	appconf = *conf;
+	app_ppm_configure(appconf.app_ppm_ctrl_type, appconf.app_ppm_pid_max_erpm, appconf.app_ppm_hyst,
+			appconf.app_ppm_timeout, appconf.app_ppm_pulse_start, appconf.app_ppm_pulse_width);
+	app_uartcomm_configure(appconf.app_uart_baudrate, appconf.app_uart_timeout);
 }
diff --git a/applications/app.h b/applications/app.h
index 578c8598..a3e4a65d 100644
--- a/applications/app.h
+++ b/applications/app.h
@@ -34,9 +34,10 @@ void app_set_configuration(app_configuration *conf);
 
 // Standard apps
 void app_ppm_start(void);
-void app_ppm_configure(ppm_control_type ctrlt, float pme);
+void app_ppm_configure(ppm_control_type ctrlt, float pme, float hyst,
+		float timeout, float pulse_start, float pulse_width);
 void app_uartcomm_start(void);
-void app_uartcomm_configure(uint32_t baudrate);
+void app_uartcomm_configure(uint32_t baudrate, uint32_t timeout);
 
 // Custom apps
 void app_gurgalof_init(void);
diff --git a/applications/app_ppm.c b/applications/app_ppm.c
index 182da156..56811cf1 100644
--- a/applications/app_ppm.c
+++ b/applications/app_ppm.c
@@ -13,7 +13,7 @@
 
     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.
-    */
+ */
 
 /*
  * app_ppm.c
@@ -31,6 +31,12 @@
 #include "mcpwm.h"
 #include <math.h>
 
+// Types
+typedef enum {
+	EV_TYPE_SERVO = 0,
+	EV_TYPE_TIMER
+} ev_type;
+
 // Threads
 static msg_t ppm_thread(void *arg);
 static WORKING_AREA(ppm_thread_wa, 1024);
@@ -42,12 +48,27 @@ static void servodec_func(void);
 static void trig_func(void *p);
 
 // Private variables
-static volatile ppm_control_type ctrl_type;
-static volatile float pid_max_erpm;
+static volatile ppm_control_type ctrl_type = PPM_CTRL_TYPE_CURRENT;
+static volatile float pid_max_erpm = 15000.0;
+static volatile systime_t pulse_timeout = 1000.0;;
+static volatile bool is_running = false;
+static volatile ev_type ev = EV_TYPE_SERVO;
+static volatile float hysteres = 0.15;
+static volatile float pulse_s = 1.0;
+static volatile float pulse_e = 1.0;
 
-void app_ppm_configure(ppm_control_type ctrlt, float pme) {
+void app_ppm_configure(ppm_control_type ctrlt, float pme, float hyst,
+		float timeout, float pulse_start, float pulse_width) {
 	ctrl_type = ctrlt;
 	pid_max_erpm = pme;
+	pulse_timeout = timeout;
+	hysteres = hyst;
+	pulse_s = pulse_start;
+	pulse_e = pulse_width;
+
+	if (is_running) {
+		servodec_set_pulse_options(pulse_s, pulse_e);
+	}
 }
 
 void app_ppm_start(void) {
@@ -59,13 +80,14 @@ static void trig_func(void *p) {
 
 	chSysLock();
 	chVTSetI(&vt, MS2ST(10), trig_func, NULL);
-	chSysUnlock();
-
+	ev = EV_TYPE_TIMER;
 	chEvtSignalI(ppm_tp, (eventmask_t) 1);
+	chSysUnlock();
 }
 
 static void servodec_func(void) {
 	chSysLockFromIsr();
+	ev = EV_TYPE_SERVO;
 	chEvtSignalI(ppm_tp, (eventmask_t) 1);
 	chSysUnlockFromIsr();
 }
@@ -76,19 +98,20 @@ static msg_t ppm_thread(void *arg) {
 	chRegSetThreadName("APP_PPM");
 	ppm_tp = chThdSelf();
 
+	servodec_set_pulse_options(pulse_s, pulse_e);
 	servodec_init(servodec_func);
 
 	chSysLock();
 	chVTSetI(&vt, MS2ST(10), trig_func, NULL);
 	chSysUnlock();
 
+	is_running = true;
+
 	for(;;) {
 		chEvtWaitAny((eventmask_t) 1);
 
-#define HYST			0.15
-
-		if (servodec_get_time_since_update() < 500) {
-			float servo_val = servodec_get_servo_as_float(0);
+		if (ev == EV_TYPE_SERVO) {
+			float servo_val = servodec_get_servo(0);
 
 			switch (ctrl_type) {
 			case PPM_CTRL_TYPE_CURRENT_NOREV:
@@ -102,12 +125,12 @@ static msg_t ppm_thread(void *arg) {
 				break;
 			}
 
-			servo_val /= (1.0 - HYST);
+			servo_val /= (1.0 - hysteres);
 
-			if (servo_val > HYST) {
-				servo_val -= HYST;
-			} else if (servo_val < -HYST) {
-				servo_val += HYST;
+			if (servo_val > hysteres) {
+				servo_val -= hysteres;
+			} else if (servo_val < -hysteres) {
+				servo_val += hysteres;
 			} else {
 				servo_val = 0.0;
 			}
@@ -144,7 +167,9 @@ static msg_t ppm_thread(void *arg) {
 				break;
 			}
 		} else {
-			mcpwm_set_current(0.0);
+			if (pulse_timeout != 0 && servodec_get_time_since_update() > pulse_timeout) {
+				mcpwm_set_current(0.0);
+			}
 		}
 	}
 
diff --git a/applications/app_uartcomm.c b/applications/app_uartcomm.c
index 96349543..8f1221f4 100644
--- a/applications/app_uartcomm.c
+++ b/applications/app_uartcomm.c
@@ -23,20 +23,16 @@
  */
 
 #include "app.h"
-
 #include "ch.h"
 #include "hal.h"
-#include "stm32f4xx_conf.h"
-#include "servo_dec.h"
-#include "mcpwm.h"
-#include "utils.h"
-#include "packet.h"
-#include "buffer.h"
 #include "hw.h"
-#include <stdint.h>
+#include "mcpwm.h"
+#include "packet.h"
+#include "commands.h"
+
+#include <string.h>
 
 // Settings
-#define TIMEOUT_MSEC				1000
 #define BAUDRATE					115200
 #define PACKET_HANDLER				1
 #define SERIAL_RX_BUFFER_SIZE		1024
@@ -45,26 +41,21 @@
 static msg_t uart_thread(void *arg);
 static msg_t packet_process_thread(void *arg);
 static WORKING_AREA(uart_thread_wa, 1024);
-static WORKING_AREA(packet_process_thread_wa, 1024);
+static WORKING_AREA(packet_process_thread_wa, 4096);
 static Thread *process_tp;
 
 // Variables
 static volatile systime_t last_uart_update_time;
+static volatile systime_t timeout_msec = 1000;
 static uint8_t serial_rx_buffer[SERIAL_RX_BUFFER_SIZE];
 static int serial_rx_read_pos = 0;
 static int serial_rx_write_pos = 0;
 static int is_running = 0;
 
 // Private functions
-static void process_packet(unsigned char *buffer, unsigned char len);
-static void send_packet(unsigned char *buffer, unsigned char len);
-
-typedef enum {
-	UARTCOMM_CMD_SET_DUTY = 0,
-	UARTCOMM_CMD_SET_CURRENT,
-	UARTCOMM_CMD_SET_CURRENT_BRAKE,
-	UARTCOMM_CMD_SET_RPM
-} UARTCOMM_CMD;
+static void process_packet(unsigned char *data, unsigned char len);
+static void send_packet_wrapper(unsigned char *data, unsigned char len);
+static void send_packet(unsigned char *data, unsigned char len);
 
 /*
  * This callback is invoked when a transmission buffer has been completely
@@ -127,40 +118,27 @@ static UARTConfig uart_cfg = {
 		0
 };
 
-static void process_packet(unsigned char *buffer, unsigned char len) {
-	if (!len) {
-		return;
-	}
-
-	int32_t ind = 1;
-
-	switch (buffer[0]) {
-	case UARTCOMM_CMD_SET_DUTY:
-		mcpwm_set_duty((float)buffer_get_int32(buffer, &ind) / 100000.0);
-		last_uart_update_time = chTimeNow();
-		break;
-
-	case UARTCOMM_CMD_SET_CURRENT:
-		mcpwm_set_current((float)buffer_get_int32(buffer, &ind) / 1000.0);
-		last_uart_update_time = chTimeNow();
-		break;
-
-	case UARTCOMM_CMD_SET_CURRENT_BRAKE:
-		mcpwm_set_brake_current((float)buffer_get_int32(buffer, &ind) / 1000.0);
-		last_uart_update_time = chTimeNow();
-		break;
-
-	case UARTCOMM_CMD_SET_RPM:
-		mcpwm_set_pid_speed((float)buffer_get_int32(buffer, &ind));
-		last_uart_update_time = chTimeNow();
-		break;
-
-	default:
-		break;
-	}
+static void process_packet(unsigned char *data, unsigned char len) {
+	commands_set_send_func(send_packet_wrapper);
+	commands_process_packet(data, len);
+	last_uart_update_time = chTimeNow();
 }
 
-static void send_packet(unsigned char *buffer, unsigned char len) {
+static void send_packet_wrapper(unsigned char *data, unsigned char len) {
+	packet_send_packet(data, len, PACKET_HANDLER);
+}
+
+static void send_packet(unsigned char *data, unsigned char len) {
+	// Wait for the previous transmission to finish.
+	while (HW_UART_DEV.txstate == UART_TX_ACTIVE) {
+		chThdSleep(1);
+	}
+
+	// Copy this data to a new buffer in case the provided one is re-used
+	// after this function returns.
+	static uint8_t buffer[300];
+	memcpy(buffer, data, len);
+
 	uartStartSend(&HW_UART_DEV, len, buffer);
 }
 
@@ -170,8 +148,9 @@ void app_uartcomm_start(void) {
 	chThdCreateStatic(packet_process_thread_wa, sizeof(packet_process_thread_wa), NORMALPRIO, packet_process_thread, NULL);
 }
 
-void app_uartcomm_configure(uint32_t baudrate) {
+void app_uartcomm_configure(uint32_t baudrate, uint32_t timeout) {
 	uart_cfg.speed = baudrate;
+	timeout_msec = timeout;
 
 	if (is_running) {
 		uartStart(&HW_UART_DEV, &uart_cfg);
@@ -198,8 +177,8 @@ static msg_t uart_thread(void *arg) {
 	for(;;) {
 		time += MS2ST(40);
 
-		if (chTimeElapsedSince(last_uart_update_time) > MS2ST(TIMEOUT_MSEC)) {
-			mcpwm_set_duty(0.0);
+		if (timeout_msec != 0 && chTimeElapsedSince(last_uart_update_time) > MS2ST(timeout_msec)) {
+			mcpwm_release_motor();
 		}
 
 		chThdSleepUntil(time);
diff --git a/comm_usb.c b/comm_usb.c
new file mode 100644
index 00000000..85834a6b
--- /dev/null
+++ b/comm_usb.c
@@ -0,0 +1,128 @@
+/*
+	Copyright 2012-2014 Benjamin Vedder	benjamin@vedder.se
+
+	This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    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, see <http://www.gnu.org/licenses/>.
+    */
+
+/*
+ * comm.c
+ *
+ *  Created on: 22 nov 2012
+ *      Author: benjamin
+ */
+
+#include "ch.h"
+#include "hal.h"
+#include "comm_usb.h"
+#include "packet.h"
+#include "myUSB.h"
+#include "commands.h"
+
+// Settings
+#define PACKET_HANDLER				0
+
+// Private variables
+#define SERIAL_RX_BUFFER_SIZE		2048
+static uint8_t serial_rx_buffer[SERIAL_RX_BUFFER_SIZE];
+static int serial_rx_read_pos = 0;
+static int serial_rx_write_pos = 0;
+static WORKING_AREA(serial_read_thread_wa, 512);
+static WORKING_AREA(serial_process_thread_wa, 4096);
+static Mutex send_mutex;
+static Thread *process_tp;
+
+// Private functions
+static void process_packet(unsigned char *data, unsigned char len);
+static void send_packet(unsigned char *buffer, unsigned char len);
+static void send_packet_wrapper(unsigned char *data, unsigned char len);
+
+static msg_t serial_read_thread(void *arg) {
+	(void)arg;
+
+	chRegSetThreadName("USB-Serial read");
+
+	uint8_t buffer[128];
+	int i;
+	int len;
+	int had_data = 0;
+
+	for(;;) {
+		len = chSequentialStreamRead(&SDU1, (uint8_t*) buffer, 1);
+
+		for (i = 0;i < len;i++) {
+			serial_rx_buffer[serial_rx_write_pos++] = buffer[i];
+
+			if (serial_rx_write_pos == SERIAL_RX_BUFFER_SIZE) {
+				serial_rx_write_pos = 0;
+			}
+
+			had_data = 1;
+		}
+
+		if (had_data) {
+			chEvtSignal(process_tp, (eventmask_t) 1);
+			had_data = 0;
+		}
+	}
+
+	return 0;
+}
+
+static msg_t serial_process_thread(void *arg) {
+	(void)arg;
+
+	chRegSetThreadName("USB-Serial process");
+
+	process_tp = chThdSelf();
+
+	for(;;) {
+		chEvtWaitAny((eventmask_t) 1);
+
+		while (serial_rx_read_pos != serial_rx_write_pos) {
+			packet_process_byte(serial_rx_buffer[serial_rx_read_pos++], PACKET_HANDLER);
+
+			if (serial_rx_read_pos == SERIAL_RX_BUFFER_SIZE) {
+				serial_rx_read_pos = 0;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static void process_packet(unsigned char *data, unsigned char len) {
+	commands_set_send_func(send_packet_wrapper);
+	commands_process_packet(data, len);
+}
+
+static void send_packet_wrapper(unsigned char *data, unsigned char len) {
+	packet_send_packet(data, len, PACKET_HANDLER);
+}
+
+static void send_packet(unsigned char *buffer, unsigned char len) {
+	chMtxLock(&send_mutex);
+	chSequentialStreamWrite(&SDU1, buffer, len);
+	chMtxUnlock();
+}
+
+void comm_usb_init(void) {
+	myUSBinit();
+	packet_init(send_packet, process_packet, PACKET_HANDLER);
+
+	chMtxInit(&send_mutex);
+
+	// Threads
+	chThdCreateStatic(serial_read_thread_wa, sizeof(serial_read_thread_wa), NORMALPRIO, serial_read_thread, NULL);
+	chThdCreateStatic(serial_process_thread_wa, sizeof(serial_process_thread_wa), NORMALPRIO, serial_process_thread, NULL);
+}
diff --git a/comm.h b/comm_usb.h
similarity index 70%
rename from comm.h
rename to comm_usb.h
index 0fcd5235..4a897156 100644
--- a/comm.h
+++ b/comm_usb.h
@@ -16,23 +16,18 @@
     */
 
 /*
- * comm.h
+ * comm_usb.h
  *
  *  Created on: 22 nov 2012
  *      Author: benjamin
  */
 
-#ifndef COMM_H_
-#define COMM_H_
+#ifndef COMM_USB_H_
+#define COMM_USB_H_
 
 #include "conf_general.h"
 
 // Functions
-void comm_init(void);
-void comm_printf(char* format, ...);
-void comm_send_samples(uint8_t *data, int len);
-void comm_send_rotor_pos(float rotor_pos);
-void comm_print_fault_code(mc_fault_code fault_code);
-void comm_send_experiment_samples(float *samples, int len);
+void comm_usb_init(void);
 
-#endif /* COMM_INTERFACE_H_ */
+#endif /* COMM_USB_H_ */
diff --git a/comm.c b/commands.c
similarity index 69%
rename from comm.c
rename to commands.c
index 60765bcd..e29905c2 100644
--- a/comm.c
+++ b/commands.c
@@ -1,35 +1,17 @@
 /*
-	Copyright 2012-2014 Benjamin Vedder	benjamin@vedder.se
-
-	This program is free software: you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation, either version 3 of the License, or
-    (at your option) any later version.
-
-    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, see <http://www.gnu.org/licenses/>.
-    */
-
-/*
- * comm.c
+ * commands.c
  *
- *  Created on: 22 nov 2012
+ *  Created on: 19 sep 2014
  *      Author: benjamin
  */
 
+#include "commands.h"
 #include "ch.h"
 #include "hal.h"
-#include "comm.h"
 #include "main.h"
 #include "stm32f4xx_conf.h"
 #include "servo.h"
 #include "buffer.h"
-#include "packet.h"
 #include "myUSB.h"
 #include "terminal.h"
 #include "hw.h"
@@ -41,108 +23,40 @@
 #include <stdarg.h>
 #include <stdio.h>
 
-// Internal data types
-typedef enum {
-	COMM_GET_VALUES = 0,
-	COMM_SET_DUTY,
-	COMM_SET_CURRENT,
-	COMM_SET_CURRENT_BRAKE,
-	COMM_SET_RPM,
-	COMM_SET_DETECT,
-	COMM_SET_SERVO_OFFSET,
-	COMM_SET_MCCONF,
-	COMM_GET_MCCONF,
-	COMM_SET_APPCONF,
-	COMM_GET_APPCONF,
-	COMM_SAMPLE_PRINT,
-	COMM_TERMINAL_CMD,
-	COMM_PRINT,
-	COMM_ROTOR_POSITION,
-	COMM_EXPERIMENT_SAMPLE,
-	COMM_DETECT_MOTOR_PARAM,
-	COMM_REBOOT
-} COMM_PACKET_ID;
+static void(*send_func)(unsigned char *data, unsigned char len) = 0;
 
-// Settings
-#define PACKET_BUFFER_LEN	30
-#define PRINT_BUFFER_LEN	10
-#define PRINT_MAXLEN		240
-
-// Private variables
-#define SERIAL_RX_BUFFER_SIZE		4096
-static uint8_t serial_rx_buffer[SERIAL_RX_BUFFER_SIZE];
-static int serial_rx_read_pos = 0;
-static int serial_rx_write_pos = 0;
-static WORKING_AREA(serial_read_thread_wa, 1024);
-static WORKING_AREA(serial_process_thread_wa, 4096);
-static Mutex send_mutex;
-static Thread *process_tp;
-
-// Private functions
-static void process_packet(unsigned char *data, unsigned char len);
-static void send_packet(unsigned char *buffer, unsigned char len);
-
-static msg_t serial_read_thread(void *arg) {
-	(void)arg;
-
-	chRegSetThreadName("Serial read");
-
-	uint8_t buffer[128];
-	int i;
-	int len;
-	int had_data = 0;
-
-	for(;;) {
-		len = chSequentialStreamRead(&SDU1, (uint8_t*) buffer, 1);
-
-		for (i = 0;i < len;i++) {
-			serial_rx_buffer[serial_rx_write_pos++] = buffer[i];
-
-			if (serial_rx_write_pos == SERIAL_RX_BUFFER_SIZE) {
-				serial_rx_write_pos = 0;
-			}
-
-			had_data = 1;
-		}
-
-		if (had_data) {
-			chEvtSignal(process_tp, (eventmask_t) 1);
-			had_data = 0;
-		}
+static void send_packet(unsigned char *data, unsigned char len) {
+	if (send_func) {
+		send_func(data, len);
 	}
-
-	return 0;
 }
 
-static msg_t serial_process_thread(void *arg) {
-	(void)arg;
-
-	chRegSetThreadName("Serial process");
-
-	process_tp = chThdSelf();
-
-	for(;;) {
-		chEvtWaitAny((eventmask_t) 1);
-
-		while (serial_rx_read_pos != serial_rx_write_pos) {
-			packet_process_byte(serial_rx_buffer[serial_rx_read_pos++], 0);
-
-			if (serial_rx_read_pos == SERIAL_RX_BUFFER_SIZE) {
-				serial_rx_read_pos = 0;
-			}
-		}
-	}
-
-	return 0;
+/**
+ * Provide a function to use the next time there are packets to be sent.
+ *
+ * @param func
+ * A pointer to the packet sending function.
+ */
+void commands_set_send_func(void(*func)(unsigned char *data, unsigned char len)) {
+	send_func = func;
 }
 
-static void process_packet(unsigned char *data, unsigned char len) {
+/**
+ * Process a received buffer with commands and data.
+ *
+ * @param data
+ * The buffer to process.
+ *
+ * @param len
+ * The length of the buffer.
+ */
+void commands_process_packet(unsigned char *data, unsigned char len) {
 	if (!len) {
 		return;
 	}
 
 	COMM_PACKET_ID packet_id;
-	uint8_t send_buffer[256];
+	static uint8_t send_buffer[256];
 	int32_t ind = 0;
 	uint16_t sample_len;
 	uint8_t decimation;
@@ -177,7 +91,7 @@ static void process_packet(unsigned char *data, unsigned char len) {
 		buffer_append_int16(send_buffer, (int16_t)(mcpwm_get_duty_cycle_now() * 1000.0), &ind);
 		buffer_append_int32(send_buffer, (int32_t)mcpwm_get_rpm(), &ind);
 		buffer_append_int16(send_buffer, (int16_t)(GET_INPUT_VOLTAGE() * 10.0), &ind);
-		packet_send_packet(send_buffer, ind, 0);
+		send_packet(send_buffer, ind);
 		break;
 
 	case COMM_SET_DUTY:
@@ -209,6 +123,8 @@ static void process_packet(unsigned char *data, unsigned char len) {
 		break;
 
 	case COMM_SET_MCCONF:
+		mcconf = *mcpwm_get_configuration();
+
 		ind = 0;
 		mcconf.pwm_mode = data[ind++];
 		mcconf.comm_mode = data[ind++];
@@ -298,15 +214,23 @@ static void process_packet(unsigned char *data, unsigned char len) {
 
 		buffer_append_int32(send_buffer, mcconf.m_fault_stop_time_ms, &ind);
 
-		packet_send_packet(send_buffer, ind, 0);
+		send_packet(send_buffer, ind);
 		break;
 
 	case COMM_SET_APPCONF:
+		appconf = *app_get_configuration();
+
 		ind = 0;
 		appconf.app_to_use = data[ind++];
 		appconf.app_ppm_ctrl_type = data[ind++];
 		appconf.app_ppm_pid_max_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
+		appconf.app_ppm_hyst = (float)buffer_get_int32(data, &ind) / 1000.0;
+		appconf.app_ppm_timeout = buffer_get_uint32(data, &ind);
+		appconf.app_ppm_pulse_start = (float)buffer_get_int32(data, &ind) / 1000.0;
+		appconf.app_ppm_pulse_width = (float)buffer_get_int32(data, &ind) / 1000.0;
+
 		appconf.app_uart_baudrate = buffer_get_uint32(data, &ind);
+		appconf.app_uart_timeout = buffer_get_uint32(data, &ind);
 
 		conf_general_store_app_configuration(&appconf);
 		app_set_configuration(&appconf);
@@ -320,9 +244,15 @@ static void process_packet(unsigned char *data, unsigned char len) {
 		send_buffer[ind++] = appconf.app_to_use;
 		send_buffer[ind++] = appconf.app_ppm_ctrl_type;
 		buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_pid_max_erpm * 1000.0), &ind);
-		buffer_append_uint32(send_buffer, appconf.app_uart_baudrate, &ind);
+		buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_hyst * 1000.0), &ind);
+		buffer_append_uint32(send_buffer, appconf.app_ppm_timeout, &ind);
+		buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_pulse_start * 1000.0), &ind);
+		buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_pulse_width * 1000.0), &ind);
 
-		packet_send_packet(send_buffer, ind, 0);
+		buffer_append_uint32(send_buffer, appconf.app_uart_baudrate, &ind);
+		buffer_append_uint32(send_buffer, appconf.app_uart_timeout, &ind);
+
+		send_packet(send_buffer, ind);
 		break;
 
 	case COMM_SAMPLE_PRINT:
@@ -354,7 +284,7 @@ static void process_packet(unsigned char *data, unsigned char len) {
 		send_buffer[ind++] = COMM_DETECT_MOTOR_PARAM;
 		buffer_append_int32(send_buffer, (int32_t)(detect_cycle_int_limit * 1000.0), &ind);
 		buffer_append_int32(send_buffer, (int32_t)(detect_coupling_k * 1000.0), &ind);
-		packet_send_packet(send_buffer, ind, 0);
+		send_packet(send_buffer, ind);
 		break;
 
 	case COMM_REBOOT:
@@ -368,24 +298,7 @@ static void process_packet(unsigned char *data, unsigned char len) {
 	}
 }
 
-static void send_packet(unsigned char *buffer, unsigned char len) {
-	chMtxLock(&send_mutex);
-	chSequentialStreamWrite(&SDU1, buffer, len);
-	chMtxUnlock();
-}
-
-void comm_init(void) {
-	myUSBinit();
-	packet_init(send_packet, process_packet, 0);
-
-	chMtxInit(&send_mutex);
-
-	// Threads
-	chThdCreateStatic(serial_read_thread_wa, sizeof(serial_read_thread_wa), NORMALPRIO, serial_read_thread, NULL);
-	chThdCreateStatic(serial_process_thread_wa, sizeof(serial_process_thread_wa), NORMALPRIO, serial_process_thread, NULL);
-}
-
-void comm_printf(char* format, ...) {
+void commands_printf(char* format, ...) {
 	va_list arg;
 	va_start (arg, format);
 	int len;
@@ -396,11 +309,11 @@ void comm_printf(char* format, ...) {
 	va_end (arg);
 
 	if(len>0) {
-		packet_send_packet((unsigned char*)print_buffer, (len<254)? len+1: 255, 0);
+		send_packet((unsigned char*)print_buffer, (len<254)? len+1: 255);
 	}
 }
 
-void comm_send_samples(uint8_t *data, int len) {
+void commands_send_samples(uint8_t *data, int len) {
 	uint8_t buffer[len + 1];
 	int index = 0;
 
@@ -410,31 +323,31 @@ void comm_send_samples(uint8_t *data, int len) {
 		buffer[index++] = data[i];
 	}
 
-	packet_send_packet(buffer, index, 0);
+	send_packet(buffer, index);
 }
 
-void comm_send_rotor_pos(float rotor_pos) {
+void commands_send_rotor_pos(float rotor_pos) {
 	uint8_t buffer[5];
 	int32_t index = 0;
 
 	buffer[index++] = COMM_ROTOR_POSITION;
 	buffer_append_int32(buffer, (int32_t)(rotor_pos * 100000.0), &index);
 
-	packet_send_packet(buffer, index, 0);
+	send_packet(buffer, index);
 }
 
-void comm_print_fault_code(mc_fault_code fault_code) {
+void commands_print_fault_code(mc_fault_code fault_code) {
 	switch (fault_code) {
-	case FAULT_CODE_NONE: comm_printf("FAULT_CODE_NONE\n"); break;
-	case FAULT_CODE_OVER_VOLTAGE: comm_printf("FAULT_CODE_OVER_VOLTAGE\n"); break;
-	case FAULT_CODE_UNDER_VOLTAGE: comm_printf("FAULT_CODE_UNDER_VOLTAGE\n"); break;
-	case FAULT_CODE_DRV8302: comm_printf("FAULT_CODE_DRV8302\n"); break;
-	case FAULT_CODE_ABS_OVER_CURRENT: comm_printf("FAULT_CODE_ABS_OVER_CURRENT\n"); break;
+	case FAULT_CODE_NONE: commands_printf("FAULT_CODE_NONE\n"); break;
+	case FAULT_CODE_OVER_VOLTAGE: commands_printf("FAULT_CODE_OVER_VOLTAGE\n"); break;
+	case FAULT_CODE_UNDER_VOLTAGE: commands_printf("FAULT_CODE_UNDER_VOLTAGE\n"); break;
+	case FAULT_CODE_DRV8302: commands_printf("FAULT_CODE_DRV8302\n"); break;
+	case FAULT_CODE_ABS_OVER_CURRENT: commands_printf("FAULT_CODE_ABS_OVER_CURRENT\n"); break;
 	default: break;
 	}
 }
 
-void comm_send_experiment_samples(float *samples, int len) {
+void commands_send_experiment_samples(float *samples, int len) {
 	if ((len * 4 + 1) > 256) {
 		return;
 	}
@@ -448,5 +361,5 @@ void comm_send_experiment_samples(float *samples, int len) {
 		buffer_append_int32(buffer, (int32_t)(samples[i] * 10000.0), &index);
 	}
 
-	packet_send_packet(buffer, index, 0);
+	send_packet(buffer, index);
 }
diff --git a/commands.h b/commands.h
new file mode 100644
index 00000000..f14020e3
--- /dev/null
+++ b/commands.h
@@ -0,0 +1,22 @@
+/*
+ * commands.h
+ *
+ *  Created on: 19 sep 2014
+ *      Author: benjamin
+ */
+
+#ifndef COMMANDS_H_
+#define COMMANDS_H_
+
+#include "datatypes.h"
+
+// Functions
+void commands_set_send_func(void(*func)(unsigned char *data, unsigned char len));
+void commands_process_packet(unsigned char *data, unsigned char len);
+void commands_printf(char* format, ...);
+void commands_send_samples(uint8_t *data, int len);
+void commands_send_rotor_pos(float rotor_pos);
+void commands_print_fault_code(mc_fault_code fault_code);
+void commands_send_experiment_samples(float *samples, int len);
+
+#endif /* COMMANDS_H_ */
diff --git a/conf_general.c b/conf_general.c
index a9b78e8a..5d712936 100644
--- a/conf_general.c
+++ b/conf_general.c
@@ -136,7 +136,12 @@ void conf_general_read_app_configuration(app_configuration *conf) {
 		conf->app_to_use = APP_NONE;
 		conf->app_ppm_ctrl_type = PPM_CTRL_TYPE_CURRENT;
 		conf->app_ppm_pid_max_erpm = 15000;
+		conf->app_ppm_hyst = 0.15;
+		conf->app_ppm_timeout = 1000.0;
+		conf->app_ppm_pulse_start = 1.0;
+		conf->app_ppm_pulse_width = 1.0;
 		conf->app_uart_baudrate = 115200;
+		conf->app_uart_timeout = 1000;
 	}
 }
 
diff --git a/datatypes.h b/datatypes.h
index 74d20daf..94f852fc 100644
--- a/datatypes.h
+++ b/datatypes.h
@@ -102,7 +102,8 @@ typedef struct {
 typedef enum {
 	APP_NONE = 0,
 	APP_PPM,
-	APP_UARTCOMM,
+	APP_UART,
+	APP_PPM_UART,
 	APP_CUSTOM
 } app_use;
 
@@ -124,9 +125,36 @@ typedef struct {
 	// PPM application settings
 	ppm_control_type app_ppm_ctrl_type;
 	float app_ppm_pid_max_erpm;
+	float app_ppm_hyst;
+	uint32_t app_ppm_timeout;
+	float app_ppm_pulse_start;
+	float app_ppm_pulse_width;
 
 	// UART application settings
 	uint32_t app_uart_baudrate;
+	uint32_t app_uart_timeout;
 } app_configuration;
 
+// Communication commands
+typedef enum {
+	COMM_GET_VALUES = 0,
+	COMM_SET_DUTY,
+	COMM_SET_CURRENT,
+	COMM_SET_CURRENT_BRAKE,
+	COMM_SET_RPM,
+	COMM_SET_DETECT,
+	COMM_SET_SERVO_OFFSET,
+	COMM_SET_MCCONF,
+	COMM_GET_MCCONF,
+	COMM_SET_APPCONF,
+	COMM_GET_APPCONF,
+	COMM_SAMPLE_PRINT,
+	COMM_TERMINAL_CMD,
+	COMM_PRINT,
+	COMM_ROTOR_POSITION,
+	COMM_EXPERIMENT_SAMPLE,
+	COMM_DETECT_MOTOR_PARAM,
+	COMM_REBOOT
+} COMM_PACKET_ID;
+
 #endif /* DATATYPES_H_ */
diff --git a/halconf.h b/halconf.h
index 3941f191..71b6fc06 100644
--- a/halconf.h
+++ b/halconf.h
@@ -94,7 +94,7 @@
  * @brief   Enables the ICU subsystem.
  */
 #if !defined(HAL_USE_ICU) || defined(__DOXYGEN__)
-#define HAL_USE_ICU                 FALSE
+#define HAL_USE_ICU                 TRUE
 #endif
 
 /**
diff --git a/hwconf/hw_40.h b/hwconf/hw_40.h
index 38cb685d..1e3c750d 100644
--- a/hwconf/hw_40.h
+++ b/hwconf/hw_40.h
@@ -107,4 +107,10 @@
 #define HW_UART_RX_PORT		GPIOC
 #define HW_UART_RX_PIN		7
 
+// ICU Peripheral for servo decoding
+#define HW_ICU_CHANNEL		ICU_CHANNEL_2
+#define HW_ICU_GPIO_AF		GPIO_AF_TIM3
+#define HW_ICU_GPIO			GPIOB
+#define HW_ICU_PIN			5
+
 #endif /* HW_40_H_ */
diff --git a/hwconf/hw_bw.h b/hwconf/hw_bw.h
index 9d3eae62..35c2dc60 100644
--- a/hwconf/hw_bw.h
+++ b/hwconf/hw_bw.h
@@ -99,4 +99,18 @@
 // Number of servo outputs
 #define HW_SERVO_NUM		2
 
+// UART Peripheral
+#define HW_UART_DEV			UARTD6
+#define HW_UART_GPIO_AF		GPIO_AF_USART6
+#define HW_UART_TX_PORT		GPIOC
+#define HW_UART_TX_PIN		6
+#define HW_UART_RX_PORT		GPIOC
+#define HW_UART_RX_PIN		7
+
+// ICU Peripheral for servo decoding
+#define HW_ICU_CHANNEL		ICU_CHANNEL_2
+#define HW_ICU_GPIO_AF		GPIO_AF_TIM3
+#define HW_ICU_GPIO			GPIOB
+#define HW_ICU_PIN			5
+
 #endif /* HW_40_H_ */
diff --git a/hwconf/hw_r2.h b/hwconf/hw_r2.h
index dd1e5e0d..98034278 100644
--- a/hwconf/hw_r2.h
+++ b/hwconf/hw_r2.h
@@ -117,4 +117,10 @@
 #define HW_UART_RX_PORT		GPIOC
 #define HW_UART_RX_PIN		11
 
+// ICU Peripheral for servo decoding
+#define HW_ICU_CHANNEL		ICU_CHANNEL_2
+#define HW_ICU_GPIO_AF		GPIO_AF_TIM3
+#define HW_ICU_GPIO			GPIOB
+#define HW_ICU_PIN			5
+
 #endif /* HW_R2_H_ */
diff --git a/irq_handlers.c b/irq_handlers.c
index 017daa63..4ed958ae 100644
--- a/irq_handlers.c
+++ b/irq_handlers.c
@@ -22,8 +22,6 @@
 #include "main.h"
 #include "mcpwm.h"
 #include "servo.h"
-#include "comm.h"
-#include "servo_dec.h"
 
 CH_IRQ_HANDLER(TIM7_IRQHandler) {
 	CH_IRQ_PROLOGUE();
@@ -38,35 +36,3 @@ CH_IRQ_HANDLER(ADC1_2_3_IRQHandler) {
 	mcpwm_adc_inj_int_handler();
 	CH_IRQ_EPILOGUE();
 }
-
-CH_IRQ_HANDLER(EXTI3_IRQHandler) {
-	CH_IRQ_PROLOGUE();
-	if (EXTI_GetITStatus(EXTI_Line3) != RESET) {
-		EXTI_ClearITPendingBit(EXTI_Line3);
-		servodec_int_handler();
-	}
-	CH_IRQ_EPILOGUE();
-}
-
-CH_IRQ_HANDLER(EXTI15_10_IRQHandler) {
-	CH_IRQ_PROLOGUE();
-	if (EXTI_GetITStatus(EXTI_Line13) != RESET) {
-		EXTI_ClearITPendingBit(EXTI_Line13);
-		servodec_int_handler();
-	}
-
-	if (EXTI_GetITStatus(EXTI_Line14) != RESET) {
-		EXTI_ClearITPendingBit(EXTI_Line14);
-		servodec_int_handler();
-	}
-	CH_IRQ_EPILOGUE();
-}
-
-CH_IRQ_HANDLER(EXTI9_5_IRQHandler) {
-	CH_IRQ_PROLOGUE();
-	if (EXTI_GetITStatus(EXTI_Line5) != RESET) {
-		EXTI_ClearITPendingBit(EXTI_Line5);
-		servodec_int_handler();
-	}
-	CH_IRQ_EPILOGUE();
-}
diff --git a/main.c b/main.c
index 6e4c8bb0..86c22d1e 100644
--- a/main.c
+++ b/main.c
@@ -27,12 +27,13 @@
 #include "main.h"
 #include "mcpwm.h"
 #include "ledpwm.h"
-#include "comm.h"
+#include "comm_usb.h"
 #include "ledpwm.h"
 #include "terminal.h"
 #include "hw.h"
 #include "app.h"
 #include "packet.h"
+#include "commands.h"
 
 /*
  * Timers used:
@@ -105,7 +106,7 @@ static msg_t periodic_thread(void *arg) {
 			ledpwm_set_intensity(LED_RED, 1.0);
 			if (!fault_print && AUTO_PRINT_FAULTS) {
 				fault_print = 1;
-				comm_print_fault_code(mcpwm_get_fault());
+				commands_print_fault_code(mcpwm_get_fault());
 			}
 		} else {
 			ledpwm_set_intensity(LED_RED, 0.0);
@@ -113,7 +114,7 @@ static msg_t periodic_thread(void *arg) {
 		}
 
 		if (mcpwm_get_state() == MC_STATE_DETECTING) {
-			comm_send_rotor_pos(mcpwm_get_detect_pos());
+			commands_send_rotor_pos(mcpwm_get_detect_pos());
 		}
 
 		chThdSleepMilliseconds(25);
@@ -154,7 +155,7 @@ static msg_t sample_send_thread(void *arg) {
 			buffer[index++] = f_sw_samples[i] >> 8;
 			buffer[index++] = f_sw_samples[i];
 
-			comm_send_samples(buffer, index);
+			commands_send_samples(buffer, index);
 		}
 	}
 
@@ -278,7 +279,7 @@ int main(void) {
 	conf_general_read_mc_configuration(&mcconf);
 	mcpwm_init(&mcconf);
 
-	comm_init();
+	comm_usb_init();
 
 	app_configuration appconf;
 	conf_general_read_app_configuration(&appconf);
diff --git a/mcpwm.c b/mcpwm.c
index 8c00476d..ee1cb46c 100644
--- a/mcpwm.c
+++ b/mcpwm.c
@@ -34,7 +34,6 @@
 #include "digital_filter.h"
 #include "utils.h"
 #include "ledpwm.h"
-#include "comm.h"
 #include "hw.h"
 
 // Structs
diff --git a/mcuconf.h b/mcuconf.h
index 49c73eec..1d1eb6ef 100644
--- a/mcuconf.h
+++ b/mcuconf.h
@@ -152,7 +152,7 @@
  */
 #define STM32_ICU_USE_TIM1                  FALSE
 #define STM32_ICU_USE_TIM2                  FALSE
-#define STM32_ICU_USE_TIM3                  FALSE
+#define STM32_ICU_USE_TIM3                  TRUE
 #define STM32_ICU_USE_TIM4                  FALSE
 #define STM32_ICU_USE_TIM5                  FALSE
 #define STM32_ICU_USE_TIM8                  FALSE
diff --git a/servo_dec.c b/servo_dec.c
index 706428cc..975abd11 100644
--- a/servo_dec.c
+++ b/servo_dec.c
@@ -26,33 +26,59 @@
 #include "stm32f4xx_conf.h"
 #include "ch.h"
 #include "hal.h"
+#include "hw.h"
 
 /*
  * Settings
  */
-#define USE_PROGRAMMING_CONN	0
-
-#if USE_PROGRAMMING_CONN
-#define SERVO_NUM				3
-#else
 #define SERVO_NUM				1
-#endif
-
 #define TIMER_FREQ				1000000
-#define INTERRUPT_TRESHOLD		3
 
 // Private variables
-static volatile uint32_t interrupt_time = 0;
-static volatile int8_t servo_pos[SERVO_NUM];
-static volatile uint32_t time_since_update;
-static VirtualTimer vt;
-
-// Private functions
-static void update_counters(void *p);
+static volatile systime_t last_update_time;
+static volatile float servo_pos[SERVO_NUM];
+static volatile float pulse_start = 1.0;
+static volatile float pulse_width = 1.0;
 
 // Function pointers
 static void(*done_func)(void) = 0;
 
+static void icuwidthcb(ICUDriver *icup) {
+	float len = ((float)icuGetWidth(icup) / ((float)TIMER_FREQ / 1000.0)) - pulse_start;
+
+	if (len > pulse_width) {
+		if (len < pulse_width * 1.2) {
+			len = pulse_width;
+		} else {
+			// Too long pulse. Most likely something is wrong.
+			len = -1.0;
+		}
+	}
+
+	if (len > 0.0) {
+		servo_pos[0] = (len * 2.0 - pulse_width) / pulse_width;
+		last_update_time = chTimeNow();
+
+		if (done_func) {
+			done_func();
+		}
+	}
+}
+
+static void icuperiodcb(ICUDriver *icup) {
+	(void)icup;
+}
+
+static ICUConfig icucfg = {
+		ICU_INPUT_ACTIVE_HIGH,
+		TIMER_FREQ,
+		icuwidthcb,
+		icuperiodcb,
+		NULL,
+		HW_ICU_CHANNEL,
+		0
+};
+
 /**
  * Initialize the serve decoding driver.
  *
@@ -61,188 +87,43 @@ static void(*done_func)(void) = 0;
  * decoded. Can be NULL.
  */
 void servodec_init(void (*d_func)(void)) {
-	// Initialize variables
-	time_since_update = 0;
-	interrupt_time = 0;
-
-	NVIC_InitTypeDef   NVIC_InitStructure;
-	EXTI_InitTypeDef   EXTI_InitStructure;
-	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
-	uint16_t PrescalerValue = 0;
-
-	// ------------- EXTI -------------- //
-	// Clocks
-	RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
-#if USE_PROGRAMMING_CONN
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
-#endif
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
-
-#if USE_PROGRAMMING_CONN
-	palSetPadMode(GPIOA, 13, PAL_MODE_INPUT);
-	palSetPadMode(GPIOA, 14, PAL_MODE_INPUT);
-	palSetPadMode(GPIOB, 3, PAL_MODE_INPUT);
-
-	SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOA, EXTI_PinSource13);
-	SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOA, EXTI_PinSource14);
-	SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOB, EXTI_PinSource3);
-
-	EXTI_InitStructure.EXTI_Line = EXTI_Line3 | EXTI_Line13 | EXTI_Line14;
-#else
-	palSetPadMode(GPIOB, 5, PAL_MODE_INPUT);
-
-	SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOB, EXTI_PinSource5);
-
-	EXTI_InitStructure.EXTI_Line = EXTI_Line5;
-#endif
-
-	EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
-	EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
-	EXTI_InitStructure.EXTI_LineCmd = ENABLE;
-	EXTI_Init(&EXTI_InitStructure);
-
-#if USE_PROGRAMMING_CONN
-	NVIC_InitStructure.NVIC_IRQChannel = EXTI3_IRQn;
-	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
-	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
-	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-	NVIC_Init(&NVIC_InitStructure);
-
-	NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
-	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
-	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
-	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-	NVIC_Init(&NVIC_InitStructure);
-#else
-	NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn;
-	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
-	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
-	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-	NVIC_Init(&NVIC_InitStructure);
-#endif
-
-	// ------------- Timer3 ------------- //
-	/* Compute the prescaler value */
-	/* TIM3 clock enable */
-	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
-
-	PrescalerValue = (uint16_t) ((SYSTEM_CORE_CLOCK / 2) / TIMER_FREQ) - 1;
-
-	/* Time base configuration */
-	TIM_TimeBaseStructure.TIM_Period = 65535;
-	TIM_TimeBaseStructure.TIM_Prescaler = 0;
-	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
-	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
-
-	TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
-
-	/* Prescaler configuration */
-	TIM_PrescalerConfig(TIM3, PrescalerValue, TIM_PSCReloadMode_Immediate);
-
-	/* TIM3 enable counter */
-	TIM_Cmd(TIM3, ENABLE);
-
-	// Set up a virtual timer to update the counters
-	chSysLock();
-	chVTSetI(&vt, MS2ST(1), update_counters, NULL);
-	chSysUnlock();
+	icuStart(&ICUD3, &icucfg);
+	palSetPadMode(HW_ICU_GPIO, HW_ICU_PIN, PAL_MODE_ALTERNATE(HW_ICU_GPIO_AF));
+	icuEnable(&ICUD3);
 
 	// Set our function pointer
 	done_func = d_func;
 }
 
-static void update_counters(void *p) {
-	(void)p;
-
-	chSysLockFromIsr();
-	chVTSetI(&vt, MS2ST(1), update_counters, p);
-	chSysUnlockFromIsr();
-
-	interrupt_time++;
-	time_since_update++;
-}
-
-void servodec_int_handler(void) {
-	static int curr_index = 0;
-
-	// Long time since last interrupt means that a new cycle has started
-	if (interrupt_time >= INTERRUPT_TRESHOLD) {
-		curr_index = 0;
-		interrupt_time = 0;
-		TIM3->CNT = 0;
-		return;
-	}
-
-	if (curr_index < SERVO_NUM) {
-		// Use floating point because we can :)
-		float time_ms = (float)(TIM3->CNT);
-		time_ms = (time_ms * 1000.0) / (float)TIMER_FREQ;
-
-		if (time_ms < 0.4) {
-			return;
-		}
-
-		TIM3->CNT = 0;
-
-		// Check if pulse is within valid range
-		if (time_ms > 0.8 && time_ms < 2.2) {
-
-			// Truncate (just in case)
-			if (time_ms > 2.0) {
-				time_ms = 2.0;
-			}
-
-			if (time_ms < 1.0) {
-				time_ms = 1.0;
-			}
-
-			// Update position
-			servo_pos[curr_index] = (int8_t)((time_ms - 1.5)  * 255.0);
-		}
-	}
-
-	curr_index++;
-
-	if (curr_index == SERVO_NUM) {
-		time_since_update = 0;
-
-		// Call the function pointer if it is not NULL.
-		if (done_func) {
-			done_func();
-		}
-	}
-
-	interrupt_time = 0;
-}
-
 /**
- * Get a decoded servo value as an integer.
+ * Change the limits of how the servo pulses should be decoded.
  *
- * @param servo_num
- * The servo index. If it is out of range, 0 will be returned.
+ * @param start
+ * The amount of milliseconds the pulse starts at (default is 1.0)
  *
- * @return
- * The servo value in the range [-128 127].
+ * @param width
+ * The width of the pulse in milliseconds (default is 1.0)
  */
-int8_t servodec_get_servo(int servo_num) {
-	if (servo_num < SERVO_NUM) {
-		return servo_pos[servo_num];
-	} else {
-		return 0;
-	}
+void servodec_set_pulse_options(float start, float width) {
+	pulse_start = start;
+	pulse_width = width;
 }
 
 /**
- * Get a decoded servo value as a float.
+ * Get a decoded servo value.
  *
  * @param servo_num
- * The servo index. If it is out of range, 0 will be returned.
+ * The servo index. If it is out of range, 0.0 will be returned.
  *
  * @return
  * The servo value in the range [-1.0 1.0].
  */
-float servodec_get_servo_as_float(int servo_num) {
-	return (float)servodec_get_servo(servo_num) / 128.0;
+float servodec_get_servo(int servo_num) {
+	if (servo_num < SERVO_NUM) {
+		return servo_pos[servo_num];
+	} else {
+		return 0.0;
+	}
 }
 
 /**
@@ -253,5 +134,5 @@ float servodec_get_servo_as_float(int servo_num) {
  * The amount of milliseconds that have passed since an update.
  */
 uint32_t servodec_get_time_since_update(void) {
-	return time_since_update;
+	return chTimeElapsedSince(last_update_time) / (CH_FREQUENCY / 1000);
 }
diff --git a/servo_dec.h b/servo_dec.h
index 58828512..e6694a40 100644
--- a/servo_dec.h
+++ b/servo_dec.h
@@ -28,16 +28,10 @@
 #include <stdint.h>
 #include <conf_general.h>
 
-// Servo function indexes
-#define SERVODEC_IND_STEERING		0
-#define SERVODEC_IND_THROTTLE		1
-#define SERVODEC_IND_AUX			2
-
 // Functions
 void servodec_init(void (*d_func)(void));
-void servodec_int_handler(void);
-int8_t servodec_get_servo(int servo_num);
-float servodec_get_servo_as_float(int servo_num);
+void servodec_set_pulse_options(float start, float width);
+float servodec_get_servo(int servo_num);
 uint32_t servodec_get_time_since_update(void);
 
 #endif /* SERVO_DEC_H_ */
diff --git a/terminal.c b/terminal.c
index 1338499a..586e2459 100644
--- a/terminal.c
+++ b/terminal.c
@@ -25,7 +25,7 @@
 #include "ch.h"
 #include "hal.h"
 #include "terminal.h"
-#include "comm.h"
+#include "commands.h"
 #include "mcpwm.h"
 #include "main.h"
 #include "hw.h"
@@ -45,47 +45,47 @@ void terminal_process_string(char *str) {
 	}
 
 	if (argc == 0) {
-		comm_printf("No command received\n");
+		commands_printf("No command received\n");
 		return;
 	}
 
 	if (strcmp(argv[0], "ping") == 0) {
-		comm_printf("pong\n");
+		commands_printf("pong\n");
 	} else if (strcmp(argv[0], "stop") == 0) {
 		mcpwm_set_duty(0);
-		comm_printf("Motor stopped\n");
+		commands_printf("Motor stopped\n");
 	} else if (strcmp(argv[0], "last_adc_duration") == 0) {
-		comm_printf("Latest ADC duration: %.4f ms", (double)(mcpwm_get_last_adc_isr_duration() * 1000.0));
-		comm_printf("Latest injected ADC duration: %.4f ms", (double)(mcpwm_get_last_inj_adc_isr_duration() * 1000.0));
-		comm_printf("Latest main ADC duration: %.4f ms\n", (double)(main_get_last_adc_isr_duration() * 1000.0));
+		commands_printf("Latest ADC duration: %.4f ms", (double)(mcpwm_get_last_adc_isr_duration() * 1000.0));
+		commands_printf("Latest injected ADC duration: %.4f ms", (double)(mcpwm_get_last_inj_adc_isr_duration() * 1000.0));
+		commands_printf("Latest main ADC duration: %.4f ms\n", (double)(main_get_last_adc_isr_duration() * 1000.0));
 	} else if (strcmp(argv[0], "kv") == 0) {
-		comm_printf("Calculated KV: %.2f rpm/volt\n", (double)mcpwm_get_kv_filtered());
+		commands_printf("Calculated KV: %.2f rpm/volt\n", (double)mcpwm_get_kv_filtered());
 	} else if (strcmp(argv[0], "mem") == 0) {
 		size_t n, size;
 		n = chHeapStatus(NULL, &size);
-		comm_printf("core free memory : %u bytes", chCoreStatus());
-		comm_printf("heap fragments   : %u", n);
-		comm_printf("heap free total  : %u bytes\n", size);
+		commands_printf("core free memory : %u bytes", chCoreStatus());
+		commands_printf("heap fragments   : %u", n);
+		commands_printf("heap free total  : %u bytes\n", size);
 	} else if (strcmp(argv[0], "threads") == 0) {
 		Thread *tp;
 		static const char *states[] = {THD_STATE_NAMES};
-		comm_printf("    addr    stack prio refs     state           name time    ");
-		comm_printf("-------------------------------------------------------------");
+		commands_printf("    addr    stack prio refs     state           name time    ");
+		commands_printf("-------------------------------------------------------------");
 		tp = chRegFirstThread();
 		do {
-			comm_printf("%.8lx %.8lx %4lu %4lu %9s %14s %lu",
+			commands_printf("%.8lx %.8lx %4lu %4lu %9s %14s %lu",
 					(uint32_t)tp, (uint32_t)tp->p_ctx.r13,
 					(uint32_t)tp->p_prio, (uint32_t)(tp->p_refs - 1),
 					states[tp->p_state], tp->p_name, (uint32_t)tp->p_time);
 			tp = chRegNextThread(tp);
 		} while (tp != NULL);
-		comm_printf("");
+		commands_printf("");
 	} else if (strcmp(argv[0], "fault") == 0) {
-		comm_print_fault_code(mcpwm_get_fault());
+		commands_print_fault_code(mcpwm_get_fault());
 	} else if (strcmp(argv[0], "rpm") == 0) {
-		comm_printf("Electrical RPM: %.2f rpm\n", (double)mcpwm_get_rpm());
+		commands_printf("Electrical RPM: %.2f rpm\n", (double)mcpwm_get_rpm());
 	} else if (strcmp(argv[0], "tacho") == 0) {
-		comm_printf("Tachometer counts: %i\n", mcpwm_get_tachometer_value(0));
+		commands_printf("Tachometer counts: %i\n", mcpwm_get_tachometer_value(0));
 	} else if (strcmp(argv[0], "tim") == 0) {
 		chSysLock();
 		volatile int t1_cnt = TIM1->CNT;
@@ -96,15 +96,15 @@ void terminal_process_string(char *str) {
 		int voltage_samp = TIM8->CCR1;
 		int current1_samp = TIM1->CCR4;
 		int current2_samp = TIM8->CCR2;
-		comm_printf("Tim1 CNT: %i", t1_cnt);
-		comm_printf("Tim8 CNT: %u", t8_cnt);
-		comm_printf("Duty cycle: %u", duty);
-		comm_printf("Top: %u", top);
-		comm_printf("Voltage sample: %u", voltage_samp);
-		comm_printf("Current 1 sample: %u", current1_samp);
-		comm_printf("Current 2 sample: %u\n", current2_samp);
+		commands_printf("Tim1 CNT: %i", t1_cnt);
+		commands_printf("Tim8 CNT: %u", t8_cnt);
+		commands_printf("Duty cycle: %u", duty);
+		commands_printf("Top: %u", top);
+		commands_printf("Voltage sample: %u", voltage_samp);
+		commands_printf("Current 1 sample: %u", current1_samp);
+		commands_printf("Current 2 sample: %u\n", current2_samp);
 	} else if (strcmp(argv[0], "volt") == 0) {
-		comm_printf("Input voltage: %.2f\n", (double)GET_INPUT_VOLTAGE());
+		commands_printf("Input voltage: %.2f\n", (double)GET_INPUT_VOLTAGE());
 	} else if (strcmp(argv[0], "param_detect") == 0) {
 		// Use COMM_MODE_DELAY and try to figure out the motor parameters.
 		if (argc == 4) {
@@ -115,7 +115,7 @@ void terminal_process_string(char *str) {
 			sscanf(argv[2], "%f", &min_rpm);
 			sscanf(argv[3], "%f", &low_duty);
 
-			const mc_configuration *mcconf = mcpwm_get_configuration();
+			const volatile mc_configuration *mcconf = mcpwm_get_configuration();
 
 			if (current > 0.0 && current < mcconf->l_current_max &&
 					min_rpm > 10.0 && min_rpm < 3000.0 &&
@@ -124,22 +124,22 @@ void terminal_process_string(char *str) {
 				float cycle_integrator;
 				float coupling_k;
 				if (conf_general_detect_motor_param(current, min_rpm, low_duty, &cycle_integrator, &coupling_k)) {
-					comm_printf("Cycle integrator limit: %.2f", (double)cycle_integrator);
-					comm_printf("Coupling factor: %.2f\n", (double)coupling_k);
+					commands_printf("Cycle integrator limit: %.2f", (double)cycle_integrator);
+					commands_printf("Coupling factor: %.2f\n", (double)coupling_k);
 				} else {
-					comm_printf("Detection failed. Try again with different parameters.\n");
+					commands_printf("Detection failed. Try again with different parameters.\n");
 				}
 			} else {
-				comm_printf("Invalid argument(s).\n");
+				commands_printf("Invalid argument(s).\n");
 			}
 		} else {
-			comm_printf("This command requires three arguments.\n");
+			commands_printf("This command requires three arguments.\n");
 		}
 	} else if (strcmp(argv[0], "rpm_dep") == 0) {
 		mc_rpm_dep_struct rpm_dep = mcpwm_get_rpm_dep();
-		comm_printf("Cycle int limit: %.2f", (double)rpm_dep.cycle_int_limit);
-		comm_printf("Cycle int limit running: %.2f", (double)rpm_dep.cycle_int_limit_running);
-		comm_printf("Cycle int limit max: %.2f\n", (double)rpm_dep.cycle_int_limit_max);
+		commands_printf("Cycle int limit: %.2f", (double)rpm_dep.cycle_int_limit);
+		commands_printf("Cycle int limit running: %.2f", (double)rpm_dep.cycle_int_limit_running);
+		commands_printf("Cycle int limit max: %.2f\n", (double)rpm_dep.cycle_int_limit_max);
 	}
 
 	// Setters
@@ -154,67 +154,67 @@ void terminal_process_string(char *str) {
 
 			if (dir >= 0 && fwd_add >= 0 && rev_add >= 0) {
 				mcpwm_init_hall_table(dir, fwd_add, rev_add);
-				comm_printf("New hall sensor dir: %i fwd_add %i rev_add %i\n",
+				commands_printf("New hall sensor dir: %i fwd_add %i rev_add %i\n",
 						dir, fwd_add, rev_add);
 			} else {
-				comm_printf("Invalid argument(s).\n");
+				commands_printf("Invalid argument(s).\n");
 			}
 		} else {
-			comm_printf("This command requires three arguments.\n");
+			commands_printf("This command requires three arguments.\n");
 		}
 	}
 
 	// The help command
 	else if (strcmp(argv[0], "help") == 0) {
-		comm_printf("Valid commands are:");
-		comm_printf("help");
-		comm_printf("  Show this help");
+		commands_printf("Valid commands are:");
+		commands_printf("help");
+		commands_printf("  Show this help");
 
-		comm_printf("ping");
-		comm_printf("  Print pong here to see if the reply works");
+		commands_printf("ping");
+		commands_printf("  Print pong here to see if the reply works");
 
-		comm_printf("stop");
-		comm_printf("  Stop the motor");
+		commands_printf("stop");
+		commands_printf("  Stop the motor");
 
-		comm_printf("last_adc_duration");
-		comm_printf("  The time the latest ADC interrupt consumed");
+		commands_printf("last_adc_duration");
+		commands_printf("  The time the latest ADC interrupt consumed");
 
-		comm_printf("kv");
-		comm_printf("  The calculated kv of the motor");
+		commands_printf("kv");
+		commands_printf("  The calculated kv of the motor");
 
-		comm_printf("mem");
-		comm_printf("  Show memory usage");
+		commands_printf("mem");
+		commands_printf("  Show memory usage");
 
-		comm_printf("threads");
-		comm_printf("  List all threads");
+		commands_printf("threads");
+		commands_printf("  List all threads");
 
-		comm_printf("fault");
-		comm_printf("  Prints the current fault code");
+		commands_printf("fault");
+		commands_printf("  Prints the current fault code");
 
-		comm_printf("rpm");
-		comm_printf("  Prints the current electrical RPM");
+		commands_printf("rpm");
+		commands_printf("  Prints the current electrical RPM");
 
-		comm_printf("tacho");
-		comm_printf("  Prints tachometer value");
+		commands_printf("tacho");
+		commands_printf("  Prints tachometer value");
 
-		comm_printf("tim");
-		comm_printf("  Prints tim1 and tim8 settings");
+		commands_printf("tim");
+		commands_printf("  Prints tim1 and tim8 settings");
 
-		comm_printf("set_hall_table [dir] [fwd_add] [rev_add]");
-		comm_printf("  Update the hall sensor lookup table");
+		commands_printf("set_hall_table [dir] [fwd_add] [rev_add]");
+		commands_printf("  Update the hall sensor lookup table");
 
-		comm_printf("volt");
-		comm_printf("  Prints different voltages");
+		commands_printf("volt");
+		commands_printf("  Prints different voltages");
 
-		comm_printf("param_detect [current] [min_rpm] [low_duty]");
-		comm_printf("  Spin up the motor in COMM_MODE_DELAY and compute its parameters.");
-		comm_printf("  This test should be performed without load on the motor.");
-		comm_printf("  Example: param_detect 5.0 600 0.06");
+		commands_printf("param_detect [current] [min_rpm] [low_duty]");
+		commands_printf("  Spin up the motor in COMM_MODE_DELAY and compute its parameters.");
+		commands_printf("  This test should be performed without load on the motor.");
+		commands_printf("  Example: param_detect 5.0 600 0.06");
 
-		comm_printf("rpm_dep");
-				comm_printf("  Prints some rpm-dep values\n");
+		commands_printf("rpm_dep");
+		commands_printf("  Prints some rpm-dep values\n");
 	} else {
-		comm_printf("Invalid command: %s\n"
+		commands_printf("Invalid command: %s\n"
 				"type help to list all available commands\n", argv[0]);
 	}
 }