diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c
index 4bbad7c6b..07b9a5733 100644
--- a/flight/Modules/ManualControl/manualcontrol.c
+++ b/flight/Modules/ManualControl/manualcontrol.c
@@ -73,39 +73,39 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr
 static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
 
 #define assumptions1 ( \
-		(MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE 		== MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE) 		&& \
-		(MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE 		== MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE) 		&& \
-		(MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE 	== MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE) 	   \
+		((int)MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE 		== (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE) 		&& \
+		((int)MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE 		== (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE) 		&& \
+		((int)MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE 	== (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE) 	   \
 		)
 
 #define assumptions2 ( \
-		(MANUALCONTROLSETTINGS_POS1FLIGHTMODE_MANUAL 			== MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) 						&& \
-		(MANUALCONTROLSETTINGS_POS1FLIGHTMODE_STABILIZED		== MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) 					&& \
-		(MANUALCONTROLSETTINGS_POS1FLIGHTMODE_AUTO 				== MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) 						   \
+		((int)MANUALCONTROLSETTINGS_POS1FLIGHTMODE_MANUAL 			== (int)MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) 						&& \
+		((int)MANUALCONTROLSETTINGS_POS1FLIGHTMODE_STABILIZED		== (int)MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) 					&& \
+		((int)MANUALCONTROLSETTINGS_POS1FLIGHTMODE_AUTO 				== (int)MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) 						   \
 		)
 
 #define assumptions3 ( \
-		(MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_NONE 		== MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE) 		&& \
-		(MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_RATE 		== MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE) 		&& \
-		(MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_ATTITUDE 	== MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE) 	   \
+		((int)MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_NONE 		== (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE) 		&& \
+		((int)MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_RATE 		== (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE) 		&& \
+		((int)MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_ATTITUDE 	== (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE) 	   \
 		)
 
 #define assumptions4 ( \
-		(MANUALCONTROLSETTINGS_POS2FLIGHTMODE_MANUAL 			== MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) 						&& \
-		(MANUALCONTROLSETTINGS_POS2FLIGHTMODE_STABILIZED		== MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) 					&& \
-		(MANUALCONTROLSETTINGS_POS2FLIGHTMODE_AUTO 				== MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) 						   \
+		((int)MANUALCONTROLSETTINGS_POS2FLIGHTMODE_MANUAL 			== (int)MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) 						&& \
+		((int)MANUALCONTROLSETTINGS_POS2FLIGHTMODE_STABILIZED		== (int)MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) 					&& \
+		((int)MANUALCONTROLSETTINGS_POS2FLIGHTMODE_AUTO 				== (int)MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) 						   \
 		)
 
 #define assumptions5 ( \
-		(MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_NONE 		== MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE) 		&& \
-		(MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_RATE 		== MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE) 		&& \
-		(MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_ATTITUDE 	== MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE) 	   \
+		((int)MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_NONE 		== (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE) 		&& \
+		((int)MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_RATE 		== (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE) 		&& \
+		((int)MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_ATTITUDE 	== (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE) 	   \
 		)
 
 #define assumptions6 ( \
-		(MANUALCONTROLSETTINGS_POS3FLIGHTMODE_MANUAL 			== MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) 						&& \
-		(MANUALCONTROLSETTINGS_POS3FLIGHTMODE_STABILIZED		== MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) 					&& \
-		(MANUALCONTROLSETTINGS_POS3FLIGHTMODE_AUTO 				== MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) 						   \
+		((int)MANUALCONTROLSETTINGS_POS3FLIGHTMODE_MANUAL 			== (int)MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) 						&& \
+		((int)MANUALCONTROLSETTINGS_POS3FLIGHTMODE_STABILIZED		== (int)MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) 					&& \
+		((int)MANUALCONTROLSETTINGS_POS3FLIGHTMODE_AUTO 				== (int)MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) 						   \
 		)
 
 
diff --git a/flight/OpenPilot/Makefile.win32 b/flight/OpenPilot/Makefile.win32
index 68d882a60..ced16eea5 100644
--- a/flight/OpenPilot/Makefile.win32
+++ b/flight/OpenPilot/Makefile.win32
@@ -53,7 +53,7 @@ FLASH_TOOL = OPENOCD
 USE_THUMB_MODE = YES
 
 # List of modules to include
-MODULES =  Telemetry Actuator Stabilization Guidance ManualControl# FlightPlan
+MODULES =  Telemetry Actuator Stabilization Guidance ManualControl FlightPlan
 #MODULES =  Telemetry GPS ManualControl Actuator Altitude Attitude Stabilization
 #MODULES = Telemetry Example
 #MODULES = Telemetry MK/MKSerial
@@ -206,12 +206,12 @@ SRC += $(RTOSSRCDIR)/portable/GCC/Win32/port.c
 SRC += $(RTOSSRCDIR)/portable/MemMang/heap_3.c
 
 ## PyMite files
-#SRC += $(wildcard ${PYMITEVM}/*.c)
-#SRC += $(wildcard ${PYMITEPLAT}/*.c)
-#SRC += $(OUTDIR)/pmlib_img.c
-#SRC += $(OUTDIR)/pmlib_nat.c
-#SRC += $(OUTDIR)/pmlibusr_img.c
-#SRC += $(OUTDIR)/pmlibusr_nat.c
+SRC += $(wildcard ${PYMITEVM}/*.c)
+SRC += $(wildcard ${PYMITEPLAT}/*.c)
+SRC += $(OUTDIR)/pmlib_img.c
+SRC += $(OUTDIR)/pmlib_nat.c
+SRC += $(OUTDIR)/pmlibusr_img.c
+SRC += $(OUTDIR)/pmlibusr_nat.c
 
 
 
@@ -434,7 +434,7 @@ exe: $(OUTDIR)/$(TARGET).exe
 #all: begin gccversion sizebefore build sizeafter finished end
 #all: begin gccversion build sizeafter finished end
 #all: elf
-all: exe
+all: gencode exe
 
 ifeq ($(LOADFORMAT),ihex)
 build: elf hex lss sym
@@ -458,6 +458,9 @@ else
 	quote = 
 endif
 
+# Generate intermediate code
+gencode: ${OUTDIR}/InitMods.c $(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h
+
 ${OUTDIR}/InitMods.c: Makefile.win32
 	@echo ${MSG_MODINIT}
 	@echo ${quote}// Autogenerated file${quote} > ${OUTDIR}/InitMods.c 
@@ -466,12 +469,12 @@ ${OUTDIR}/InitMods.c: Makefile.win32
 	@echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Initialize();}${quote}  >> ${OUTDIR}/InitMods.c
 	@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
 	
-# Generate code for PyMite
-#$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h $(OPMODULEDIR)/FlightPlan/flightplan.c: $(wildcard $(PYMITELIB)/*.py) $(wildcard $(PYMITEPLAT)/*.py) $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(FLIGHTPLANS)/*.py) 
-#	@echo ${MSG_PYMITEINIT}
-#	@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py)
-#	@$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h
-#	@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py
+#Generate code for PyMite
+$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h $(OPMODULEDIR)/FlightPlan/flightplan.c: $(wildcard $(PYMITELIB)/*.py) $(wildcard $(PYMITEPLAT)/*.py) $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(FLIGHTPLANS)/*.py) 
+	@echo ${MSG_PYMITEINIT}
+	@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py)
+	@$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h
+	@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py
 	
 # Eye candy.
 begin:
@@ -664,6 +667,6 @@ endif
 
 
 # Listing of phony targets.
-.PHONY : all begin finish end sizebefore sizeafter gccversion \
+.PHONY : all begin gencode finish end sizebefore sizeafter gccversion \
 build exe elf hex bin lss sym clean clean_list program
 
diff --git a/flight/PiOS.win32/inc/FreeRTOSConfig.h b/flight/PiOS.win32/inc/FreeRTOSConfig.h
index 21fbc6b35..8b8b51fed 100644
--- a/flight/PiOS.win32/inc/FreeRTOSConfig.h
+++ b/flight/PiOS.win32/inc/FreeRTOSConfig.h
@@ -48,7 +48,7 @@ to exclude the API function. */
 
 #define INCLUDE_vTaskPrioritySet		1
 #define INCLUDE_uxTaskPriorityGet		1
-#define INCLUDE_vTaskDelete			0
+#define INCLUDE_vTaskDelete			1
 #define INCLUDE_vTaskCleanUpResources		0
 #define INCLUDE_vTaskSuspend			1
 #define INCLUDE_vTaskDelayUntil			1
diff --git a/flight/PiOS.win32/win32/Libraries/FreeRTOS/Source/portable/GCC/Win32/port.c b/flight/PiOS.win32/win32/Libraries/FreeRTOS/Source/portable/GCC/Win32/port.c
index 88170822a..787826afa 100644
--- a/flight/PiOS.win32/win32/Libraries/FreeRTOS/Source/portable/GCC/Win32/port.c
+++ b/flight/PiOS.win32/win32/Libraries/FreeRTOS/Source/portable/GCC/Win32/port.c
@@ -196,13 +196,16 @@ static volatile portSTACK_TYPE dwEnabledIsr = NMI;	// mask of enabled ISRs (indi
 static HANDLE hTickAck;		// acknolwledge tick interrupt
 static HANDLE hTermAck;		// acknowledge termination
 
+volatile SSIM_T *taskToDelete;
+
 BOOL bIsWow64;
 BOOL bUsingMMCSS;
 
 static enum
 {
 	SWI_ID_YIELD,
-	SWI_ID_TERMINATE
+	SWI_ID_TERMINATE,
+	SWI_ID_ENDTASK
 }
 ESWI_ID;
 
@@ -638,7 +641,7 @@ portSTACK_TYPE *pxPortInitialiseStack( portSTACK_TYPE *pxTopOfStack, pdTASK_CODE
 
 portBASE_TYPE xPortStartScheduler( void )
 {
-	BOOL bSwitch;
+	BOOL bSwitch, bTaskDelete;
 	SSIM_T *psSim;
 	DWORD dwIntr;
 	int i, iIsrCount;
@@ -675,6 +678,7 @@ portBASE_TYPE xPortStartScheduler( void )
 
 		bSwitch = (dwIntr != 0);	// switch only for a real interrupt
 		iIsrCount = 0;			// no suspensions after handling first int
+		bTaskDelete = FALSE;
 
 		for(i=0; dwIntr && i<CPU_INTR_COUNT; i++)
 		{
@@ -693,6 +697,11 @@ portBASE_TYPE xPortStartScheduler( void )
 						ReleaseMutex(hIsrMutex);
 						return 0;
 					}
+					else if(ESWI_ID == SWI_ID_ENDTASK)
+					{
+						TerminateThread(taskToDelete->hThread, 0);
+						CloseHandle(taskToDelete->hSemaphore);
+					}
 
 					psSim->yielded = TRUE;
 					iIsrCount++;
@@ -805,6 +814,23 @@ void __swi(void)
 	}
 }
 
+void __delete_task(void *tcb)
+{
+	taskToDelete = *((SSIM_T **)tcb);
+
+	if(hIsrDispatcher)
+	{
+		WaitForSingleObject(hIsrMutex, INFINITE);
+		dwPendingIsr |= (1<<CPU_INTR_SWI);
+		ESWI_ID = SWI_ID_ENDTASK;
+		SetEvent(hIsrInvoke);
+		ReleaseMutex(hIsrMutex);
+
+		if(pxCurrentTCB == tcb)
+			Sleep(100000); //This task is suicidal, wait until it dies
+	}
+}
+
 void __generate_interrupt(int iNo)
 {
 	if(hIsrDispatcher)
diff --git a/flight/PiOS.win32/win32/Libraries/FreeRTOS/Source/portable/GCC/Win32/portmacro.h b/flight/PiOS.win32/win32/Libraries/FreeRTOS/Source/portable/GCC/Win32/portmacro.h
index 396c5dc81..f9b6441fb 100644
--- a/flight/PiOS.win32/win32/Libraries/FreeRTOS/Source/portable/GCC/Win32/portmacro.h
+++ b/flight/PiOS.win32/win32/Libraries/FreeRTOS/Source/portable/GCC/Win32/portmacro.h
@@ -126,8 +126,7 @@ extern void vTaskSwitchContext( void );				\
 
 #define portOUTPUT_BYTE( a, b )
 
-extern void vPortForciblyEndThread( void *pxTaskToDelete );
-#define traceTASK_DELETE( pxTaskToDelete )		vPortForciblyEndThread( pxTaskToDelete )
+#define traceTASK_DELETE( pxTaskToDelete )		__delete_task( pxTaskToDelete )
 
 #define traceTASK_CREATE( pxNewTCB )
 
@@ -158,6 +157,9 @@ void __enable_interrupt(void);
 /* generate software interrupt - non - maskable */
 void __swi(void);
 
+/* generate an interrupt which causes the scheduler to delete the task */
+void __delete_task(void *tcb);
+
 /* To be used by interrupt generation threads. e.g. Create a thread that
 monitors console events and generate an interrupt whenever a key is pressed.
 See also implimentation of tick_generator.