diff --git a/flight/Bootloaders/OpenPilot/openpilot_bl.c b/flight/Bootloaders/OpenPilot/openpilot_bl.c
index 4158a57b6..45a21186b 100644
--- a/flight/Bootloaders/OpenPilot/openpilot_bl.c
+++ b/flight/Bootloaders/OpenPilot/openpilot_bl.c
@@ -40,10 +40,6 @@ int main()
 	/* Brings up System using CMSIS functions, enables the LEDs. */
 	PIOS_SYS_Init();
 
-	/* Initialise LED's */
-	PIOS_LED_Off(LED1);
-	PIOS_LED_Off(LED2);
-
 	/* Only go into bootloader when the USB cable is connected */
 	if(PIOS_USB_CableConnected()) {
 		/* Delay system */
diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile
index 21f5d5e0a..4aa41656d 100644
--- a/flight/OpenPilot/Makefile
+++ b/flight/OpenPilot/Makefile
@@ -27,7 +27,7 @@
 # Set to YES for debugging
 DEBUG = YES
 ENABLE_DEBUG_PINS = NO
-USE_BOOTLOADER = YES
+USE_BOOTLOADER = NO
 
 # Set to YES when using Code Sourcery toolchain
 CODE_SOURCERY = YES
diff --git a/flight/PiOS/STM32F10x/pios_led.c b/flight/PiOS/STM32F10x/pios_led.c
index 5b0cc9c25..3e3ec9c36 100644
--- a/flight/PiOS/STM32F10x/pios_led.c
+++ b/flight/PiOS/STM32F10x/pios_led.c
@@ -53,6 +53,9 @@ void PIOS_LED_Init(void)
 		RCC_APB2PeriphClockCmd(LED_GPIO_CLK[LEDNum], ENABLE);
 		GPIO_InitStructure.GPIO_Pin = LED_GPIO_PIN[LEDNum];
 		GPIO_Init(LED_GPIO_PORT[LEDNum], &GPIO_InitStructure);
+
+		/* LED's Off */
+		LED_GPIO_PORT[LEDNum]->BSRR = LED_GPIO_PIN[LEDNum];
 	}
 }
 
diff --git a/flight/PiOS/STM32F10x/pios_sys.c b/flight/PiOS/STM32F10x/pios_sys.c
index 9567a8a65..80aa150db 100644
--- a/flight/PiOS/STM32F10x/pios_sys.c
+++ b/flight/PiOS/STM32F10x/pios_sys.c
@@ -79,6 +79,45 @@ void PIOS_SYS_Init(void)
 #endif
 }
 
+/**
+* Shutdown PIOS and reset the microcontroller:<BR>
+* <UL>
+*   <LI>Disable all RTOS tasks
+*   <LI>Disable all interrupts
+*   <LI>Turn off all board LEDs
+*   <LI>Reset STM32
+* </UL>
+* \return < 0 if reset failed
+*/
+int32_t PIOS_SYS_Reset(void)
+{
+	/* Disable all RTOS tasks */
+#if defined(PIOS_INCLUDE_FREERTOS)
+	/* port specific FreeRTOS function to disable tasks (nested) */
+	portENTER_CRITICAL();
+#endif
+
+	// disable all interrupts
+	PIOS_IRQ_Disable();
+
+	// turn off all board LEDs
+	PIOS_LED_Off(LED1);
+	PIOS_LED_Off(LED2);
+
+	/* Reset STM32 */
+	//RCC_APB2PeriphResetCmd(0xfffffff8, ENABLE); /* MBHP_CORE_STM32: don't reset GPIOA/AF due to USB pins */
+	//RCC_APB1PeriphResetCmd(0xff7fffff, ENABLE); /* don't reset USB, so that the connection can survive! */
+
+	RCC_APB2PeriphResetCmd(0xffffffff, DISABLE);
+	RCC_APB1PeriphResetCmd(0xffffffff, DISABLE);
+	SCB->AIRCR = NVIC_AIRCR_VECTKEY | (1 << NVIC_VECTRESET);
+
+	while(1);
+
+	/* We will never reach this point */
+	return -1;
+}
+
 /**
 * Returns the serial number as a string
 * param[out] str pointer to a string which can store at least 32 digits + zero terminator!
diff --git a/flight/PiOS/inc/pios_sys.h b/flight/PiOS/inc/pios_sys.h
index 683c5173c..4e71b54a0 100644
--- a/flight/PiOS/inc/pios_sys.h
+++ b/flight/PiOS/inc/pios_sys.h
@@ -29,6 +29,7 @@
 
 /* Public Functions */
 extern void PIOS_SYS_Init(void);
+extern int32_t PIOS_SYS_Reset(void);
 extern int32_t PIOS_SYS_SerialNumberGet(char *str);
 
 #endif /* PIOS_SYS_H */