diff --git a/Makefile b/Makefile
index e52dc7cbf..ee4ba0de2 100644
--- a/Makefile
+++ b/Makefile
@@ -108,6 +108,7 @@ CSRC = $(PORTSRC) \
        servo_dec.c \
        utils.c \
        servo.c \
+       servo_simple.c \
        packet.c \
        terminal.c \
        conf_general.c \
diff --git a/applications/app_ppm.c b/applications/app_ppm.c
index 23e88d92a..9205d2635 100644
--- a/applications/app_ppm.c
+++ b/applications/app_ppm.c
@@ -34,6 +34,9 @@
 #include "comm_can.h"
 #include <math.h>
 
+// Only available if servo output is not active
+#if !SERVO_OUT_ENABLE
+
 // Settings
 #define MAX_CAN_AGE						0.1
 #define MIN_PULSES_WITHOUT_POWER		50
@@ -54,24 +57,32 @@ static volatile int pulses_without_power = 0;
 
 // Private functions
 static void update(void *p);
+#endif
 
 void app_ppm_configure(ppm_config *conf) {
+#if !SERVO_OUT_ENABLE
 	config = *conf;
 	pulses_without_power = 0;
 
 	if (is_running) {
 		servodec_set_pulse_options(config.pulse_start, config.pulse_end, config.median_filter);
 	}
+#else
+	(void)conf;
+#endif
 }
 
 void app_ppm_start(void) {
+#if !SERVO_OUT_ENABLE
 	chThdCreateStatic(ppm_thread_wa, sizeof(ppm_thread_wa), NORMALPRIO, ppm_thread, NULL);
 
 	chSysLock();
 	chVTSetI(&vt, MS2ST(1), update, NULL);
 	chSysUnlock();
+#endif
 }
 
+#if !SERVO_OUT_ENABLE
 static void servodec_func(void) {
 	chSysLockFromIsr();
 	timeout_reset();
@@ -308,3 +319,4 @@ static msg_t ppm_thread(void *arg) {
 
 	return 0;
 }
+#endif
diff --git a/buffer.c b/buffer.c
index 436967edf..81f09ba30 100644
--- a/buffer.c
+++ b/buffer.c
@@ -24,6 +24,16 @@
 
 #include "buffer.h"
 
+void buffer_append_int16(uint8_t* buffer, int16_t number, int32_t *index) {
+	buffer[(*index)++] = number >> 8;
+	buffer[(*index)++] = number;
+}
+
+void buffer_append_uint16(uint8_t* buffer, uint16_t number, int32_t *index) {
+	buffer[(*index)++] = number >> 8;
+	buffer[(*index)++] = number;
+}
+
 void buffer_append_int32(uint8_t* buffer, int32_t number, int32_t *index) {
 	buffer[(*index)++] = number >> 24;
 	buffer[(*index)++] = number >> 16;
@@ -38,14 +48,12 @@ void buffer_append_uint32(uint8_t* buffer, uint32_t number, int32_t *index) {
 	buffer[(*index)++] = number;
 }
 
-void buffer_append_int16(uint8_t* buffer, int16_t number, int32_t *index) {
-	buffer[(*index)++] = number >> 8;
-	buffer[(*index)++] = number;
+void buffer_append_float16(uint8_t* buffer, float number, float scale, int32_t *index) {
+    buffer_append_int16(buffer, (int16_t)(number * scale), index);
 }
 
-void buffer_append_uint16(uint8_t* buffer, uint16_t number, int32_t *index) {
-	buffer[(*index)++] = number >> 8;
-	buffer[(*index)++] = number;
+void buffer_append_float32(uint8_t* buffer, float number, float scale, int32_t *index) {
+    buffer_append_int32(buffer, (int32_t)(number * scale), index);
 }
 
 int16_t buffer_get_int16(const uint8_t *buffer, int32_t *index) {
@@ -79,3 +87,11 @@ uint32_t buffer_get_uint32(const uint8_t *buffer, int32_t *index) {
 	*index += 4;
 	return res;
 }
+
+float buffer_get_float16(const uint8_t *buffer, float scale, int32_t *index) {
+    return (float)buffer_get_int16(buffer, index) / scale;
+}
+
+float buffer_get_float32(const uint8_t *buffer, float scale, int32_t *index) {
+    return (float)buffer_get_int32(buffer, index) / scale;
+}
diff --git a/buffer.h b/buffer.h
index cd84d70d7..074303a4f 100644
--- a/buffer.h
+++ b/buffer.h
@@ -27,13 +27,17 @@
 
 #include <stdint.h>
 
-void buffer_append_int32(uint8_t* buffer, int32_t number, int32_t *index);
-void buffer_append_uint32(uint8_t* buffer, uint32_t number, int32_t *index);
 void buffer_append_int16(uint8_t* buffer, int16_t number, int32_t *index);
 void buffer_append_uint16(uint8_t* buffer, uint16_t number, int32_t *index);
+void buffer_append_int32(uint8_t* buffer, int32_t number, int32_t *index);
+void buffer_append_uint32(uint8_t* buffer, uint32_t number, int32_t *index);
+void buffer_append_float16(uint8_t* buffer, float number, float scale, int32_t *index);
+void buffer_append_float32(uint8_t* buffer, float number, float scale, int32_t *index);
 int16_t buffer_get_int16(const uint8_t *buffer, int32_t *index);
 uint16_t buffer_get_uint16(const uint8_t *buffer, int32_t *index);
 int32_t buffer_get_int32(const uint8_t *buffer, int32_t *index);
 uint32_t buffer_get_uint32(const uint8_t *buffer, int32_t *index);
+float buffer_get_float16(const uint8_t *buffer, float scale, int32_t *index);
+float buffer_get_float32(const uint8_t *buffer, float scale, int32_t *index);
 
 #endif /* BUFFER_H_ */
diff --git a/commands.c b/commands.c
index e7c2f7d8d..d30b83f1b 100644
--- a/commands.c
+++ b/commands.c
@@ -28,6 +28,7 @@
 #include "main.h"
 #include "stm32f4xx_conf.h"
 #include "servo.h"
+#include "servo_simple.h"
 #include "buffer.h"
 #include "myUSB.h"
 #include "terminal.h"
@@ -215,8 +216,15 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		timeout_reset();
 		break;
 
-	case COMM_SET_SERVO_OFFSET:
-		servos[0].offset = data[0];
+	case COMM_SET_SERVO_POS:
+#if SERVO_OUT_ENABLE
+		ind = 0;
+#if SERVO_OUT_SIMPLE
+		servo_simple_set_output(buffer_get_float16(data, 1000.0, &ind));
+#else
+		servos[0].pos = (int16_t)(buffer_get_float16(data, 1000.0, &ind) * 255.0);
+#endif
+#endif
 		break;
 
 	case COMM_SET_MCCONF:
diff --git a/conf_general.h b/conf_general.h
index 3f13e79a3..e79a3e9e4 100644
--- a/conf_general.h
+++ b/conf_general.h
@@ -27,7 +27,7 @@
 
 // Firmware version
 #define FW_VERSION_MAJOR	1
-#define FW_VERSION_MINOR	8
+#define FW_VERSION_MINOR	9
 
 #include "datatypes.h"
 
@@ -87,7 +87,7 @@
 /*
  * Settings for the external LEDs (hardcoded for now)
  */
-#define LED_EXT_BATT_LOW		25.6
+#define LED_EXT_BATT_LOW		28.0
 #define LED_EXT_BATT_HIGH		33.0
 
 /*
@@ -99,6 +99,15 @@
 #define WS2811_LED_NUM			14
 #define WS2811_USE_CH2			1		// 0: CH1 (PB6) 1: CH2 (PB7)
 
+/*
+ * Servo output driver
+ */
+#define SERVO_OUT_ENABLE		0		// Enable servo output
+#define SERVO_OUT_SIMPLE		1		// Use simple HW-based driver (recommended)
+#define SERVO_OUT_PULSE_MIN_US	1000	// Minimum pulse length in microseconds
+#define SERVO_OUT_PULSE_MAX_US	2000	// Maximum pulse length in microseconds
+#define SERVO_OUT_RATE_HZ		50		// Update rate in Hz
+
 // Functions
 void conf_general_init(void);
 void conf_general_read_app_configuration(app_configuration *conf);
diff --git a/datatypes.h b/datatypes.h
index d4ed8f7d0..acb69a3d9 100644
--- a/datatypes.h
+++ b/datatypes.h
@@ -274,7 +274,7 @@ typedef enum {
 	COMM_SET_RPM,
 	COMM_SET_POS,
 	COMM_SET_DETECT,
-	COMM_SET_SERVO_OFFSET,
+	COMM_SET_SERVO_POS,
 	COMM_SET_MCCONF,
 	COMM_GET_MCCONF,
 	COMM_SET_APPCONF,
@@ -363,7 +363,8 @@ typedef struct {
 
 typedef enum {
 	MOTE_PACKET_BATT_LEVEL = 0,
-	MOTE_PACKET_BUTTONS
+	MOTE_PACKET_BUTTONS,
+	MOTE_PACKET_ALIVE
 } MOTE_PACKET;
 
 #endif /* DATATYPES_H_ */
diff --git a/irq_handlers.c b/irq_handlers.c
index 559fe7cdf..9e8c07b52 100644
--- a/irq_handlers.c
+++ b/irq_handlers.c
@@ -24,12 +24,14 @@
 #include "servo.h"
 #include "hw.h"
 
+#if SERVO_OUT_ENABLE && !SERVO_OUT_SIMPLE
 CH_IRQ_HANDLER(TIM7_IRQHandler) {
 	CH_IRQ_PROLOGUE();
 	TIM_ClearITPendingBit(TIM7, TIM_IT_Update);
 	servo_irq();
 	CH_IRQ_EPILOGUE();
 }
+#endif
 
 CH_IRQ_HANDLER(ADC1_2_3_IRQHandler) {
 	CH_IRQ_PROLOGUE();
diff --git a/main.c b/main.c
index 44ce94ee3..dc0054dac 100644
--- a/main.c
+++ b/main.c
@@ -39,6 +39,8 @@
 #include "ws2811.h"
 #include "led_external.h"
 #include "encoder.h"
+#include "servo.h"
+#include "servo_simple.h"
 
 /*
  * Timers used:
@@ -47,7 +49,7 @@
  * TIM2: mcpwm
  * TIM12: mcpwm
  * TIM8: mcpwm
- * TIM3: servo_dec/Encoder (HW_R2)
+ * TIM3: servo_dec/Encoder (HW_R2)/servo_simple
  * TIM4: WS2811/WS2812 LEDs/Encoder (other HW)
  *
  * DMA/stream	Device		Function
@@ -322,6 +324,14 @@ int main(void) {
 	encoder_init();
 #endif
 
+#if SERVO_OUT_ENABLE
+#if SERVO_OUT_SIMPLE
+	servo_simple_init();
+#else
+	servo_init();
+#endif
+#endif
+
 	// Threads
 	chThdCreateStatic(periodic_thread_wa, sizeof(periodic_thread_wa), NORMALPRIO, periodic_thread, NULL);
 	chThdCreateStatic(sample_send_thread_wa, sizeof(sample_send_thread_wa), NORMALPRIO - 1, sample_send_thread, NULL);
diff --git a/nrf/nrf_driver.c b/nrf/nrf_driver.c
index cbab48596..d1fa0b0fd 100644
--- a/nrf/nrf_driver.c
+++ b/nrf/nrf_driver.c
@@ -58,9 +58,12 @@ static msg_t tx_thread(void *arg) {
 	chRegSetThreadName("Nrf TX");
 
 	for(;;) {
-		// TODO! Send status
+		uint8_t pl[6];
+		int32_t index = 0;
+		pl[index++] = MOTE_PACKET_ALIVE;
+		rfhelp_send_data_crc((char*)pl, index);
 
-		chThdSleepMilliseconds(500);
+		chThdSleepMilliseconds(50);
 	}
 
 	return 0;
@@ -79,7 +82,7 @@ static msg_t rx_thread(void *arg) {
 		int pipe;
 
 		for(;;) {
-			int res = rfhelp_read_rx_data((char*)buf, &len, &pipe);
+			int res = rfhelp_read_rx_data_crc((char*)buf, &len, &pipe);
 			chuck_data cdata;
 			int32_t index = 0;
 			int buttons;
diff --git a/nrf/rf.c b/nrf/rf.c
index 70eba296b..b54bf7f59 100644
--- a/nrf/rf.c
+++ b/nrf/rf.c
@@ -23,7 +23,7 @@
 void rf_init(void) {
 	spi_sw_init();
 
-	rf_power_up();
+	rf_power_down();
 
 	// Set default register values (TODO for the rest)
 	rf_write_reg_byte(NRF_REG_EN_RXADDR, 0);
@@ -44,6 +44,7 @@ void rf_init(void) {
 
 	// Note: The address should be set by the application.
 
+	rf_power_up();
 	rf_mode_rx();
 	rf_flush_all();
 	rf_clear_irq();
diff --git a/nrf/rfhelp.c b/nrf/rfhelp.c
index 63b606855..5b022e584 100644
--- a/nrf/rfhelp.c
+++ b/nrf/rfhelp.c
@@ -19,6 +19,7 @@
 #include "rf.h"
 #include "ch.h"
 #include "hal.h"
+#include "crc.h"
 #include <string.h>
 #include <stdbool.h>
 
@@ -129,6 +130,33 @@ int rfhelp_send_data(char *data, int len) {
 	return retval;
 }
 
+/**
+ * Same as rfhelp_send_data, but will add a crc checksum to the end. This is
+ * useful for protecting against corruption between the NRF and the MCU in case
+ * there are errors on the SPI bus.
+ *
+ * @param data
+ * The data to be sent.
+ *
+ * @param len
+ * Length of the data. Should be no more than 30 bytes.
+ *
+ * @return
+ * 0: Send OK.
+ * -1: Max RT.
+ * -2: Timeout
+ */
+int rfhelp_send_data_crc(char *data, int len) {
+	char buffer[len + 2];
+	unsigned short crc = crc16((unsigned char*)data, len);
+
+	memcpy(buffer, data, len);
+	buffer[len] = (char)(crc >> 8);
+	buffer[len + 1] = (char)(crc & 0xFF);
+
+	return rfhelp_send_data(buffer, len + 2);
+}
+
 /**
  * Read data from the RX fifo
  *
@@ -182,6 +210,42 @@ int rfhelp_read_rx_data(char *data, int *len, int *pipe) {
 	return retval;
 }
 
+/**
+ * Same as rfhelp_read_rx_data, but will check if there is a valid CRC in the
+ * end of the payload.
+ *
+ * @param data
+ * Pointer to the array in which to store the data.
+ *
+ * @param len
+ * Pointer to variable storing the data length.
+ *
+ * @param pipe
+ * Pointer to the pipe on which the data was received. Can be 0.
+ *
+ * @return
+ * 1: Read OK, more data to read.
+ * 0: Read OK
+ * -1: No RX data
+ * -2: Wrong length read. Something is likely wrong.
+ * -3: Data read, but CRC does not match.
+ */
+int rfhelp_read_rx_data_crc(char *data, int *len, int *pipe) {
+	int res = rfhelp_read_rx_data(data, len, pipe);
+
+	if (res >= 0 && *len > 2) {
+		unsigned short crc = crc16((unsigned char*)data, *len - 2);
+
+		if (crc	!= ((unsigned short) data[*len - 2] << 8 | (unsigned short) data[*len - 1])) {
+			res = -3;
+		}
+	}
+
+	*len -= 2;
+
+	return res;
+}
+
 int rfhelp_rf_status(void) {
 	chMtxLock(&rf_mutex);
 	int s = rf_status();
@@ -212,3 +276,15 @@ void rfhelp_set_rx_addr(int pipe, const char *addr, int addr_len) {
 	rf_set_rx_addr(pipe, addr, address_length);
 	chMtxUnlock();
 }
+
+void rfhelp_power_down(void) {
+	chMtxLock(&rf_mutex);
+	rf_power_down();
+	chMtxUnlock();
+}
+
+void rfhelp_power_up(void) {
+	chMtxLock(&rf_mutex);
+	rf_power_up();
+	chMtxUnlock();
+}
diff --git a/nrf/rfhelp.h b/nrf/rfhelp.h
index e00600790..8e77b788f 100644
--- a/nrf/rfhelp.h
+++ b/nrf/rfhelp.h
@@ -22,9 +22,13 @@
 void rfhelp_init(void);
 void rfhelp_restart(void);
 int rfhelp_send_data(char *data, int len);
+int rfhelp_send_data_crc(char *data, int len);
 int rfhelp_read_rx_data(char *data, int *len, int *pipe);
+int rfhelp_read_rx_data_crc(char *data, int *len, int *pipe);
 int rfhelp_rf_status(void);
 void rfhelp_set_tx_addr(const char *addr, int addr_len);
 void rfhelp_set_rx_addr(int pipe, const char *addr, int addr_len);
+void rfhelp_power_down(void);
+void rfhelp_power_up(void);
 
 #endif /* RFHELP_H_ */
diff --git a/servo.c b/servo.c
index b86948126..c7fdb3e79 100644
--- a/servo.c
+++ b/servo.c
@@ -29,6 +29,8 @@
 #include "ch.h"
 #include "hal.h"
 
+#if SERVO_OUT_ENABLE && !SERVO_OUT_SIMPLE
+
 volatile SERVO servos[SERVOS_NUM];
 
 #if USE_COMMANDS
@@ -526,3 +528,5 @@ static void servo_get_copy(SERVO *a) {
 		a[i].offset = servos[i].offset;
 	}
 }
+
+#endif
diff --git a/servo.h b/servo.h
index 81a0ba100..cddeb72bb 100644
--- a/servo.h
+++ b/servo.h
@@ -47,7 +47,7 @@
 #include "ch.h"
 #include "hal.h"
 #include "hw.h"
-
+#include "conf_general.h"
 #include "stm32f4xx_conf.h"
 
 #ifndef _BV
@@ -85,16 +85,16 @@
  * #define S_COOLDOWN		20000L
  *
  * You can experiment with these to make your servo move further.
- * For mg995 these can be WAY out of spec.
+ * For some servos these can be way out of spec.
  *
  * Note that S_PULSELEN is not accurate at all for low F_CPU. However,
  * it will be rounded up to the nearest possible value (hence the strange
  * calculation below)
  *
  */
-#define S_STARTPULSE	1000L
-#define S_PULSELEN		1000L
-#define S_COOLDOWN		20000L
+#define S_STARTPULSE	SERVO_OUT_PULSE_MIN_US
+#define S_PULSELEN		(SERVO_OUT_PULSE_MAX_US - SERVO_OUT_PULSE_MIN_US)
+#define S_COOLDOWN		(1000000 / SERVO_OUT_RATE_HZ)
 
 /*
  * Dynamic servo parameters
diff --git a/servo_simple.c b/servo_simple.c
new file mode 100644
index 000000000..d7737d32b
--- /dev/null
+++ b/servo_simple.c
@@ -0,0 +1,77 @@
+/*
+	Copyright 2012-2015 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/>.
+    */
+
+/*
+ * servo_simple.c
+ *
+ *  Created on: 31 jul 2015
+ *      Author: benjamin
+ */
+
+#include "servo_simple.h"
+#include "ch.h"
+#include "hal.h"
+#include "hw.h"
+#include "conf_general.h"
+#include "stm32f4xx_conf.h"
+#include "utils.h"
+
+// Settings
+#define TIM_CLOCK			1000000 // Hz
+
+#if SERVO_OUT_ENABLE && SERVO_OUT_SIMPLE
+
+void servo_simple_init(void) {
+	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
+	TIM_OCInitTypeDef  TIM_OCInitStructure;
+
+	palSetPadMode(HW_ICU_GPIO, HW_ICU_PIN, PAL_MODE_ALTERNATE(HW_ICU_GPIO_AF) |
+			PAL_STM32_OSPEED_HIGHEST | PAL_STM32_PUDR_FLOATING);
+
+	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
+
+	TIM_TimeBaseStructure.TIM_Period = (uint16_t)((uint32_t)TIM_CLOCK / (uint32_t)SERVO_OUT_RATE_HZ);
+	TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)((168000000 / 2) / TIM_CLOCK) - 1;
+	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+
+	TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
+
+	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
+	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+	TIM_OCInitStructure.TIM_Pulse = 0;
+	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+
+	TIM_OC2Init(TIM3, &TIM_OCInitStructure);
+	TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
+
+	TIM_ARRPreloadConfig(TIM3, ENABLE);
+
+	servo_simple_set_output(0.5);
+
+	TIM_Cmd(TIM3, ENABLE);
+}
+
+void servo_simple_set_output(float out) {
+	utils_truncate_number(&out, 0.0, 1.0);
+
+	float us = (float)SERVO_OUT_PULSE_MIN_US + out * (float)(SERVO_OUT_PULSE_MAX_US - SERVO_OUT_PULSE_MIN_US);
+	us *= (float)TIM_CLOCK / 1000000.0;
+	TIM3->CCR2 = (uint32_t)us;
+}
+
+#endif
diff --git a/servo_simple.h b/servo_simple.h
new file mode 100644
index 000000000..05466b214
--- /dev/null
+++ b/servo_simple.h
@@ -0,0 +1,32 @@
+/*
+	Copyright 2012-2015 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/>.
+    */
+
+/*
+ * servo_simple.h
+ *
+ *  Created on: 31 jul 2015
+ *      Author: benjamin
+ */
+
+#ifndef SERVO_SIMPLE_H_
+#define SERVO_SIMPLE_H_
+
+// Functions
+void servo_simple_init(void);
+void servo_simple_set_output(float out);
+
+#endif /* SERVO_SIMPLE_H_ */