diff --git a/flight/OpenPilot/System/inc/pios_board.h b/flight/OpenPilot/System/inc/pios_board.h
index d5c7eff8b..e787b1ee5 100644
--- a/flight/OpenPilot/System/inc/pios_board.h
+++ b/flight/OpenPilot/System/inc/pios_board.h
@@ -28,6 +28,23 @@
 #define PIOS_BOARD_H
 
 
+//------------------------
+// Timers and Channels Used
+//------------------------
+/*
+Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
+------+-----------+-----------+-----------+----------
+TIM1  | RC In 3   | RC In 6   | RC In 5   |
+TIM2  | --------------- PIOS_DELAY -----------------
+TIM3  | RC In 7   | RC In 8   | RC In 1   | RC In 2
+TIM4  | Servo 1   | Servo 2   | Servo 3   | Servo 4
+TIM5  | RC In 4   |           |           |
+TIM6  | ----------- PIOS_PWM (Supervisor) ----------
+TIM7  |           |           |           |
+TIM8  | Servo 5   | Servo 6   | Servo 7   | Servo 8
+------+-----------+-----------+-----------+----------
+*/
+
 //------------------------
 // DMA Channels Used
 //------------------------
@@ -181,7 +198,7 @@
 // Delay Timer
 //-------------------------
 #define PIOS_DELAY_TIMER			TIM2
-#define PIOS_DELAY_TIMER_RCC			RCC_APB1Periph_TIM2
+#define PIOS_DELAY_TIMER_RCC_FUNC		RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE)
 
 //-------------------------
 // Master Clock
@@ -232,7 +249,13 @@
 #define RECEIVER8_PIN				GPIO_Pin_5      // PB5
 #define RECEIVER8_TIM_PORT			TIM3
 #define RECEIVER8_CH				TIM_Channel_2   // TIM3_CH2
-#define NUM_RECEIVER_INPUTS			8
+#define PIOS_PWM_NUM_INPUTS			1
+#define PIOS_PWM_SUPV_ENABLED			1
+#define PIOS_PWM_SUPV_TIMER			TIM6
+#define PIOS_PWM_SUPV_TIMER_RCC_FUNC		RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE)
+#define PIOS_PWM_SUPV_HZ			25
+#define PIOS_PWM_SUPV_IRQ_CHANNEL		TIM6_IRQn
+#define PIOS_PWM_SUPV_IRQ_FUNC			void TIM6_IRQHandler(void)
 
 //-------------------------
 // Servo outputs   
diff --git a/flight/OpenPilot/System/openpilot.c b/flight/OpenPilot/System/openpilot.c
index 2d4e70a7e..e4b5a5ac8 100644
--- a/flight/OpenPilot/System/openpilot.c
+++ b/flight/OpenPilot/System/openpilot.c
@@ -106,7 +106,7 @@ int main()
 	/* Create a FreeRTOS task */
 	xTaskCreate(TaskTick, (signed portCHAR *)"Test", configMINIMAL_STACK_SIZE , NULL, 1, NULL);
 	xTaskCreate(TaskTesting, (signed portCHAR *)"TaskTesting", configMINIMAL_STACK_SIZE , NULL, 4, NULL);
-	//xTaskCreate(TaskServos, (signed portCHAR *)"Servos", configMINIMAL_STACK_SIZE , NULL, 4, NULL);
+	xTaskCreate(TaskServos, (signed portCHAR *)"Servos", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
 	//xTaskCreate(TaskSDCard, (signed portCHAR *)"SDCard", configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 2), NULL);
 
 	/* Start the FreeRTOS scheduler */
@@ -142,7 +142,7 @@ static void TaskTick(void *pvParameters)
 
 static void TaskTesting(void *pvParameters)
 {
-	portTickType xDelay = 500 / portTICK_RATE_MS;
+	portTickType xDelay = 1000 / portTICK_RATE_MS;
 	portTickType xTimeout = 10 / portTICK_RATE_MS;
 
 	for(;;)
@@ -160,7 +160,7 @@ static void TaskTesting(void *pvParameters)
 		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u\r", PIOS_BMP085_GetPressure());
 		*/
 
-		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u uS\r", PIOS_PWM_Get());
+		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u uS\r", PIOS_PWM_Get(0));
 
 		/* This blocks the task until there is something on the buffer */
 		/*xSemaphoreTake(PIOS_USART1_Buffer, portMAX_DELAY);
diff --git a/flight/PiOS/STM32F10x/pios_delay.c b/flight/PiOS/STM32F10x/pios_delay.c
index 4619091ad..6c140349c 100644
--- a/flight/PiOS/STM32F10x/pios_delay.c
+++ b/flight/PiOS/STM32F10x/pios_delay.c
@@ -44,11 +44,7 @@
 int32_t PIOS_DELAY_Init(void)
 {
 	/* Enable timer clock */
-	if(PIOS_DELAY_TIMER_RCC == RCC_APB2Periph_TIM1 || PIOS_DELAY_TIMER_RCC == RCC_APB2Periph_TIM8) {
-	    RCC_APB2PeriphClockCmd(PIOS_DELAY_TIMER_RCC, ENABLE);
-	} else {
-		RCC_APB1PeriphClockCmd(PIOS_DELAY_TIMER_RCC, ENABLE);
-	}
+	PIOS_DELAY_TIMER_RCC_FUNC;
 
 	/* Time base configuration */
 	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
diff --git a/flight/PiOS/STM32F10x/pios_pwm.c b/flight/PiOS/STM32F10x/pios_pwm.c
index c4e0615e4..4cc849796 100644
--- a/flight/PiOS/STM32F10x/pios_pwm.c
+++ b/flight/PiOS/STM32F10x/pios_pwm.c
@@ -32,20 +32,40 @@
 #if !defined(PIOS_DONT_USE_PWM)
 
 /* Local Variables */
-static volatile uint16_t ic3_readvalue1 = 0, ic3_readvalue2 = 0;
-static volatile uint16_t capture_number = 0;
-static volatile uint32_t CAPTURE = 0;
-static volatile uint32_t TIM3_FREQ = 0;
+static uint8_t CaptureState[PIOS_PWM_NUM_INPUTS];
+static uint16_t RiseValue[PIOS_PWM_NUM_INPUTS];
+static uint16_t FallValue[PIOS_PWM_NUM_INPUTS];
+static uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS];
+
+static uint8_t SupervisorState = 0;
+static uint32_t CapCounter[PIOS_PWM_NUM_INPUTS];
+static uint32_t CapCounterPrev[PIOS_PWM_NUM_INPUTS];
+
 
 /**
 * Initialises all the LED's
 */
 void PIOS_PWM_Init(void)
 {
+	/* Flush counter variables */
+	int32_t i;
+	for(i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
+		CaptureState[i] = 0;
+	}
+	for(i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
+		RiseValue[i] = 0;
+	}
+	for(i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
+		FallValue[i] = 0;
+	}
+	for(i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
+		CaptureValue[i] = 0;
+	}
+
 	/* Setup RCC */
 	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
 
-	/* Enable the TIM3 global Interrupt */
+	/* Enable timer interrupts */
 	NVIC_InitTypeDef NVIC_InitStructure;
 	NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
 	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
@@ -53,8 +73,10 @@ void PIOS_PWM_Init(void)
 	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
 	NVIC_Init(&NVIC_InitStructure);
 
+	/* Partial pin remap for PB5 */
 	GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3, ENABLE);
 
+	/* Configure input pins */
 	GPIO_InitTypeDef GPIO_InitStructure;
 	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
 	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
@@ -62,8 +84,8 @@ void PIOS_PWM_Init(void)
 	GPIO_InitStructure.GPIO_Pin = RECEIVER8_PIN;
 	GPIO_Init(RECEIVER8_GPIO_PORT, &GPIO_InitStructure);
 
+	/* Configure timer for input capture */
 	TIM_ICInitTypeDef TIM_ICInitStructure;
-	TIM_ICInitStructure.TIM_Channel = RECEIVER8_CH;
 	TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
 	TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
 	TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
@@ -72,20 +94,75 @@ void PIOS_PWM_Init(void)
 	TIM_ICInitStructure.TIM_Channel = RECEIVER8_CH;
 	TIM_ICInit(RECEIVER8_TIM_PORT, &TIM_ICInitStructure);
 
+	/* Configure timer clocks */
 	TIM_InternalClockConfig(RECEIVER8_TIM_PORT);
 	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
 	TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
 	TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
-	TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1;//17; // fCK_PSC / (17 + 1) 1ms = 4000
+	TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1;
 	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
 	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
 	TIM_TimeBaseInit(RECEIVER8_TIM_PORT, &TIM_TimeBaseStructure);
 
-	/* TIM enable counter */
-	TIM_Cmd(RECEIVER8_TIM_PORT, ENABLE);
-
 	/* Enable the CC2 Interrupt Request */
 	TIM_ITConfig(RECEIVER8_TIM_PORT, TIM_IT_CC2, ENABLE);
+
+	/* Enable timers */
+	TIM_Cmd(RECEIVER8_TIM_PORT, ENABLE);
+
+
+	/* Supervisor Setup */
+#if (PIOS_PWM_SUPV_ENABLED)
+	/* Flush counter variables */
+	for(i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
+		CapCounter[i] = 0;
+	}
+	for(i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
+		CapCounterPrev[i] = 0;
+	}
+
+	/* Enable timer clock */
+	PIOS_PWM_SUPV_TIMER_RCC_FUNC;
+
+	/* Configure interrupts */
+	NVIC_InitStructure.NVIC_IRQChannel = PIOS_PWM_SUPV_IRQ_CHANNEL;
+	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+	NVIC_Init(&NVIC_InitStructure);
+
+	/* Time base configuration */
+	TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
+	TIM_TimeBaseStructure.TIM_Period = ((1000000 / PIOS_PWM_SUPV_HZ) - 1);
+	TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1; /* For 1 uS accuracy */
+	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
+	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+	TIM_TimeBaseInit(PIOS_PWM_SUPV_TIMER, &TIM_TimeBaseStructure);
+
+	/* Enable the CC2 Interrupt Request */
+	TIM_ITConfig(PIOS_PWM_SUPV_TIMER, TIM_IT_Update, ENABLE);
+
+	/* Clear update pending flag */
+	TIM_ClearFlag(TIM2, TIM_FLAG_Update);
+
+	/* Enable counter */
+	TIM_Cmd(PIOS_PWM_SUPV_TIMER, ENABLE);
+#endif
+}
+
+/**
+* Get the value of an input channel
+* \param[in] Channel Number of the channel desired
+* \output -1 Channel not available
+* \output >0 Channel value
+*/
+int32_t PIOS_PWM_Get(int8_t Channel)
+{
+	/* Return error if channel not available */
+	if(Channel >= PIOS_PWM_NUM_INPUTS) {
+		return -1;
+	}
+	return CaptureValue[Channel];
 }
 
 /**
@@ -97,49 +174,82 @@ void TIM3_IRQHandler(void)
 		/* Clear TIM3 Capture compare interrupt pending bit */
 		TIM_ClearITPendingBit(RECEIVER8_TIM_PORT, TIM_IT_CC2);
 
-		if(capture_number == 0) {
+		/* Simple rise or fall state machine */
+		if(CaptureState[0] == 0) {
 			/* Get the Input Capture value */
-			ic3_readvalue1 = TIM_GetCapture2(RECEIVER8_TIM_PORT);
-			capture_number = 1;
+			RiseValue[0] = TIM_GetCapture2(RECEIVER8_TIM_PORT);
 
+			/* Switch states */
+			CaptureState[0] = 1;
+
+			/* Switch polarity of input capture */
 			TIM_ICInitTypeDef TIM_ICInitStructure;
-			TIM_ICInitStructure.TIM_Channel = RECEIVER8_CH;
+			TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
+			TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
+			TIM_ICInitStructure.TIM_ICFilter = 0x0;
+
 			TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
-			TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
-			TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
-			TIM_ICInitStructure.TIM_ICFilter = 0x0;
 			TIM_ICInitStructure.TIM_Channel = RECEIVER8_CH;
 			TIM_ICInit(RECEIVER8_TIM_PORT, &TIM_ICInitStructure);
 
-		} else if(capture_number == 1) {
+		} else {
 			/* Get the Input Capture value */
-			ic3_readvalue2 = TIM_GetCapture2(RECEIVER8_TIM_PORT);
-
-			TIM_ICInitTypeDef TIM_ICInitStructure;
-			TIM_ICInitStructure.TIM_Channel = RECEIVER8_CH;
-			TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
-			TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
-			TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
-			TIM_ICInitStructure.TIM_ICFilter = 0x0;
-			TIM_ICInitStructure.TIM_Channel = RECEIVER8_CH;
-			TIM_ICInit(RECEIVER8_TIM_PORT, &TIM_ICInitStructure);
+			FallValue[0] = TIM_GetCapture2(RECEIVER8_TIM_PORT);
 
 			/* Capture computation */
-			if (ic3_readvalue2 > ic3_readvalue1) {
-				CAPTURE = (ic3_readvalue2 - ic3_readvalue1);
+			if (FallValue[0] > RiseValue[0]) {
+				CaptureValue[0] = (FallValue[0] - RiseValue[0]);
 			} else {
-				CAPTURE = ((0xFFFF - ic3_readvalue1) + ic3_readvalue2);
+				CaptureValue[0] = ((0xFFFF - RiseValue[0]) + FallValue[0]);
 			}
 
-			capture_number = 0;
+			/* Switch states */
+			CaptureState[0] = 0;
 
+			/* Increase supervisor counter */
+			CapCounter[0]++;
+
+			/* Switch polarity of input capture */
+			TIM_ICInitTypeDef TIM_ICInitStructure;
+			TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
+			TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
+			TIM_ICInitStructure.TIM_ICFilter = 0x0;
+
+			TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
+			TIM_ICInitStructure.TIM_Channel = RECEIVER8_CH;
+			TIM_ICInit(RECEIVER8_TIM_PORT, &TIM_ICInitStructure);
 		}
 	}
 }
 
-uint32_t PIOS_PWM_Get(void)
+/**
+* This function handles TIM3 global interrupt request.
+*/
+PIOS_PWM_SUPV_IRQ_FUNC
 {
-	return CAPTURE;
+	/* Clear timer interrupt pending bit */
+	TIM_ClearITPendingBit(PIOS_PWM_SUPV_TIMER, TIM_IT_Update);
+
+	/* Simple state machine */
+	if(SupervisorState == 0) {
+		/* Save this states values */
+		for(int32_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
+			CapCounterPrev[i] = CapCounter[i];
+		}
+
+		/* Move to next state */
+		SupervisorState = 1;
+	} else {
+		/* See what channels have been updated */
+		for(int32_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
+			if(CapCounter[i] == CapCounterPrev[i]) {
+				CaptureValue[i] = 0;
+			}
+		}
+
+		/* Move to next state */
+		SupervisorState = 0;
+	}
 }
 
 #endif
diff --git a/flight/PiOS/inc/pios_pwm.h b/flight/PiOS/inc/pios_pwm.h
index bacaee426..1c0c5da82 100644
--- a/flight/PiOS/inc/pios_pwm.h
+++ b/flight/PiOS/inc/pios_pwm.h
@@ -28,6 +28,6 @@
 
 /* Public Functions */
 extern void PIOS_PWM_Init(void);
-extern uint32_t PIOS_PWM_Get(void);
+extern int32_t PIOS_PWM_Get(int8_t Channel);
 
 #endif /* PIOS_PWM_H */