diff --git a/flight/CopterControl/System/coptercontrol.c b/flight/CopterControl/System/coptercontrol.c
index 060fb3646..e55cf16f9 100644
--- a/flight/CopterControl/System/coptercontrol.c
+++ b/flight/CopterControl/System/coptercontrol.c
@@ -53,7 +53,7 @@ extern void PIOS_Board_Init(void);
 *
 * Initialize PiOS<BR>
 * Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR>
-* Start FreeRTOS Scheduler (vTaskStartScheduler)<BR>
+* Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller)
 * If something goes wrong, blink LED1 and LED2 every 100ms
 *
 */
@@ -67,12 +67,15 @@ int main()
 
 	/* Initialize the system thread */
 	SystemModInitialize();
-	
-	/* Start the FreeRTOS scheduler */
-	vTaskStartScheduler();
 
-	/* If all is well we will never reach here as the scheduler will now be running. */
-	/* If we do get here, it will most likely be because we ran out of heap space. */
+#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
+	/* Start the FreeRTOS scheduler which never returns.*/
+	/* only do this for posix and win32 since the caller will take care
+	 * of starting the scheduler and increase the heap and swith back to
+	 * MSP stack. (all arch specific is hidden from here and take care by reset handler)
+	 */
+    vTaskStartScheduler();
+#endif
 
 	return 0;
 }
diff --git a/flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/MemMang/heap_1.c b/flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/MemMang/heap_1.c
index a8128f314..638dc220a 100644
--- a/flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/MemMang/heap_1.c
+++ b/flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/MemMang/heap_1.c
@@ -84,6 +84,8 @@ static union xRTOS_HEAP
 } xHeap __attribute__ ((section (".heap")));
 
 static size_t xNextFreeByte = ( size_t ) 0;
+static size_t currentTOTAL_HEAP_SISE = configTOTAL_HEAP_SIZE;
+
 /*-----------------------------------------------------------*/
 
 void *pvPortMalloc( size_t xWantedSize )
@@ -102,7 +104,7 @@ void *pvReturn = NULL;
 	vTaskSuspendAll();
 	{
 		/* Check there is enough room left for the allocation. */
-		if( ( ( xNextFreeByte + xWantedSize ) < configTOTAL_HEAP_SIZE ) &&
+		if( ( ( xNextFreeByte + xWantedSize ) < currentTOTAL_HEAP_SISE ) &&
 			( ( xNextFreeByte + xWantedSize ) > xNextFreeByte )	)/* Check for overflow. */
 		{
 			/* Return the next free byte then increment the index past this
@@ -145,8 +147,14 @@ void vPortInitialiseBlocks( void )
 
 size_t xPortGetFreeHeapSize( void )
 {
-	return ( configTOTAL_HEAP_SIZE - xNextFreeByte );
+	return ( currentTOTAL_HEAP_SISE - xNextFreeByte );
 }
+/*-----------------------------------------------------------*/
 
-
-
+void xPortIncreaseHeapSize( size_t bytes )
+{
+	vTaskSuspendAll();
+	currentTOTAL_HEAP_SISE = configTOTAL_HEAP_SIZE + bytes;
+	xTaskResumeAll();
+}
+/*-----------------------------------------------------------*/
diff --git a/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld b/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld
index d9e08c336..48e9cc112 100644
--- a/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld
+++ b/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld
@@ -1,5 +1,7 @@
-/* This is the size of the stack for early init and for all FreeRTOS IRQs */
+/* This is the size of the stack for all FreeRTOS IRQs */
 _irq_stack_size = 0x180;
+/* This is the size of the stack for early init: life span is until scheduler starts */
+_init_stack_size = 0x80;
 
 /* Stub out these functions since we don't use them anyway */
 PROVIDE (   vPortSVCHandler = 0 ) ;
@@ -86,11 +88,17 @@ SECTIONS
     .heap (NOLOAD) :
     {
         _sheap = . ;
+        _sheap_pre_rtos = . ;
         *(.heap)
         . = ALIGN(4);
         _eheap = . ;
+        _eheap_pre_rtos = . ;
+        _init_stack_end = . ; 
+        _sheap_post_rtos = . ;
+        . = . + _init_stack_size ;
+        . = ALIGN(4);
+        _eheap_post_rtos = . ;
         _init_stack_top = . - 4 ;
-         . = ALIGN(4);
     } > SRAM
 
     . = ALIGN(4);
diff --git a/flight/PiOS/STM32F10x/startup_stm32f10x_MD_CC.S b/flight/PiOS/STM32F10x/startup_stm32f10x_MD_CC.S
index 11e865ff8..f35ca0699 100644
--- a/flight/PiOS/STM32F10x/startup_stm32f10x_MD_CC.S
+++ b/flight/PiOS/STM32F10x/startup_stm32f10x_MD_CC.S
@@ -33,6 +33,8 @@
 
 .global  g_pfnVectors
 .global  Default_Handler
+.global  vTaskStartScheduler
+.global  xPortIncreaseHeapSize
 
 /* start address for the initialization values of the .data section. 
 defined in linker script */
@@ -124,6 +126,19 @@ LoopFillZerobss:
   bcc  FillZerobss
 /* Call the application's entry point.*/
   bl  main
+/* Switches stack back momentarily to MSP */
+  add r0, #0
+  msr control, r0
+/* add heap_post_rtos to the heap (if the capability/function exist) */
+  ldr r0, = _init_stack_size
+  bl xPortIncreaseHeapSize
+/* Start the FreeRTOS scheduler which never returns.*/
+  bl vTaskStartScheduler
+/* If all is well we will never reach here as the scheduler will now be running. */
+/* If we do get here, it will most likely be because we ran out of heap space. */
+/* let the Memory manager handler figure it out (at least for now, it will make it more obvious */
+  bl  MemManage_Handler
+/* will never return here for now until mem manager give up */
   bx  lr
 .size  Reset_Handler, .-Reset_Handler