diff --git a/flight/OpenPilot/System/openpilot.c b/flight/OpenPilot/System/openpilot.c
index be50653f7..3f89f2df6 100644
--- a/flight/OpenPilot/System/openpilot.c
+++ b/flight/OpenPilot/System/openpilot.c
@@ -88,7 +88,14 @@ int main()
 	/* Initialize the system thread */
 	SystemModInitialize();
 
-	/* Start the FreeRTOS scheduler */
+	/* 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)
+	 * LED blinking in case of scheduler returning back should be handled in NMI or other
+	 * appropriate handlers like mem manager.
+	 */
+#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
+	/* Start the FreeRTOS scheduler which never returns.*/
 	vTaskStartScheduler();
 
 	/* If all is well we will never reach here as the scheduler will now be running. */
@@ -100,7 +107,7 @@ int main()
 		PIOS_LED_Toggle(LED2);
 		PIOS_DELAY_WaitmS(100);
 	}
-
+#endif
 	return 0;
 }
 
diff --git a/flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/MemMang/heap_2.c b/flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/MemMang/heap_2.c
index 0f8097561..aec6b41c6 100644
--- a/flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/MemMang/heap_2.c
+++ b/flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/MemMang/heap_2.c
@@ -101,6 +101,7 @@ static xBlockLink xStart, xEnd;
 /* Keeps track of the number of free bytes remaining, but says nothing about
 fragmentation. */
 static size_t xFreeBytesRemaining = configTOTAL_HEAP_SIZE;
+static size_t currentTOTAL_HEAP_SISE = configTOTAL_HEAP_SIZE;
 
 /* STATIC FUNCTIONS ARE DEFINED AS MACROS TO MINIMIZE THE FUNCTION CALL DEPTH. */
 
@@ -140,13 +141,13 @@ xBlockLink *pxFirstFreeBlock;														\
 	xStart.xBlockSize = ( size_t ) 0;												\
 																					\
 	/* xEnd is used to mark the end of the list of free blocks. */					\
-	xEnd.xBlockSize = configTOTAL_HEAP_SIZE;										\
+	xEnd.xBlockSize = currentTOTAL_HEAP_SISE;										\
 	xEnd.pxNextFreeBlock = NULL;													\
 																					\
 	/* To start with there is a single free block that is sized to take up the		\
 	entire heap space. */															\
 	pxFirstFreeBlock = ( void * ) xHeap.ucHeap;										\
-	pxFirstFreeBlock->xBlockSize = configTOTAL_HEAP_SIZE;							\
+	pxFirstFreeBlock->xBlockSize = currentTOTAL_HEAP_SISE;							\
 	pxFirstFreeBlock->pxNextFreeBlock = &xEnd;										\
 }
 /*-----------------------------------------------------------*/
@@ -181,7 +182,7 @@ void *pvReturn = NULL;
 			}
 		}
 
-		if( ( xWantedSize > 0 ) && ( xWantedSize < configTOTAL_HEAP_SIZE ) )
+		if( ( xWantedSize > 0 ) && ( xWantedSize < currentTOTAL_HEAP_SISE ) )
 		{
 			/* Blocks are stored in byte order - traverse the list from the start
 			(smallest) block until one of adequate size is found. */
@@ -276,3 +277,13 @@ void vPortInitialiseBlocks( void )
 {
 	/* This just exists to keep the linker quiet. */
 }
+
+void xPortIncreaseHeapSize( size_t bytes )
+{
+	vTaskSuspendAll();
+	currentTOTAL_HEAP_SISE = configTOTAL_HEAP_SIZE + bytes;
+	xEnd.xBlockSize = currentTOTAL_HEAP_SISE;
+	xFreeBytesRemaining += bytes;
+	xTaskResumeAll();
+}
+/*-----------------------------------------------------------*/
diff --git a/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld b/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld
index 12059ac1e..e3919e295 100644
--- a/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld
+++ b/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld
@@ -1,5 +1,7 @@
 /* This is the size of the stack for early init and for all FreeRTOS IRQs */
 _irq_stack_size = 0x400;
+/* This is the size of the stack for early init: life span is until scheduler starts */
+_init_stack_size = 0x400;
 
 /* Check valid alignment for VTOR */
 ASSERT(ORIGIN(FLASH) == ALIGN(ORIGIN(FLASH), 0x80), "Start of memory region flash not aligned for startup vector table");
@@ -264,11 +266,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);
     } > RAM
     
     PROVIDE ( end = _ebss );
diff --git a/flight/PiOS/STM32F10x/startup_stm32f10x_HD_OP.S b/flight/PiOS/STM32F10x/startup_stm32f10x_HD_OP.S
index 7563e4c0d..a734143d8 100644
--- a/flight/PiOS/STM32F10x/startup_stm32f10x_HD_OP.S
+++ b/flight/PiOS/STM32F10x/startup_stm32f10x_HD_OP.S
@@ -36,6 +36,8 @@
 .global  g_pfnVectors
 .global  SystemInit_ExtMemCtl_Dummy
 .global  Default_Handler
+.global  vTaskStartScheduler
+.global  xPortIncreaseHeapSize
 
 /* start address for the initialization values of the .data section. 
 defined in linker script */
@@ -71,7 +73,6 @@ Reset_Handler:
 /* restore original stack pointer */  
   LDR r0, =_irq_stack_top
   MSR msp, r0
-
   LDR r0, =_init_stack_top
   MSR psp, r0
 /* DO
@@ -94,7 +95,6 @@ LoopFillIRQStack:
   ldr  r3, = _irq_stack_top
   cmp  r2, r3
   bcc  FillIRQStack
-
 /* Copy the data segment initializers from flash to SRAM */  
   movs  r1, #0
   b  LoopCopyDataInit
@@ -124,7 +124,20 @@ LoopFillZerobss:
   bcc  FillZerobss
 /* Call the application's entry point.*/
   bl  main
-  bx  lr    
+/* 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
 
 /**
@@ -296,13 +309,13 @@ g_pfnVectors:
     
   .weak  NMI_Handler
   .thumb_set NMI_Handler,Default_Handler
-  
+
   .weak  HardFault_Handler
   .thumb_set HardFault_Handler,Default_Handler
-  
+
   .weak  MemManage_Handler
   .thumb_set MemManage_Handler,Default_Handler
-  
+
   .weak  BusFault_Handler
   .thumb_set BusFault_Handler,Default_Handler
 
diff --git a/flight/PiOS/STM32F10x/startup_stm32f10x_MD_CC.S b/flight/PiOS/STM32F10x/startup_stm32f10x_MD_CC.S
index f35ca0699..a935c64fc 100644
--- a/flight/PiOS/STM32F10x/startup_stm32f10x_MD_CC.S
+++ b/flight/PiOS/STM32F10x/startup_stm32f10x_MD_CC.S
@@ -168,7 +168,7 @@ Infinite_Loop:
     
     
 g_pfnVectors:
-  .word  _irq_stack_top /*_irq_stack_top */
+  .word  _irq_stack_top
   .word  Reset_Handler
   .word  NMI_Handler
   .word  HardFault_Handler