diff --git a/embedded/common/app/algorithm_t.c b/embedded/common/app/algorithm_t.c
index eb82366..d10a917 100644
--- a/embedded/common/app/algorithm_t.c
+++ b/embedded/common/app/algorithm_t.c
@@ -26,6 +26,8 @@
 /*--------------------------------------------------------------------*\
  |    E X T E R N A L   V A R I A B L E S   &   F U N C T I O N S
 \*--------------------------------------------------------------------*/
+osMutexDef(mutexCritSection);
+osMutexId mutex_id;
 
 /*--------------------------------------------------------------------*\
  |    P U B L I C   V A R I A B L E S   D E F I N I T I O N S
@@ -247,8 +249,6 @@ static  const OSP_Library_Version_t* version;
 static ResultHandle_t _outSensorHandles[NUM_ANDROID_SENSOR_TYPE];   // Android Sensors
 static ResultHandle_t _outPSensorHandles[NUM_PRIVATE_SENSOR_TYPE];  // Private Sensors
 
-static OS_MUT mutexCritSection;
-
 /*--------------------------------------------------------------------*\
  |    F O R W A R D   F U N C T I O N   D E C L A R A T I O N S
 \*--------------------------------------------------------------------*/
@@ -277,12 +277,12 @@ SystemDescriptor_t gSystemDesc =
  **********************************************************************/
 __inline void EnterCriticalSection(void)
 {
-    os_mut_wait( mutexCritSection, OS_WAIT_FOREVER );
+    osMutexWait(mutex_id,osWaitForever);
 }
 
 __inline void ExitCriticalSection(void)
 {
-    os_mut_release( mutexCritSection );
+    osMutexRelease(mutex_id);
 }
 
 
@@ -825,6 +825,9 @@ ASF_TASK  void AlgorithmTask (ASF_TASK_ARG)
     OSP_GetLibraryVersion(&version);
     D1_printf("OSP Version: %s\r\n", version->VersionString);
 
+    /* Initialize the mutex */
+    mutex_id = osMutexCreate(osMutex(mutexCritSection));
+
     OSP_Status = OSP_Initialize(&gSystemDesc);
     ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "OSP_Initialize Failed");
     OSP_SetCalibrationConfig( 0x1);     // disable rotational cal.
@@ -909,6 +912,7 @@ ASF_TASK  void AlgorithmTask (ASF_TASK_ARG)
             D1_printf("Alg-FG:!!!UNHANDLED MESSAGE:%d!!!\r\n", rcvMsg->msgId);
             break;
         }
+        ASFDeleteMessage( ALGORITHM_TASK_ID, &rcvMsg );
 #ifdef DEBUG_TEST_SENSOR_SUBSCRIPTION
         // Testing subscribe and unsubscribe sensors
         DebugTestSensorSubscription();
@@ -952,6 +956,7 @@ ASF_TASK  void AlgBackGndTask (ASF_TASK_ARG)
             D1_printf("Alg-BG:!!!UNHANDLED MESSAGE:%d!!!\r\n", rcvMsg->msgId);
             break;
         }
+        ASFDeleteMessage( ALG_BG_TASK_ID, &rcvMsg );
     }
 }
 
diff --git a/embedded/common/app/cmdhandler_t.c b/embedded/common/app/cmdhandler_t.c
index ee4ef6c..80383d8 100644
--- a/embedded/common/app/cmdhandler_t.c
+++ b/embedded/common/app/cmdhandler_t.c
@@ -77,7 +77,8 @@ static uint8_t SerialRead( PortInfo *pPort, int8_t *pBuff, uint16_t length, uint
     uint32_t readIdx;
     uint16_t bytesRead = 0;
     uint8_t  retVal = APP_OK;
-    uint16_t evtFlags = 0;
+    osEvent evtFlags;
+    evtFlags.value.v=0;
 
     if ((pBuff == NULL) || (length == 0) || (length > RX_BUFFER_SIZE))
     {
@@ -91,8 +92,18 @@ static uint8_t SerialRead( PortInfo *pPort, int8_t *pBuff, uint16_t length, uint
             ((pPort->rxReadIdx + 1) % RX_BUFFER_SIZE))
         {
             /* Wait here for ISR event */
-            os_evt_wait_or( UART_CMD_RECEIVE | UART_CRLF_RECEIVE, EVT_WAIT_FOREVER );
-            evtFlags = os_evt_get();
+            osThreadId myId = osThreadGetId();
+            while(1){
+                evtFlags = osSignalWait(UART_CMD_RECEIVE,200);
+                if (evtFlags.status == osEventTimeout){
+                    evtFlags = osSignalWait(UART_CRLF_RECEIVE,200);
+                }
+                if (evtFlags.status == osEventSignal){
+                    break;
+                }
+            }
+            osSignalClear(myId,UART_CMD_RECEIVE);
+            osSignalClear(myId,UART_CRLF_RECEIVE);
         }
         else
         {
@@ -105,7 +116,7 @@ static uint8_t SerialRead( PortInfo *pPort, int8_t *pBuff, uint16_t length, uint
             }
             pBuff[bytesRead++] = pPort->rxBuffer[readIdx];
             pPort->rxReadIdx = readIdx;
-            if ( evtFlags & UART_CRLF_RECEIVE )
+            if ( evtFlags.value.signals & UART_CRLF_RECEIVE )
             {
                 break;
             }
diff --git a/embedded/common/app/common.h b/embedded/common/app/common.h
index 0b3511b..a46e14d 100644
--- a/embedded/common/app/common.h
+++ b/embedded/common/app/common.h
@@ -32,8 +32,6 @@
 /*-------------------------------------------------------------------------------------------------*\
  |    C O N S T A N T S   &   M A C R O S
 \*-------------------------------------------------------------------------------------------------*/
-#define OS_WAIT_NEVER                           0x00    ///< Zero wait as defined by RTX
-#define OS_WAIT_FOREVER                         0xFFFF  ///< Wait forever as defined by RTX
 #define TIMER_SYS_ID                            0xC0DEFEEDUL
 
 /* Critical Section Locks */
@@ -130,7 +128,7 @@ typedef enum AppResultCodesTag
 
 typedef struct AsfTimerTag
 {
-    TimerId         timerId;   /**< Id of the timer - internal use    */
+    osTimerId       timerId;   /**< Id of the timer - internal use    */
     TaskId          owner;     /**< Owner task that created the timer */
     uint16_t        ticks;     /**< Timeout value in system ticks     */
     uint16_t        userValue; /**< User defined value                */
@@ -146,7 +144,7 @@ typedef osp_bool_t (*fpInputValidate_t)(uint8_t);
 /* UART  driver data structure */
 typedef struct PortInfoTag
 {
-    uint32_t       *pBuffPool;
+    osPoolId       pBuffPool;
 #ifdef UART_DMA_ENABLE
     void           *pHead;
     void           *pTail;
@@ -228,7 +226,8 @@ void put_u32(const uint32_t);
 void _ASFTimerStart( TaskId owner, uint16_t ref, uint16_t tick, AsfTimer *pTimer, char *_file, int _line  );
 osp_bool_t ASFTimerStarted ( AsfTimer *pTimer );
 void _ASFKillTimer( AsfTimer *pTimer, char *_file, int _line );
-void _ASFTimerExpiry ( uint16_t info, char *_file, int _line );
+void _ASFTimerExpiry ( uint32_t info, char *_file, int _line );
+void ASFTimerCallback(void const *argument);
 void AsfInitialiseTasks ( void );
 
 /* User instrumentation hooks */
diff --git a/embedded/common/app/debugprint.c b/embedded/common/app/debugprint.c
index 56e7dc7..1a9b658 100644
--- a/embedded/common/app/debugprint.c
+++ b/embedded/common/app/debugprint.c
@@ -43,7 +43,13 @@ PortInfo gDbgUartPort;      //Debug information port
  */
 #ifdef UART_DMA_ENABLE
 # define DPRINTF_MPOOL_SIZE         (DPRINTF_BUFF_SIZE + 8)
-_declare_box( gMemPoolDprintf, DPRINTF_MPOOL_SIZE, MAX_DPRINTF_MESSAGES);
+typedef struct TempStructForPoolDefTag
+{
+    unsigned char uc_temp_for_pool_def[DPRINTF_MPOOL_SIZE]; //This variable is unused struct
+} TempStructForPoolDef;
+ /* Define memory pool */
+osPoolDef(gMemPoolDprintf, MAX_DPRINTF_MESSAGES, TempStructForPoolDef );
+osPoolId gMemPoolDprintf; 
 #endif
 
 
@@ -143,7 +149,7 @@ void *RemoveFromList( PortInfo *pPort )
 void DebugPortInit( void )
 {
 #if 0
-    _init_box( gMemPoolDprintf, sizeof(gMemPoolDprintf), DPRINTF_MPOOL_SIZE );
+    gMemPoolDprintf = osPoolCreate(osPool(gMemPoolDprintf));
     gDbgUartPort.pBuffPool = gMemPoolDprintf;
     gDbgUartPort.rxWriteIdx = 1;
     gDbgUartPort.rxReadIdx  = 0;
@@ -200,12 +206,12 @@ void RxBytesToBuff( PortInfo *pPort, uint8_t byte )
         {
             if (byte == '\r' || byte == '\n')
             {
-                isr_evt_set( UART_CRLF_RECEIVE, asfTaskHandleTable[pPort->rcvTask].handle );
+                osSignalSet(asfTaskHandleTable[pPort->rcvTask].posThreadId,UART_CRLF_RECEIVE);
             }
             else
             {
                 /* Wake up the task. */
-                isr_evt_set( UART_CMD_RECEIVE, asfTaskHandleTable[pPort->rcvTask].handle );
+                osSignalSet(asfTaskHandleTable[pPort->rcvTask].posThreadId,UART_CMD_RECEIVE);
             }
         }
 
@@ -233,7 +239,7 @@ void RxBytesToBuff( PortInfo *pPort, uint8_t byte )
 void *GetNextBuffer( PortInfo *pPort )
 {
     void *pFreeBuff = RemoveFromList( pPort );
-    ASF_assert(_free_box( pPort->pBuffPool, pFreeBuff ) == 0); //Free the current consumed buffer
+    ASF_assert(osPoolFree( pPort->pBuffPool, pFreeBuff ) == 0); //Free the current consumed buffer
     return pPort->pHead; //Return the current head of the list
 }
 #endif
@@ -266,7 +272,7 @@ int Print_LIPS( const char *fmt, ... )
 
 #if defined UART_DMA_ENABLE
     /* Note: Output will be truncated to allowed max size */
-    pNewBuff = _alloc_box(pPort->pBuffPool);
+    pNewBuff = osPoolAlloc(pPort->pBuffPool);
     ASF_assert( pNewBuff != NULL );
     pPrintBuff = M_GetBuffStart(pNewBuff);
     if (pPrintBuff != NULL)
@@ -334,7 +340,7 @@ int _dprintf( uint8_t dbgLvl, const char *fmt, ... )
 
 #ifdef UART_DMA_ENABLE
             /* Note: Output will be truncated to allowed max size */
-            pNewBuff = _alloc_box(pPort->pBuffPool);
+            pNewBuff = osPoolAlloc(pPort->pBuffPool);
             ASF_assert( pNewBuff != NULL );
             if (pNewBuff != NULL)
             {
diff --git a/embedded/common/app/instrmgr_t.c b/embedded/common/app/instrmgr_t.c
index 0e5a62d..3dad698 100644
--- a/embedded/common/app/instrmgr_t.c
+++ b/embedded/common/app/instrmgr_t.c
@@ -33,8 +33,6 @@ extern uint32_t gStackSize;
 
 extern const AsfTaskInitDef C_gAsfTaskInitTable[NUMBER_OF_TASKS];
 extern uint32_t gSystemRTCRefTime;
-extern struct OS_TCB os_idle_TCB; //RTX internal
-extern void *os_active_TCB[];     //RTX internal
 
 extern void InitializeTasks( void );
 extern uint8_t GetTaskList( uint8_t **pTaskList );
@@ -111,6 +109,7 @@ ASF_TASK void InstrManagerTask( ASF_TASK_ARG )
 
             }
         }
+        ASFDeleteMessage( INSTR_MANAGER_TASK_ID, &rcvMsg );
     }
 }
 
diff --git a/embedded/common/app/instrmgr_user.c b/embedded/common/app/instrmgr_user.c
index 015335b..169cdab 100644
--- a/embedded/common/app/instrmgr_user.c
+++ b/embedded/common/app/instrmgr_user.c
@@ -33,7 +33,7 @@ RtcClock_t gRtcClockData;
 /*-------------------------------------------------------------------------------------------------*\
  |    P R I V A T E   C O N S T A N T S   &   M A C R O S
 \*-------------------------------------------------------------------------------------------------*/
-
+#define	INSTR_MGR_SAMPLE_PERIOD (1000)	//This is currently unused
 /*-------------------------------------------------------------------------------------------------*\
  |    P R I V A T E   T Y P E   D E F I N I T I O N S
 \*-------------------------------------------------------------------------------------------------*/
@@ -110,7 +110,7 @@ void InstrManagerUserInit( void )
 {
     sInitialTime = os_time;
 
-   // ASFTimerStart( INSTR_MANAGER_TASK_ID, TIMER_REF_RTC_UPDATE, TICS_PER_SEC, &sRtcUpdateTimer );
+    // ASFTimerStart( INSTR_MANAGER_TASK_ID, TIMER_REF_RTC_UPDATE, INSTR_MGR_SAMPLE_PERIOD, &sRtcUpdateTimer );
 }
 
 
@@ -137,7 +137,7 @@ osp_bool_t InstrManagerUserHandler( MessageBuffer *pMsg )
             UpdateRTC();
 
             /* Restart timer for periodic output */
-            // ASFTimerStart( INSTR_MANAGER_TASK_ID, TIMER_REF_RTC_UPDATE, TICS_PER_SEC, &sRtcUpdateTimer );
+            // ASFTimerStart( INSTR_MANAGER_TASK_ID, TIMER_REF_RTC_UPDATE, INSTR_MGR_SAMPLE_PERIOD, &sRtcUpdateTimer );
             break;
 
         default:
diff --git a/embedded/common/app/sensoracq_t.c b/embedded/common/app/sensoracq_t.c
index fbf573b..963eff5 100644
--- a/embedded/common/app/sensoracq_t.c
+++ b/embedded/common/app/sensoracq_t.c
@@ -51,6 +51,12 @@ void WaitForHostSync(void);
 static AsfTimer sSensorTimer = NULL_TIMER;
 #endif
 static AsfTimer sPressureTimer = NULL_TIMER;
+
+/*  This flag prevents data-ready messeges to be sent until
+ *  Sensor Acquisition task is ready
+ */
+static uint8_t sTskRdyFlag = 0;
+
 /*-------------------------------------------------------------------*\
  |    F O R W A R D   F U N C T I O N   D E C L A R A T I O N S
 \*-------------------------------------------------------------------*/
@@ -333,18 +339,25 @@ void SensorControlCmdHandler(MsgSensorControlData *pData)
 void SendDataReadyIndication(uint8_t sensorId, uint32_t timeStamp)
 {
     MessageBuffer *pSendMsg = NULLP;
-    if (ASFCreateMessage(MSG_SENSOR_DATA_RDY,
-            sizeof(MsgSensorDataRdy), &pSendMsg) == ASF_OK) {
-        pSendMsg->msg.msgSensorDataRdy.sensorId = sensorId;
-        pSendMsg->msg.msgSensorDataRdy.timeStamp = timeStamp;
-
-        if ( ASFSendMessage(SENSOR_ACQ_TASK_ID, pSendMsg) != ASF_OK ) {
-            D0_printf("Error sending sensoracq message for senosr id %d\r\n", sensorId);
+/* Do not send a message until sensor acquisition task is ready */
+    if (0 != sTskRdyFlag){
+        if (ASFCreateMessage(MSG_SENSOR_DATA_RDY,
+                sizeof(MsgSensorDataRdy), &pSendMsg) == ASF_OK) {
+            pSendMsg->msg.msgSensorDataRdy.sensorId = sensorId;
+            pSendMsg->msg.msgSensorDataRdy.timeStamp = timeStamp;
+            
+            if ( ASFSendMessage(SENSOR_ACQ_TASK_ID, pSendMsg) != ASF_OK ) {
+                D0_printf("Error sending sensoracq message for senosr id %d\r\n", sensorId);           
+            }        
+        } else {
+            D0_printf("Error creating sensoracq message for sensor id %d\r\n", sensorId);
         }
-    } else {
-        D0_printf("Error creating sensoracq message for sensor id %d\r\n", sensorId);
     }
-
+/* Mag interrupt needs to be explicitly cleaned after the interrupt is read/ignored */
+    else if(sensorId == MAG_INPUT_SENSOR)
+    {
+        Mag_ClearDataInt();
+    }
 }
 
 /*******************************************************************
@@ -370,7 +383,7 @@ ASF_TASK void SensorAcqTask(ASF_TASK_ARG)
     volatile uint8_t  i;
 
 #ifndef WAIT_FOR_HOST_SYNC
-    os_dly_wait(MSEC_TO_TICS(50)); /* Allow startup time for sensors */
+    osDelay(50);
 #else
     WaitForHostSync(); //This also allows for startup time for sensors
 #endif
@@ -417,6 +430,8 @@ ASF_TASK void SensorAcqTask(ASF_TASK_ARG)
 
     /* Magnetometer sensor does not re-generate interrupt if its outputs are not read. */
     Mag_ClearDataInt();
+    /* Indicate sensor init done */
+    sTskRdyFlag = 1;
 
     while (1) {
         ASFReceiveMessage(SENSOR_ACQ_TASK_ID, &rcvMsg);
@@ -450,6 +465,7 @@ ASF_TASK void SensorAcqTask(ASF_TASK_ARG)
             D2_printf("SensorAcqTask:!!!UNHANDLED MESSAGE:%d!!!\r\n", rcvMsg->msgId);
             break;
         }
+        ASFDeleteMessage( SENSOR_ACQ_TASK_ID, &rcvMsg );
     }
 }
 
diff --git a/embedded/common/asf/asf_messages.c b/embedded/common/asf/asf_messages.c
index 7bb0ff7..6981a5a 100644
--- a/embedded/common/asf/asf_messages.c
+++ b/embedded/common/asf/asf_messages.c
@@ -26,7 +26,6 @@
 /*-------------------------------------------------------------------------------------------------*\
  |    E X T E R N A L   V A R I A B L E S   &   F U N C T I O N S
 \*-------------------------------------------------------------------------------------------------*/
-extern const AsfTaskInitDef C_gAsfTaskInitTable[NUMBER_OF_TASKS];
 
 /*-------------------------------------------------------------------------------------------------*\
  |    P U B L I C   V A R I A B L E S   D E F I N I T I O N S
@@ -35,7 +34,6 @@ extern const AsfTaskInitDef C_gAsfTaskInitTable[NUMBER_OF_TASKS];
 /*-------------------------------------------------------------------------------------------------*\
  |    P R I V A T E   C O N S T A N T S   &   M A C R O S
 \*-------------------------------------------------------------------------------------------------*/
-#define MESSAGE_BLOCK_SIZE  (sizeof(MessageBlock))
 
 /*-------------------------------------------------------------------------------------------------*\
  |    P R I V A T E   T Y P E   D E F I N I T I O N S
@@ -51,12 +49,8 @@ uint32_t debugMessageBlockSize = 0;
 
 #define TRACK_MSG_POOL	0
 #if USE_ALLOC
-_declare_box( mpool, ///< this memory pool will be used to allocate the messages
-    MESSAGE_BLOCK_SIZE, /**< this is the size of the regular messages.
-          To avoid variable length, we allocate from this fixed size. If memory usage become issue
-          we can divide the messages among various pool size that would optimize memory usage */
-    MAX_SYSTEM_MESSAGES   //< Max (non dprintf) messages in the system
-    );
+osPoolDef(cmpool, MAX_SYSTEM_MESSAGES, MessageBlock); 		   /* Define memory pool			  */
+osPoolId cmpool;
 #else
 struct _MsgPool {
 	volatile int32_t	status;
@@ -141,7 +135,7 @@ static void MsgPool_put(struct _MsgPool *p, void *m)
  *
  * @see     ASFCreateMessage(), ASFSendMessage(), ASFReceiveMessage()
  ***************************************************************************************************/
-static void _ASFDeleteMessage ( MessageBuffer **pMbuf, char *_file, int _line )
+void _ASFDeleteMessage ( TaskId rcvTask, MessageBuffer **pMbuf, char *_file, int _line )
 {
     MessageBlock *pBlock;
 
@@ -151,7 +145,7 @@ static void _ASFDeleteMessage ( MessageBuffer **pMbuf, char *_file, int _line )
         /* Get the block pointer */
         M_GetMsgBlockFromBuffer (pBlock, *pMbuf);
 #if USE_ALLOC
-        ASF_assert( _free_box( mpool, pBlock ) == 0 );
+        ASF_assert( osPoolFree( cmpool, pBlock ) == osOK);
 #else
         MsgPool_put(MsgPool, pBlock);
 #endif
@@ -174,7 +168,7 @@ static void _ASFDeleteMessage ( MessageBuffer **pMbuf, char *_file, int _line )
 void ASFMessagingInit( void )
 {
 #if USE_ALLOC
-    _init_box( mpool, sizeof(mpool), MESSAGE_BLOCK_SIZE );
+    cmpool = osPoolCreate(osPool(cmpool));
 #else
 	MsgPool_init(MsgPool);
 #endif
@@ -204,7 +198,7 @@ AsfResult_t _ASFCreateMessage( MessageId msgId, uint16_t msgSize, MessageBuffer
     ASF_assert_var( *pMbuf == NULLP, msgId, 0, 0 );
 
 #if USE_ALLOC
-    pBlock = _alloc_box(mpool);
+    pBlock = osPoolAlloc(cmpool);
 #else
 	pBlock = MsgPool_get(MsgPool, _file, _line);
 #endif
@@ -235,7 +229,7 @@ uint32_t	wtf_msg_cnt = 0;
 AsfResult_t _ASFSendMessage ( TaskId destTask, MessageBuffer *pMbuf, char *_file, int _line )
 {
     MessageBlock *pBlock;
-    OS_RESULT err;
+    osStatus os_ret = osErrorOS;
 
     /* Check for the usual - null pointers etc. */
     ASF_assert_var( pMbuf != NULLP, pMbuf->msgId, 0, 0 );
@@ -244,33 +238,16 @@ AsfResult_t _ASFSendMessage ( TaskId destTask, MessageBuffer *pMbuf, char *_file
     M_GetMsgBlockFromBuffer (pBlock, pMbuf);
 
     pBlock->header.destTask = destTask;
-
-    /* Send the message without pending */
-    if ( GetContext() != CTX_ISR )
+    os_ret = osMailPut(asfTaskHandleTable[destTask].posMailQId,pMbuf);
+    if(osOK != os_ret)
     {
-        err = os_mbx_send( C_gAsfTaskInitTable[destTask].queue, pMbuf, 0 );
-        if (err != OS_R_OK) //Mailbox is not valid or full
-        {
 #if USE_ALLOC
-            ASF_assert( _free_box( mpool, pBlock ) == 0 );
+        ASF_assert( osPoolFree( cmpool, pBlock ) == 0 );
 #else
 		MsgPool_put(MsgPool, pBlock);
 #endif
-            return ASF_ERR_Q_FULL;
-        }
-    }
-    else
-    {
-        err = isr_mbx_check( C_gAsfTaskInitTable[destTask].queue );
-	if (err != 0) {
-#if 0
-		ASF_assert_var(err != 0, err, pMbuf->msgId, destTask);
-#endif
-		isr_mbx_send( C_gAsfTaskInitTable[destTask].queue, pMbuf );
-	} else {
-		wtf_msg_cnt++;
+        return ASF_ERR_Q_FULL;
 	}
-    }
     return ASF_OK;
 }
 
@@ -292,17 +269,18 @@ AsfResult_t _ASFSendMessage ( TaskId destTask, MessageBuffer *pMbuf, char *_file
  ***************************************************************************************************/
 void _ASFReceiveMessage ( TaskId rcvTask, MessageBuffer **pMbuf, char *_file, int _line )
 {
-    OS_RESULT   err;
+    osEvent  evt;
 
     /* Delete old/previous message to release its buffer */
-    _ASFDeleteMessage( pMbuf, _file, _line );
+    _ASFDeleteMessage( rcvTask, pMbuf, _file, _line );
 
-    /* Wait for receive */
-    err = os_mbx_wait( C_gAsfTaskInitTable[rcvTask].queue, (void **)pMbuf, OS_WAIT_FOREVER );
-    ASF_assert_var(((err == OS_R_OK) || (err == OS_R_MBX)), err, 0, 0);
+    evt = osMailGet(asfTaskHandleTable[rcvTask].posMailQId,osWaitForever);
+    if (evt.status == osEventMail)
+    {
+        *pMbuf = evt.value.p;
+    }
 }
 
-
 /****************************************************************************************************
  * @fn      ASFReceiveMessagePoll
  *          This function tries to receive a message on the queue of the calling task without blocking.
@@ -320,18 +298,21 @@ void _ASFReceiveMessage ( TaskId rcvTask, MessageBuffer **pMbuf, char *_file, in
  ***************************************************************************************************/
 osp_bool_t _ASFReceiveMessagePoll ( TaskId rcvTask, MessageBuffer **pMbuf, char *_file, int _line )
 {
-    OS_RESULT   err;
+    osEvent  evt;
 
     /* Delete old message to release its buffer */
-    _ASFDeleteMessage( pMbuf, _file, _line );
+    _ASFDeleteMessage( rcvTask, pMbuf, _file, _line );
 
     /* Wait for receive */
-    err = os_mbx_wait( C_gAsfTaskInitTable[rcvTask].queue, (void **)pMbuf, OS_WAIT_NEVER );
-    if (err == OS_R_TMO)
+    evt = osMailGet( asfTaskHandleTable[rcvTask].posMailQId, 0 );
+    if (evt.status == osEventTimeout)
     {
         return false;
     }
-    ASF_assert_var(((err == OS_R_OK) || (err == OS_R_MBX)), err, 0, 0);
+    if (evt.status == osEventMail)
+    {
+        *pMbuf = evt.value.p;
+    }
     return true;
 }
 
diff --git a/embedded/common/asf/asf_msgstruct.h b/embedded/common/asf/asf_msgstruct.h
index 89f22b4..59a2283 100644
--- a/embedded/common/asf/asf_msgstruct.h
+++ b/embedded/common/asf/asf_msgstruct.h
@@ -58,7 +58,7 @@ typedef struct MsgNoDataTag
 typedef struct MsgTimerExpiryTag
 {
     uint16_t userValue;
-    TimerId  timerId;
+    osTimerId	timerId;
 } MsgTimerExpiry;
 
 
@@ -122,8 +122,8 @@ union Message
 #define ASFReceiveMessagePoll( id, pm )    \
     _ASFReceiveMessagePoll( id, pm, __MODULE__, __LINE__ )
 
-#define ASFDeleteMessage( pm )  \
-    _ASFDeleteMessage( pm, __MODULE__, __LINE__ )
+#define ASFDeleteMessage( id, pm )  \
+    _ASFDeleteMessage( id, pm, __MODULE__, __LINE__ )
 
 
 /*-------------------------------------------------------------------------------------------------*\
@@ -179,7 +179,7 @@ void ASFMessagingInit( void );
 AsfResult_t _ASFCreateMessage( MessageId msgId, uint16_t msgSize, MessageBuffer **pMbuf, char *_file, int _line );
 AsfResult_t _ASFSendMessage ( TaskId destTask, MessageBuffer *pMbuf, char *_file, int _line );
 void _ASFReceiveMessage ( TaskId rcvTask, MessageBuffer **pMbuf, char *_file, int _line );
-void _ASFDeleteMessage ( MessageBuffer **pMbuf, char *_file, int _line );
+void _ASFDeleteMessage ( TaskId rcvTask, MessageBuffer **pMbuf, char *_file, int _line );
 osp_bool_t _ASFReceiveMessagePoll ( TaskId rcvTask, MessageBuffer **pMbuf, char *_file, int _line );
 
 
diff --git a/embedded/common/asf/asf_taskdeftype.h b/embedded/common/asf/asf_taskdeftype.h
index 9e5ac2d..7c02984 100644
--- a/embedded/common/asf/asf_taskdeftype.h
+++ b/embedded/common/asf/asf_taskdeftype.h
@@ -37,6 +37,8 @@
 #define ASF_QUEUE_SETUP     3
 #define ASF_TASK_SETUP      4
 #define ASF_TASK_DECLARE    5
+#define ASF_THREAD_SETUP    6
+#define ASF_TIMER_SETUP     7
 
 #define IN_ASF_TASK_DEF
 
@@ -58,8 +60,8 @@
 #undef ASF_TASK_DEF_TYPE
 #undef IN_ASF_TASK_DEF
 
-#define ASF_TASK        __task
-#define ASF_TASK_ARG    void *argv
+#define ASF_TASK 
+#define ASF_TASK_ARG    void const *argv
 
 
 /*-------------------------------------------------------------------------------------------------*\
diff --git a/embedded/common/asf/asf_taskinit.c b/embedded/common/asf/asf_taskinit.c
index d9bb235..5dc7c73 100644
--- a/embedded/common/asf/asf_taskinit.c
+++ b/embedded/common/asf/asf_taskinit.c
@@ -21,7 +21,7 @@
 #include "common.h"
 #include "asf_taskstruct.h"
 #include <stdlib.h>
-
+#include "cmsis_os.h"
 
 /*-------------------------------------------------------------------------------------------------*\
  |    E X T E R N A L   V A R I A B L E S   &   F U N C T I O N S
@@ -29,7 +29,6 @@
 void ASFMessagingInit( void );
 extern uint8_t GetTaskList( uint8_t **pTaskList );
 
-
 #define STACK_INCREASE                  0
 
 
@@ -56,6 +55,19 @@ extern uint8_t GetTaskList( uint8_t **pTaskList );
 #include "asf_taskdeftype.h"
 #include "asf_tasks.h"
 
+/**
+ * Define the CMSIS Thread structure
+ */
+#define ASF_TASK_DEF_TYPE ASF_THREAD_SETUP
+#include "asf_taskdeftype.h"
+#include "asf_tasks.h"
+
+/**
+ * Define the CMSIS Timer structure
+ */
+#define ASF_TASK_DEF_TYPE ASF_TIMER_SETUP
+#include "asf_taskdeftype.h"
+#include "asf_tasks.h"
 
 /**
  * This is the task initialization table which details all the information
@@ -76,6 +88,7 @@ const AsfTaskInitDef C_gAsfTaskInitTable[NUMBER_OF_TASKS] =
  */
 AsfTaskHandle asfTaskHandleTable[NUMBER_OF_TASKS];
 
+
 /*-------------------------------------------------------------------------------------------------*\
  |    P R I V A T E   C O N S T A N T S   &   M A C R O S
 \*-------------------------------------------------------------------------------------------------*/
@@ -89,9 +102,6 @@ const uint32_t TotalStkNeeded =
 #include "asf_tasks.h"
 );
 
-/* Heap Area defined here */
-U64 NewHeap[TotalStkNeeded/8] = {0};
-
 
 /*-------------------------------------------------------------------------------------------------*\
  |    P R I V A T E   T Y P E   D E F I N I T I O N S
@@ -127,7 +137,6 @@ void InitializeTasks( void )
     uint8_t  taskCounter, numTasks;
     TaskId tid;
     uint8_t *pTaskTable;
-    uint32_t *pU64Aligned;
 
     /* Create tasks  based on the mode we are in */
     numTasks = GetTaskList( &pTaskTable );
@@ -137,31 +146,22 @@ void InitializeTasks( void )
 
         if (tid != INSTR_MANAGER_TASK_ID)
         {
-            /* Allocate task stack from heap */
-            /* NOTE: All mallocs are 8-byte aligned as per ARM stack alignment requirements */
-            pU64Aligned = malloc( C_gAsfTaskInitTable[tid].stackSize );
-            ASF_assert( pU64Aligned != NULL );
-            ASF_assert( ((uint32_t)pU64Aligned & 0x7) == 0 ); //Ensure 64-bit aligned
-
-            asfTaskHandleTable[tid].handle  = os_tsk_create_user( C_gAsfTaskInitTable[tid].entryPoint,
-                C_gAsfTaskInitTable[tid].priority, pU64Aligned, C_gAsfTaskInitTable[tid].stackSize);
-            ASF_assert( asfTaskHandleTable[tid].handle != 0 );
+            asfTaskHandleTable[tid].posThreadId  = osThreadCreate(C_gAsfTaskInitTable[tid].posThreadDef,NULL);
+
+            ASF_assert( asfTaskHandleTable[tid].posThreadId != NULL );
             asfTaskHandleTable[tid].stkSize = C_gAsfTaskInitTable[tid].stackSize;
-            asfTaskHandleTable[tid].pStack = pU64Aligned; /* Keep track of our stack pointer */
+            asfTaskHandleTable[tid].pStack = NULL; /* Keep track of our stack pointer */
         }
 
         /* Initialize the associated queue */
-        if (asfTaskHandleTable[tid].handle != 0)
+        if (NULL != asfTaskHandleTable[tid].posThreadId)
         {
-            os_mbx_init( C_gAsfTaskInitTable[tid].queue, C_gAsfTaskInitTable[tid].queueSize );
+            asfTaskHandleTable[tid].posMailQId = osMailCreate(C_gAsfTaskInitTable[tid].mailQDef,NULL);
         }
     }
 
-    /* Initialize the messaging */
-    ASFMessagingInit();
-
     /* Switch the priority to be lowest now */
-    os_tsk_prio_self( C_gAsfTaskInitTable[INSTR_MANAGER_TASK_ID].priority );
+    osThreadSetPriority(asfTaskHandleTable[INSTR_MANAGER_TASK_ID].posThreadId,osPriorityLow);
 }
 
 
@@ -178,18 +178,27 @@ void InitializeTasks( void )
  ***************************************************************************************************/
 void AsfInitialiseTasks ( void )
 {
-    uint32_t *pU64Aligned;
+    uint8_t  taskCounter=0;
+    TaskId tid;
+    uint8_t *pTaskTable;
 
+    /* Create tasks  based on the mode we are in */
+    GetTaskList( &pTaskTable );
+    tid = (TaskId)pTaskTable[taskCounter];
+
+    /* Allocate task stack from heap */
     /* NOTE: All mallocs are 8-byte aligned as per ARM stack alignment requirements */
-    pU64Aligned = malloc( C_gAsfTaskInitTable[INSTR_MANAGER_TASK_ID].stackSize );
-    ASF_assert( ((uint32_t)pU64Aligned & 0x7) == 0 ); //Ensure 64-bit aligned
+    /* Initialize the messaging */
+    ASFMessagingInit();
+
+    asfTaskHandleTable[tid].posThreadId = osThreadCreate(C_gAsfTaskInitTable[tid].posThreadDef,NULL);
+
+    ASF_assert( NULL != asfTaskHandleTable[tid].posThreadId );
 
-    asfTaskHandleTable[INSTR_MANAGER_TASK_ID].handle  = 1; //Initial task always gets this OS_ID
-    asfTaskHandleTable[INSTR_MANAGER_TASK_ID].stkSize = C_gAsfTaskInitTable[INSTR_MANAGER_TASK_ID].stackSize;
-    asfTaskHandleTable[INSTR_MANAGER_TASK_ID].pStack = pU64Aligned;
+    asfTaskHandleTable[tid].stkSize = C_gAsfTaskInitTable[tid].stackSize;
+    asfTaskHandleTable[tid].pStack = NULL; /* Keep track of our stack pointer */
 
-    /* Initialize RTX and start initialTask */
-    os_sys_init_user( InstrManagerTask, 254, pU64Aligned, C_gAsfTaskInitTable[INSTR_MANAGER_TASK_ID].stackSize );
+    osThreadTerminate(osThreadGetId());
 }
 
 
diff --git a/embedded/common/asf/asf_tasks.h b/embedded/common/asf/asf_tasks.h
index cbffa09..ae6186b 100644
--- a/embedded/common/asf/asf_tasks.h
+++ b/embedded/common/asf/asf_tasks.h
@@ -51,13 +51,13 @@
 */
 /* Declare all ASF tasks here */
 /* This task creates other tasks and OS resources and must always be present */
-ASF_TASK_STATIC ( INSTR_MANAGER_TASK_ID,  InstrManagerTask,   50,  0x800,    4 )
+ASF_TASK_STATIC ( INSTR_MANAGER_TASK_ID,  InstrManagerTask,   osPriorityNormal,  0x200,    4 )
 #if 0
 /* Handles command input from UART */
-ASF_TASK_STATIC ( CMD_HNDLR_TASK_ID,      CmdHandlerTask,     92,  0x800,    4 )
+ASF_TASK_STATIC ( CMD_HNDLR_TASK_ID,      CmdHandlerTask,     osPriorityNormal,  0x800,    4 )
 #endif
 /* Sensor data handler task */
-ASF_TASK_STATIC ( SENSOR_ACQ_TASK_ID,     SensorAcqTask,      95, 0x800,   64 )
+ASF_TASK_STATIC ( SENSOR_ACQ_TASK_ID,     SensorAcqTask,      osPriorityAboveNormal, 0x400,   64 )
 
 /* Additional tasks specific to application is defined in App_Tasks.h */
 #include "app_tasks.h"
diff --git a/embedded/common/asf/asf_taskstruct.h b/embedded/common/asf/asf_taskstruct.h
index 1de0c65..4593c12 100644
--- a/embedded/common/asf/asf_taskstruct.h
+++ b/embedded/common/asf/asf_taskstruct.h
@@ -26,6 +26,7 @@
 #endif
 
 #include <stdint.h>
+#include "cmsis_os.h"
 
 /*-------------------------------------------------------------------------------------------------*\
  |    T Y P E   D E F I N I T I O N S
@@ -41,12 +42,12 @@ typedef struct AsfTaskInitDefTag
     TaskId      taskId;
     void        (*entryPoint)();
     char        *tskName;
-    OS_ID       queue;
-    uint16_t    queueSize;
     uint16_t    stackSize;
-    uint8_t     priority;
+    int8_t      priority;   /* Task priority can be negative in CMSIS */
     char        *tidString; /* String equivalent of the TASK_ID enum */
-
+    const osThreadDef_t * posThreadDef;
+    const osMailQDef_t	*mailQDef;
+    const osTimerDef_t	*timerDef;
 } AsfTaskInitDef;
  
 
diff --git a/embedded/common/asf/asf_tdefmacros.h b/embedded/common/asf/asf_tdefmacros.h
index c929227..a158035 100644
--- a/embedded/common/asf/asf_tdefmacros.h
+++ b/embedded/common/asf/asf_tdefmacros.h
@@ -48,7 +48,7 @@
 
 #if ASF_TASK_DEF_TYPE == ASF_QUEUE_SETUP
 # define ASF_TASK_STATIC( ThreadId, EntryFunction, Priority, StackSize, QueueSize )           \
-    os_mbx_declare( Q_##ThreadId, QueueSize );
+    osMailQDef(ThreadId, QueueSize, MessageBlock);
 #endif
 
 #if ASF_TASK_DEF_TYPE == ASF_TASK_DECLARE
@@ -58,7 +58,7 @@
 
 #if ASF_TASK_DEF_TYPE == ASF_TASK_SETUP
 # define ASF_TASK_STATIC( ThreadId, EntryFunction, Priority, StackSize, QueueSize )             \
-        { ThreadId, EntryFunction, #EntryFunction, Q_##ThreadId, sizeof(Q_##ThreadId), StackSize, Priority, #ThreadId },
+        { ThreadId, EntryFunction, #EntryFunction, StackSize, Priority, #ThreadId, osThread(EntryFunction),osMailQ(ThreadId),osTimer(ThreadId)},
 #endif
 
 #if ASF_TASK_DEF_TYPE == ASF_TOTAL_STACK_NEEDED
@@ -66,7 +66,17 @@
     +ThreadId##_StkSize
 #endif
 
+#if ASF_TASK_DEF_TYPE == ASF_THREAD_SETUP
+# define ASF_TASK_STATIC( ThreadId, EntryFunction, Priority, StackSize, QueueSize )           \
+    osThreadDef(EntryFunction,Priority,1,StackSize);
+#endif
+
+#if ASF_TASK_DEF_TYPE == ASF_TIMER_SETUP
+#define ASF_TASK_STATIC( ThreadId, EntryFunction, Priority, StackSize, QueueSize )           \
+    osTimerDef(ThreadId, ASFTimerCallback);
+#endif
 
 /*-------------------------------------------------------------------------------------------------*\
  |    E N D   O F   F I L E
 \*-------------------------------------------------------------------------------------------------*/
+
diff --git a/embedded/common/asf/asf_timers.c b/embedded/common/asf/asf_timers.c
index 14f8ee7..07e1623 100644
--- a/embedded/common/asf/asf_timers.c
+++ b/embedded/common/asf/asf_timers.c
@@ -19,7 +19,7 @@
  |    I N C L U D E   F I L E S
 \*-------------------------------------------------------------------------------------------------*/
 #include "common.h"
-
+#include "asf_taskstruct.h"
 /*-------------------------------------------------------------------------------------------------*\
  |    E X T E R N A L   V A R I A B L E S   &   F U N C T I O N S
 \*-------------------------------------------------------------------------------------------------*/
@@ -34,6 +34,7 @@
 #ifndef RAM_START
 # define RAM_START           NVIC_VectTab_RAM
 #endif
+extern const AsfTaskInitDef C_gAsfTaskInitTable[NUMBER_OF_TASKS];
 
 #ifndef OS_TIMERCNT 
     #define OS_TIMERCNT (8)    // Make sure this value matches file rtx_conf_cm.c
@@ -125,39 +126,13 @@ static void SendTimerExpiry ( AsfTimer *pTimer )
     MessageBuffer *pSendMsg = NULLP;
 
     ASF_assert( ASFCreateMessage( MSG_TIMER_EXPIRY, sizeof(MsgTimerExpiry), &pSendMsg ) == ASF_OK );
+    pSendMsg->msg.msgTimerExpiry.timerId   = (osTimerId)pTimer->timerId;
     pSendMsg->msg.msgTimerExpiry.userValue = pTimer->userValue;
-    pSendMsg->msg.msgTimerExpiry.timerId   = pTimer->timerId;
+    __enable_irq();
     ASF_assert( ASFSendMessage( pTimer->owner, pSendMsg ) == ASF_OK );
+    __disable_irq();
 }
 
-
-
-/****************************************************************************************************
- * @fn      ASFTimerStart
- *          Creates a new timer in the system with the given attributes.
- *
- * @param   pTimer  Pointer to timer control block containing the attributes of the timer to be
- *                  created.
- *
- * @return  none
- *
- * @see     ASFDeleteTimer()
- ***************************************************************************************************/
-static void _TimerStart ( AsfTimer *pTimer, char *_file, int _line )
-{    
-    if ( _asfTimerInitialized == FALSE ) AsfTimerInit();
-
-    ASF_assert( pTimer != NULLP );
-    ASF_assert( pTimer->sysUse != TIMER_SYS_ID ); //In case we are trying to restart a running timer
-
-    // Add this timer to the managed list 
-    pTimer->info = AsfTimerAddTimerToList(pTimer);
-    pTimer->sysUse = TIMER_SYS_ID;
-    pTimer->timerId = os_tmr_create( pTimer->ticks, pTimer->info );
-    ASF_assert( pTimer->timerId != NULL );
-}
-
-
 /*-------------------------------------------------------------------------------------------------*\
  |    P U B L I C     F U N C T I O N S
 \*-------------------------------------------------------------------------------------------------*/
@@ -194,10 +169,21 @@ osp_bool_t ASFTimerStarted ( AsfTimer *pTimer )
 ***************************************************************************************************/
 void _ASFTimerStart( TaskId owner, uint16_t ref, uint16_t tick, AsfTimer *pTimer, char *_file, int _line  )
 {
+    uint16_t index = AsfTimerAddTimerToList(pTimer);  // Add this timer to the managed list 
+
     pTimer->owner = owner;
     pTimer->ticks = tick;
     pTimer->userValue = ref;
-    _TimerStart( pTimer, _file, _line );
+    if(pTimer->timerId == NULL)
+    {
+        pTimer->timerId = osTimerCreate(C_gAsfTaskInitTable[owner].timerDef,osTimerOnce,(void *)index);
+    }
+    if (pTimer->timerId) 
+    {
+        pTimer->sysUse = TIMER_SYS_ID;
+        osTimerStart(pTimer->timerId, pTimer->ticks);
+    }
+
 }
 
 
@@ -211,17 +197,21 @@ void _ASFTimerStart( TaskId owner, uint16_t ref, uint16_t tick, AsfTimer *pTimer
  *
  * @see     ASFKillTimer()
  ***************************************************************************************************/
-void _ASFTimerExpiry ( uint16_t info, char *_file, int _line )
+void _ASFTimerExpiry ( uint32_t info, char *_file, int _line )
 {
     AsfTimer *pTimer;
     int wasMasked = __disable_irq();
     pTimer = AsfTimerGetTimerFromList(info);
+    AsfTimerRemoveTimerFromList(info);
 
     //Look for our magic number to be sure we got the right pointer
     ASF_assert_var( pTimer->sysUse == TIMER_SYS_ID,  pTimer->ticks, pTimer->userValue, pTimer->owner);
-    SendTimerExpiry( pTimer );
+    /* Reset timer before starting to process it.
+     *  This is to prevent a race condition, where the processing task restarts a timer before we reset here.
+     * Timer thread runs on a lower priority then, the processing task
+     */
     pTimer->sysUse = (uint32_t)-1; //Timer no longer in use
-    AsfTimerRemoveTimerFromList(info);   
+    SendTimerExpiry( pTimer );
     if (!wasMasked) __enable_irq();
 }
 
@@ -239,14 +229,27 @@ void _ASFTimerExpiry ( uint16_t info, char *_file, int _line )
  ***************************************************************************************************/
 void _ASFKillTimer ( AsfTimer *pTimer, char *_file, int _line )
 {
-    TimerId ret;
+    osStatus    os_ret = osErrorOS;
     ASF_assert( pTimer != NULLP );
-    ret = os_tmr_kill( pTimer->timerId );
-    ASF_assert( ret == NULL );
+    os_ret = osTimerDelete(pTimer->timerId);
+    ASF_assert( os_ret == osOK );
     pTimer->sysUse = (uint32_t)-1; //Timer no longer in use
     AsfTimerRemoveTimerFromList(pTimer->info);
 }
-
+/****************************************************************************************************
+ * @fn      ASFTimerCallback
+ *          Timer callback registered with CMSIS for timer expiry notification
+ *
+ * @param   argument  Param provided to CMSIS for callback, this is the index used to retrieve the task(owner) info.
+ *
+ * @return  none
+ *
+ * @see     ASFTimerExpiry()
+ ***************************************************************************************************/
+void ASFTimerCallback(void const *argument)
+{
+    ASFTimerExpiry((uint32_t)argument);
+}
 
 /*-------------------------------------------------------------------------------------------------*\
  |    E N D   O F   F I L E
diff --git a/embedded/common/asf/asf_types.h b/embedded/common/asf/asf_types.h
index 669eaea..c03a141 100644
--- a/embedded/common/asf/asf_types.h
+++ b/embedded/common/asf/asf_types.h
@@ -22,7 +22,7 @@
  |    I N C L U D E   F I L E S
 \*-------------------------------------------------------------------------------------------------*/
 #include <stdint.h>
-#include "rtl.h"
+#include "cmsis_os.h"
 
 /*-------------------------------------------------------------------------------------------------*\
  |    C O N S T A N T S   &   M A C R O S
@@ -35,7 +35,8 @@
  * Task Handle type. This type is OS-dependent.
  */
 typedef struct AsfTaskHandleTag {
-    OS_TID   handle;
+    osThreadId  posThreadId;
+    osMailQId   posMailQId;
     void     *pStack;
     uint16_t stkSize;
 } AsfTaskHandle;
@@ -43,13 +44,12 @@ typedef struct AsfTaskHandleTag {
 /*
  * Semaphore ID type. This type is OS-dependent.
  */
-typedef OS_SEM* AsfSemIdType;
+#define AsfSemIdType(sem) osSemaphoreDef(sem)
 
 /*
  * Timer ID type. This type is OS-dependent.
  */
-typedef OS_ID TimerId;
-
+typedef osTimerId TimerId;
 
 /*-------------------------------------------------------------------------------------------------*\
  |    E X T E R N A L   V A R I A B L E S   &   F U N C T I O N S
diff --git a/embedded/common/modules/sensor-drivers/acc_bmc150_i2c.c b/embedded/common/modules/sensor-drivers/acc_bmc150_i2c.c
index aa674c6..a79ff64 100644
--- a/embedded/common/modules/sensor-drivers/acc_bmc150_i2c.c
+++ b/embedded/common/modules/sensor-drivers/acc_bmc150_i2c.c
@@ -43,7 +43,7 @@ extern uint32_t AccelTimeExtend;
 /*-------------------------------------------------------------------------------------------------*\
  |    P R I V A T E   C O N S T A N T S   &   M A C R O S
 \*-------------------------------------------------------------------------------------------------*/
-#define delay_ms(msec)      os_dly_wait(MSEC_TO_TICS(msec))
+#define delay_ms(msec)      osDelay(msec))
 #define PORT_ACCIRQREQ                  4
 #define PIN_ACCIRQREQ                   11
 #define ACCEL_INT_IRQCh                 PIN_INT0_IRQn
diff --git a/embedded/common/modules/sensor-drivers/acc_lsm303dlhc_i2c.c b/embedded/common/modules/sensor-drivers/acc_lsm303dlhc_i2c.c
index 4f5ce9e..fb767e6 100644
--- a/embedded/common/modules/sensor-drivers/acc_lsm303dlhc_i2c.c
+++ b/embedded/common/modules/sensor-drivers/acc_lsm303dlhc_i2c.c
@@ -40,7 +40,7 @@
 /*-------------------------------------------------------------------------------------------------*\
  |    P R I V A T E   C O N S T A N T S   &   M A C R O S
 \*-------------------------------------------------------------------------------------------------*/
-#define delay_ms(msec)      os_dly_wait(MSEC_TO_TICS(msec))
+#define delay_ms(msec)      osDelay(msec)
 
 /*-------------------------------------------------------------------------------------------------*\
  |    P R I V A T E   T Y P E   D E F I N I T I O N S
diff --git a/embedded/common/modules/sensor-drivers/gyro_bmg160_i2c.c b/embedded/common/modules/sensor-drivers/gyro_bmg160_i2c.c
index 1a5aaa6..7913e56 100644
--- a/embedded/common/modules/sensor-drivers/gyro_bmg160_i2c.c
+++ b/embedded/common/modules/sensor-drivers/gyro_bmg160_i2c.c
@@ -43,7 +43,7 @@ extern uint32_t GyroTimeExtend;
 /*-------------------------------------------------------------------------------------------------*\
  |    P R I V A T E   C O N S T A N T S   &   M A C R O S
 \*-------------------------------------------------------------------------------------------------*/
-#define delay_ms(msec)      os_dly_wait(MSEC_TO_TICS(msec))
+#define delay_ms(msec)      osDelay(msec))
 
 /*-------------------------------------------------------------------------------------------------*\
  |    P R I V A T E   T Y P E   D E F I N I T I O N S
diff --git a/embedded/common/modules/sensor-drivers/gyro_l3gd20_i2c.c b/embedded/common/modules/sensor-drivers/gyro_l3gd20_i2c.c
index 31496cb..a835244 100644
--- a/embedded/common/modules/sensor-drivers/gyro_l3gd20_i2c.c
+++ b/embedded/common/modules/sensor-drivers/gyro_l3gd20_i2c.c
@@ -38,7 +38,7 @@
 /*-------------------------------------------------------------------------------------------------*\
  |    P R I V A T E   C O N S T A N T S   &   M A C R O S
 \*-------------------------------------------------------------------------------------------------*/
-#define delay_ms(msec)      os_dly_wait(MSEC_TO_TICS(msec))
+#define delay_ms(msec)      osDelay(msec)
 
 /*-------------------------------------------------------------------------------------------------*\
  |    P R I V A T E   T Y P E   D E F I N I T I O N S
@@ -288,7 +288,7 @@ void Gyro_TriggerDataAcq( void )
  *          Waits until data is ready to be read
  *
  ***************************************************************************************************/
-void Gyro_WaitDataReady( U16 timeOut )
+void Gyro_WaitDataReady( uint16_t timeOut )
 {
     /* Not used in this manner */
 }
diff --git a/embedded/common/modules/sensor-drivers/mag_bmc150_i2c.c b/embedded/common/modules/sensor-drivers/mag_bmc150_i2c.c
index d7e6437..f63d080 100644
--- a/embedded/common/modules/sensor-drivers/mag_bmc150_i2c.c
+++ b/embedded/common/modules/sensor-drivers/mag_bmc150_i2c.c
@@ -43,7 +43,7 @@ extern uint32_t MagTimeExtend;
 /*-------------------------------------------------------------------------------------------------*\
  |    P R I V A T E   C O N S T A N T S   &   M A C R O S
 \*-------------------------------------------------------------------------------------------------*/
-#define delay_ms(msec)      os_dly_wait(MSEC_TO_TICS(msec))
+#define delay_ms(msec)      osDelay(msec))
 
 /*-------------------------------------------------------------------------------------------------*\
  |    P R I V A T E   T Y P E   D E F I N I T I O N S
diff --git a/embedded/common/modules/sensor-drivers/mag_lsm303dlhc_i2c.c b/embedded/common/modules/sensor-drivers/mag_lsm303dlhc_i2c.c
index d872edf..311ecc1 100644
--- a/embedded/common/modules/sensor-drivers/mag_lsm303dlhc_i2c.c
+++ b/embedded/common/modules/sensor-drivers/mag_lsm303dlhc_i2c.c
@@ -35,7 +35,7 @@ extern AsfTaskHandle ossTaskHandleTable[];
 /*-------------------------------------------------------------------------------------------------*\
  |    P R I V A T E   C O N S T A N T S   &   M A C R O S
 \*-------------------------------------------------------------------------------------------------*/
-#define delay_ms(msec)          os_dly_wait(MSEC_TO_TICS(msec))
+#define delay_ms(msec)      osDelay(msec))
 
 /*-------------------------------------------------------------------------------------------------*\
  |    P R I V A T E   T Y P E   D E F I N I T I O N S
diff --git a/embedded/projects/osp-lpc54102/Keil/RTE/CMSIS/RTX_Conf_CM.c b/embedded/projects/osp-lpc54102/Keil/RTE/CMSIS/RTX_Conf_CM.c
new file mode 100644
index 0000000..e64dc05
--- /dev/null
+++ b/embedded/projects/osp-lpc54102/Keil/RTE/CMSIS/RTX_Conf_CM.c
@@ -0,0 +1,312 @@
+/*----------------------------------------------------------------------------
+ *      CMSIS-RTOS  -  RTX
+ *----------------------------------------------------------------------------
+ *      Name:    RTX_Conf_CM.C
+ *      Purpose: Configuration of CMSIS RTX Kernel for Cortex-M
+ *      Rev.:    V4.70.1
+ *----------------------------------------------------------------------------
+ *
+ * Copyright (c) 1999-2009 KEIL, 2009-2015 ARM Germany GmbH
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *  - Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  - Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *  - Neither the name of ARM  nor the names of its contributors may be used 
+ *    to endorse or promote products derived from this software without 
+ *    specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *---------------------------------------------------------------------------*/
+ 
+#include "common.h"
+#include "asf_tqenum.h"
+
+/*----------------------------------------------------------------------------
+ *      RTX User configuration part BEGIN
+ *---------------------------------------------------------------------------*/
+ 
+//-------- <<< Use Configuration Wizard in Context Menu >>> -----------------
+//
+// <h>Thread Configuration
+// =======================
+//
+//   <o>Number of concurrent running user threads <1-250>
+//   <i> Defines max. number of user threads that will run at the same time.
+//   <i> Default: 6
+#ifndef OS_TASKCNT
+ #define OS_TASKCNT     6
+#endif
+ 
+//   <o>Default Thread stack size [bytes] <64-4096:8><#/4>
+//   <i> Defines default stack size for threads with osThreadDef stacksz = 0
+//   <i> Default: 200
+#ifndef OS_STKSIZE
+ #define OS_STKSIZE     1024      // this stack size value is in words
+#endif
+ 
+//   <o>Main Thread stack size [bytes] <64-32768:8><#/4>
+//   <i> Defines stack size for main thread.
+//   <i> Default: 200
+#ifndef OS_MAINSTKSIZE
+ #define OS_MAINSTKSIZE 200      // this stack size value is in words
+#endif
+ 
+//   <o>Number of threads with user-provided stack size <0-250>
+//   <i> Defines the number of threads with user-provided stack size.
+//   <i> Default: 0
+#ifndef OS_PRIVCNT
+ #define OS_PRIVCNT     NUMBER_OF_TASKS
+#endif
+ 
+//   <o>Total stack size [bytes] for threads with user-provided stack size <0-1048576:8><#/4>
+//   <i> Defines the combined stack size for threads with user-provided stack size.
+//   <i> Default: 0
+#ifndef OS_PRIVSTKSIZE
+ #define OS_PRIVSTKSIZE NUMBER_OF_TASKS * 800       // this stack size value is in words
+#endif
+ 
+//   <q>Stack overflow checking
+//   <i> Enable stack overflow checks at thread switch.
+//   <i> Enabling this option increases slightly the execution time of a thread switch.
+#ifndef OS_STKCHECK
+ #define OS_STKCHECK    0
+#endif
+ 
+//   <q>Stack usage watermark
+//   <i> Initialize thread stack with watermark pattern for analyzing stack usage (current/maximum) in System and Thread Viewer.
+//   <i> Enabling this option increases significantly the execution time of osThreadCreate.
+#ifndef OS_STKINIT
+#define OS_STKINIT      0
+#endif
+ 
+//   <o>Processor mode for thread execution 
+//     <0=> Unprivileged mode 
+//     <1=> Privileged mode
+//   <i> Default: Privileged mode
+#ifndef OS_RUNPRIV
+ #define OS_RUNPRIV     1
+#endif
+ 
+// </h>
+ 
+// <h>RTX Kernel Timer Tick Configuration
+// ======================================
+//   <q> Use Cortex-M SysTick timer as RTX Kernel Timer
+//   <i> Cortex-M processors provide in most cases a SysTick timer that can be used as 
+//   <i> as time-base for RTX.
+#ifndef OS_SYSTICK
+ #define OS_SYSTICK     1
+#endif
+//
+//   <o>RTOS Kernel Timer input clock frequency [Hz] <1-1000000000>
+//   <i> Defines the input frequency of the RTOS Kernel Timer.  
+//   <i> When the Cortex-M SysTick timer is used, the input clock 
+//   <i> is on most systems identical with the core clock.
+#ifndef OS_CLOCK
+ #define OS_CLOCK       12000000
+#endif
+ 
+//   <o>RTX Timer tick interval value [us] <1-1000000>
+//   <i> The RTX Timer tick interval value is used to calculate timeout values.
+//   <i> When the Cortex-M SysTick timer is enabled, the value also configures the SysTick timer.
+//   <i> Default: 1000  (1ms)
+#ifndef OS_TICK
+ #define OS_TICK        1000
+#endif
+ 
+// </h>
+ 
+// <h>System Configuration
+// =======================
+//
+// <e>Round-Robin Thread switching
+// ===============================
+//
+// <i> Enables Round-Robin Thread switching.
+#ifndef OS_ROBIN
+ #define OS_ROBIN       0
+#endif
+ 
+//   <o>Round-Robin Timeout [ticks] <1-1000>
+//   <i> Defines how long a thread will execute before a thread switch.
+//   <i> Default: 5
+#ifndef OS_ROBINTOUT
+ #define OS_ROBINTOUT   5
+#endif
+ 
+// </e>
+ 
+// <e>User Timers
+// ==============
+//   <i> Enables user Timers
+#ifndef OS_TIMERS
+ #define OS_TIMERS      1
+#endif
+ 
+//   <o>Timer Thread Priority
+//                        <1=> Low
+//     <2=> Below Normal  <3=> Normal  <4=> Above Normal
+//                        <5=> High
+//                        <6=> Realtime (highest)
+//   <i> Defines priority for Timer Thread
+//   <i> Default: High
+#ifndef OS_TIMERPRIO
+ #define OS_TIMERPRIO   4
+#endif
+ 
+//   <o>Timer Thread stack size [bytes] <64-4096:8><#/4>
+//   <i> Defines stack size for Timer thread.
+//   <i> Default: 200
+#ifndef OS_TIMERSTKSZ
+ #define OS_TIMERSTKSZ  50     // this stack size value is in words
+#endif
+ 
+//   <o>Timer Callback Queue size <1-32>
+//   <i> Number of concurrent active timer callback functions.
+//   <i> Default: 4
+#ifndef OS_TIMERCBQS
+ #define OS_TIMERCBQS   4
+#endif
+ 
+// </e>
+ 
+//   <o>ISR FIFO Queue size<4=>   4 entries  <8=>   8 entries
+//                         <12=> 12 entries  <16=> 16 entries
+//                         <24=> 24 entries  <32=> 32 entries
+//                         <48=> 48 entries  <64=> 64 entries
+//                         <96=> 96 entries
+//   <i> ISR functions store requests to this buffer,
+//   <i> when they are called from the interrupt handler.
+//   <i> Default: 16 entries
+#ifndef OS_FIFOSZ
+ #define OS_FIFOSZ      16
+#endif
+ 
+// </h>
+ 
+//------------- <<< end of configuration section >>> -----------------------
+ 
+// Standard library system mutexes
+// ===============================
+//  Define max. number system mutexes that are used to protect 
+//  the arm standard runtime library. For microlib they are not used.
+#ifndef OS_MUTEXCNT
+ #define OS_MUTEXCNT    8
+#endif
+ 
+/*----------------------------------------------------------------------------
+ *      RTX User configuration part END
+ *---------------------------------------------------------------------------*/
+ 
+#define OS_TRV          ((uint32_t)(((double)OS_CLOCK*(double)OS_TICK)/1E6)-1)
+ 
+
+/*----------------------------------------------------------------------------
+ *      Global Functions
+ *---------------------------------------------------------------------------*/
+ 
+/*--------------------------- os_idle_demon ---------------------------------*/
+
+/// \brief The idle demon is running when no other thread is ready to run
+void os_idle_demon (void) {
+ 
+  for (;;) {
+    /* HERE: include optional user code to be executed when no thread runs.*/
+  }
+}
+ 
+#if (OS_SYSTICK == 0)   // Functions for alternative timer as RTX kernel timer
+ 
+/*--------------------------- os_tick_init ----------------------------------*/
+ 
+/// \brief Initializes an alternative hardware timer as RTX kernel timer
+/// \return                             IRQ number of the alternative hardware timer
+int os_tick_init (void) {
+  return (-1);  /* Return IRQ number of timer (0..239) */
+}
+ 
+/*--------------------------- os_tick_val -----------------------------------*/
+ 
+/// \brief Get alternative hardware timer's current value (0 .. OS_TRV)
+/// \return                             Current value of the alternative hardware timer
+uint32_t os_tick_val (void) {
+  return (0);
+}
+ 
+/*--------------------------- os_tick_ovf -----------------------------------*/
+ 
+/// \brief Get alternative hardware timer's  overflow flag
+/// \return                             Overflow flag\n
+///                                     - 1 : overflow
+///                                     - 0 : no overflow
+uint32_t os_tick_ovf (void) {
+  return (0);
+}
+ 
+/*--------------------------- os_tick_irqack --------------------------------*/
+ 
+/// \brief Acknowledge alternative hardware timer interrupt
+void os_tick_irqack (void) {
+  /* ... */
+}
+ 
+#endif   // (OS_SYSTICK == 0)
+ 
+/*--------------------------- os_error --------------------------------------*/
+ 
+/* OS Error Codes */
+#define OS_ERROR_STACK_OVF      1
+#define OS_ERROR_FIFO_OVF       2
+#define OS_ERROR_MBX_OVF        3
+#define OS_ERROR_TIMER_OVF      4
+ 
+extern osThreadId svcThreadGetId (void);
+ 
+/// \brief Called when a runtime error is detected
+/// \param[in]   error_code   actual error code that has been detected
+void os_error (uint32_t error_code) {
+ 
+  /* HERE: include optional code to be executed on runtime error. */
+  switch (error_code) {
+    case OS_ERROR_STACK_OVF:
+      /* Stack overflow detected for the currently running task. */
+      /* Thread can be identified by calling svcThreadGetId().   */
+      break;
+    case OS_ERROR_FIFO_OVF:
+      /* ISR FIFO Queue buffer overflow detected. */
+      break;
+    case OS_ERROR_MBX_OVF:
+      /* Mailbox overflow detected. */
+      break;
+    case OS_ERROR_TIMER_OVF:
+      /* User Timer Callback Queue overflow detected. */
+      break;
+  }
+  printf("Error Code: %d\n", error_code);
+  for (;;);
+}
+ 
+
+/*----------------------------------------------------------------------------
+ *      RTX Configuration Functions
+ *---------------------------------------------------------------------------*/
+ 
+#include "RTX_CM_lib.h"
+ 
+/*----------------------------------------------------------------------------
+ * end of file
+ *---------------------------------------------------------------------------*/
diff --git a/embedded/projects/osp-lpc54102/Keil/RTE/RTE_Components.h b/embedded/projects/osp-lpc54102/Keil/RTE/RTE_Components.h
new file mode 100644
index 0000000..12a400e
--- /dev/null
+++ b/embedded/projects/osp-lpc54102/Keil/RTE/RTE_Components.h
@@ -0,0 +1,16 @@
+
+/*
+ * Auto generated Run-Time-Environment Component Configuration File
+ *      *** Do not modify ! ***
+ *
+ * Project: 'osp-lpc54102' 
+ * Target:  'OSP-Reference' 
+ */
+
+#ifndef RTE_COMPONENTS_H
+#define RTE_COMPONENTS_H
+
+#define RTE_CMSIS_RTOS                  /* CMSIS-RTOS */
+        #define RTE_CMSIS_RTOS_RTX              /* CMSIS-RTOS Keil RTX */
+
+#endif /* RTE_COMPONENTS_H */
diff --git a/embedded/projects/osp-lpc54102/Keil/osp-lpc54102.uvoptx b/embedded/projects/osp-lpc54102/Keil/osp-lpc54102.uvoptx
index f1d53e8..d17778c 100644
--- a/embedded/projects/osp-lpc54102/Keil/osp-lpc54102.uvoptx
+++ b/embedded/projects/osp-lpc54102/Keil/osp-lpc54102.uvoptx
@@ -144,7 +144,7 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>UL2CM3</Key>
-          <Name>-UV0202LFE -O206 -S0 -C0 -P00 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD2000000 -FC1000 -FN1 -FF0LPC54xxx_512.FLM -FS00 -FL080000 -FP0($$Device:LPC54102$Flash\LPC54xxx_512.FLM)</Name>
+          <Name>-UV0891UAE -O206 -S0 -C0 -P00 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD2000000 -FC1000 -FN1 -FF0LPC54xxx_512.FLM -FS00 -FL080000 -FP0($$Device:LPC54102$Flash\LPC54xxx_512.FLM)</Name>
         </SetRegEntry>
       </TargetDriverDllRegistry>
       <Breakpoint/>
@@ -171,7 +171,7 @@
       <DebugFlag>
         <trace>0</trace>
         <periodic>0</periodic>
-        <aLwin>0</aLwin>
+        <aLwin>1</aLwin>
         <aCover>0</aCover>
         <aSer1>0</aSer1>
         <aSer2>0</aSer2>
@@ -209,7 +209,7 @@
 
   <Group>
     <GroupName>ASF</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -217,7 +217,7 @@
       <GroupNumber>2</GroupNumber>
       <FileNumber>1</FileNumber>
       <FileType>1</FileType>
-      <tvExp>1</tvExp>
+      <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
@@ -726,4 +726,12 @@
     </File>
   </Group>
 
+  <Group>
+    <GroupName>::CMSIS</GroupName>
+    <tvExp>0</tvExp>
+    <tvExpOptDlg>0</tvExpOptDlg>
+    <cbSel>0</cbSel>
+    <RteFlg>1</RteFlg>
+  </Group>
+
 </ProjectOpt>
diff --git a/embedded/projects/osp-lpc54102/Keil/osp-lpc54102.uvprojx b/embedded/projects/osp-lpc54102/Keil/osp-lpc54102.uvprojx
index 66386c9..ab37608 100644
--- a/embedded/projects/osp-lpc54102/Keil/osp-lpc54102.uvprojx
+++ b/embedded/projects/osp-lpc54102/Keil/osp-lpc54102.uvprojx
@@ -175,7 +175,7 @@
           </Flash1>
           <bUseTDR>1</bUseTDR>
           <Flash2>BIN\UL2CM3.DLL</Flash2>
-          <Flash3></Flash3>
+          <Flash3>"" ()</Flash3>
           <Flash4></Flash4>
           <pFcarmOut></pFcarmOut>
           <pFcarmGrp></pFcarmGrp>
@@ -206,11 +206,11 @@
             <AdsLsym>1</AdsLsym>
             <AdsLszi>1</AdsLszi>
             <AdsLtoi>1</AdsLtoi>
-            <AdsLsun>1</AdsLsun>
+            <AdsLsun>0</AdsLsun>
             <AdsLven>1</AdsLven>
-            <AdsLsxf>1</AdsLsxf>
+            <AdsLsxf>0</AdsLsxf>
             <RvctClst>1</RvctClst>
-            <GenPPlst>1</GenPPlst>
+            <GenPPlst>0</GenPPlst>
             <AdsCpuType>"Cortex-M4"</AdsCpuType>
             <RvctDeviceName></RvctDeviceName>
             <mOS>1</mOS>
@@ -347,7 +347,7 @@
           </ArmAdsMisc>
           <Cads>
             <interw>1</interw>
-            <Optim>3</Optim>
+            <Optim>1</Optim>
             <oTime>0</oTime>
             <SplitLS>0</SplitLS>
             <OneElfS>1</OneElfS>
@@ -437,7 +437,7 @@
               <RVCTZI>0</RVCTZI>
               <RVCTOtherData>0</RVCTOtherData>
               <ModuleSelection>0</ModuleSelection>
-              <IncludeInBuild>2</IncludeInBuild>
+              <IncludeInBuild>0</IncludeInBuild>
               <AlwaysBuild>2</AlwaysBuild>
               <GenerateAssemblyFile>2</GenerateAssemblyFile>
               <AssembleAssemblyFile>2</AssembleAssemblyFile>
@@ -1025,8 +1025,106 @@
             </File>
           </Files>
         </Group>
+        <Group>
+          <GroupName>::CMSIS</GroupName>
+          <GroupOption>
+            <CommonProperty>
+              <UseCPPCompiler>0</UseCPPCompiler>
+              <RVCTCodeConst>0</RVCTCodeConst>
+              <RVCTZI>0</RVCTZI>
+              <RVCTOtherData>0</RVCTOtherData>
+              <ModuleSelection>0</ModuleSelection>
+              <IncludeInBuild>1</IncludeInBuild>
+              <AlwaysBuild>2</AlwaysBuild>
+              <GenerateAssemblyFile>2</GenerateAssemblyFile>
+              <AssembleAssemblyFile>2</AssembleAssemblyFile>
+              <PublicsOnly>2</PublicsOnly>
+              <StopOnExitCode>11</StopOnExitCode>
+              <CustomArgument></CustomArgument>
+              <IncludeLibraryModules></IncludeLibraryModules>
+              <ComprImg>1</ComprImg>
+            </CommonProperty>
+            <GroupArmAds>
+              <Cads>
+                <interw>2</interw>
+                <Optim>0</Optim>
+                <oTime>2</oTime>
+                <SplitLS>2</SplitLS>
+                <OneElfS>2</OneElfS>
+                <Strict>2</Strict>
+                <EnumInt>2</EnumInt>
+                <PlainCh>2</PlainCh>
+                <Ropi>2</Ropi>
+                <Rwpi>2</Rwpi>
+                <wLevel>2</wLevel>
+                <uThumb>2</uThumb>
+                <uSurpInc>2</uSurpInc>
+                <uC99>2</uC99>
+                <useXO>2</useXO>
+                <VariousControls>
+                  <MiscControls></MiscControls>
+                  <Define></Define>
+                  <Undefine></Undefine>
+                  <IncludePath></IncludePath>
+                </VariousControls>
+              </Cads>
+              <Aads>
+                <interw>2</interw>
+                <Ropi>2</Ropi>
+                <Rwpi>2</Rwpi>
+                <thumb>2</thumb>
+                <SplitLS>2</SplitLS>
+                <SwStkChk>2</SwStkChk>
+                <NoWarn>2</NoWarn>
+                <uSurpInc>2</uSurpInc>
+                <useXO>2</useXO>
+                <VariousControls>
+                  <MiscControls></MiscControls>
+                  <Define></Define>
+                  <Undefine></Undefine>
+                  <IncludePath></IncludePath>
+                </VariousControls>
+              </Aads>
+            </GroupArmAds>
+          </GroupOption>
+        </Group>
       </Groups>
     </Target>
   </Targets>
 
+  <RTE>
+    <apis>
+      <api Capiversion="1.0" Cclass="CMSIS" Cgroup="RTOS" exclusive="1">
+        <package name="CMSIS" schemaVersion="1.3" url="http://www.keil.com/pack/" vendor="ARM" version="4.3.0"/>
+        <targetInfos>
+          <targetInfo name="OSP-Reference"/>
+        </targetInfos>
+      </api>
+    </apis>
+    <components>
+      <component Cclass="CMSIS" Cgroup="CORE" Cvendor="ARM" Cversion="4.1.0" condition="CMSIS Core">
+        <package name="CMSIS" schemaVersion="1.3" url="http://www.keil.com/pack/" vendor="ARM" version="4.3.0"/>
+        <targetInfos>
+          <targetInfo name="OSP-Reference"/>
+        </targetInfos>
+      </component>
+      <component Cclass="CMSIS" Cgroup="RTOS" Csub="Keil RTX" Cvendor="ARM" Cversion="4.78.0" condition="Cortex-M Device Startup">
+        <package name="CMSIS" schemaVersion="1.3" url="http://www.keil.com/pack/" vendor="ARM" version="4.3.0"/>
+        <targetInfos>
+          <targetInfo name="OSP-Reference"/>
+        </targetInfos>
+      </component>
+    </components>
+    <files>
+      <file attr="config" category="source" name="CMSIS\RTOS\RTX\Templates\RTX_Conf_CM.c" version="4.70.1">
+        <instance index="0">RTE\CMSIS\RTX_Conf_CM.c</instance>
+        <component Cclass="CMSIS" Cgroup="RTOS" Csub="Keil RTX" Cvendor="ARM" Cversion="4.78.0" condition="Cortex-M Device Startup"/>
+        <package name="CMSIS" schemaVersion="1.3" url="http://www.keil.com/pack/" vendor="ARM" version="4.3.0"/>
+        <targetInfos>
+          <targetInfo name="OSP-Reference"/>
+        </targetInfos>
+      </file>
+    </files>
+  </RTE>
+
 </Project>
diff --git a/embedded/projects/osp-lpc54102/sources/app/app_tasks.h b/embedded/projects/osp-lpc54102/sources/app/app_tasks.h
index 04cbae9..1819025 100644
--- a/embedded/projects/osp-lpc54102/sources/app/app_tasks.h
+++ b/embedded/projects/osp-lpc54102/sources/app/app_tasks.h
@@ -53,10 +53,10 @@
 /* NOTE: STACK_INCREASE can be used to increase the stack size of all tasks by a constant amount.
    This value is set in ASF_TaskInit.c file and is normally 0. Use this for Debugging crashes */
 #ifdef ANDROID_DEMO
-ASF_TASK_STATIC( I2CSLAVE_COMM_TASK_ID, I2CCommTask,        150,  (0x1080+STACK_INCREASE),   64 )
+ASF_TASK_STATIC( I2CSLAVE_COMM_TASK_ID, I2CCommTask,        osPriorityNormal,  (0x200+STACK_INCREASE),   64 )
 #endif
-ASF_TASK_STATIC( ALGORITHM_TASK_ID,     AlgorithmTask,      90,  (0x1080+STACK_INCREASE),   64 )
-ASF_TASK_STATIC( ALG_BG_TASK_ID,        AlgBackGndTask,     85,  (0x1080+STACK_INCREASE),   64 )
+ASF_TASK_STATIC( ALGORITHM_TASK_ID,     AlgorithmTask,      osPriorityNormal,  (0x1000+STACK_INCREASE),   64 )
+ASF_TASK_STATIC( ALG_BG_TASK_ID,        AlgBackGndTask,     osPriorityLow,  (0x200+STACK_INCREASE),   64 )
 
 /*-------------------------------------------------------------------------------------------------*\
  |    E N D   O F   F I L E
diff --git a/embedded/projects/osp-lpc54102/sources/app/i2c_slavecomm_t.c b/embedded/projects/osp-lpc54102/sources/app/i2c_slavecomm_t.c
index f01a1e1..ae85a3c 100644
--- a/embedded/projects/osp-lpc54102/sources/app/i2c_slavecomm_t.c
+++ b/embedded/projects/osp-lpc54102/sources/app/i2c_slavecomm_t.c
@@ -717,6 +717,7 @@ ASF_TASK void I2CCommTask(ASF_TASK_ARG)
 			D1_printf("I2C:!!!UNHANDLED MESSAGE:%d!!!\r\n", rcvMsg->msgId);
 			break;
         }
+        ASFDeleteMessage(I2CSLAVE_COMM_TASK_ID, &rcvMsg );
     }
 }
 #endif //ANDROID_COMM_TASK
diff --git a/embedded/projects/osp-lpc54102/sources/app/main.c b/embedded/projects/osp-lpc54102/sources/app/main.c
index 53aa89b..477e06d 100644
--- a/embedded/projects/osp-lpc54102/sources/app/main.c
+++ b/embedded/projects/osp-lpc54102/sources/app/main.c
@@ -730,8 +730,6 @@ void HardFault_Handler(uint32_t stack[])
 int main(void)
 { 
     
-    uint32_t timer = 0;
-    
     /* Update core clock variables */
     SystemCoreClockUpdate();
     
@@ -762,6 +760,24 @@ int main(void)
 	ASF_assert_fatal(false);
 }
 
+#ifdef __MICROLIB
+/***********************************************************************
+ * @fn      exit
+ *          Main Exit point from the application firmware
+ *          This function is required if microlib is used
+ *
+ * @param   Error code
+ *
+ * @return  none
+ *
+ ***********************************************************************/
+void exit(uint32_t error)
+{
+    __ASM volatile("BKPT #01");
+    while(1);
+}
+#endif
+
 /*------------------------------------------------------------------*\
  |    E N D   O F   F I L E
 \*------------------------------------------------------------------*/
diff --git a/embedded/projects/osp-lpc54102/sources/app/main.h b/embedded/projects/osp-lpc54102/sources/app/main.h
index 969953f..86f1df7 100644
--- a/embedded/projects/osp-lpc54102/sources/app/main.h
+++ b/embedded/projects/osp-lpc54102/sources/app/main.h
@@ -54,7 +54,8 @@
 #if 0
 #define MAX_SYSTEM_MESSAGES                     1200     ///< Max number of queued messages in the system
 #else
-#define MAX_SYSTEM_MESSAGES                     600    // QLY change from 600 to 300    ///< Max number of queued messages in the system
+/* PG: Reduced the number of queued messages to 200 to prevent memory wastage. */
+#define MAX_SYSTEM_MESSAGES                     200    // QLY change from 600 to 300    ///< Max number of queued messages in the system
 #endif
 /* All timer references (arbitrary unique identifiers for each timer)*/
 #define TIMER_REF_RTC_UPDATE                    0x55A5
@@ -96,7 +97,7 @@
 /* Sensor acquisition related definitions */
 
 #if !defined INTERRUPT_BASED_SAMPLING
-# define SENSOR_SAMPLE_PERIOD           MSEC_TO_TICS(20)   //tick
+# define SENSOR_SAMPLE_PERIOD           (20)   //time in ms
 # define MAG_DECIMATE_FACTOR            1
 # define ACCEL_SAMPLE_DECIMATE          1
 # define GYRO_SAMPLE_DECIMATE           1
@@ -106,7 +107,7 @@
 # define GYRO_SAMPLE_DECIMATE           1
 #endif
 
-#define	PRESSURE_SAMPLE_PERIOD		MSEC_TO_TICS(40)
+#define	PRESSURE_SAMPLE_PERIOD		(40)
 
 #ifdef TRIGGERED_MAG_SAMPLING
 # define MAG_TRIGGER_RATE_DECIMATE      1 //1/2 of Accel ODR
diff --git a/embedded/projects/osp-lpc54102/sources/boardsupport/sensacq_i2c.c b/embedded/projects/osp-lpc54102/sources/boardsupport/sensacq_i2c.c
index 8d27f05..2c3444b 100644
--- a/embedded/projects/osp-lpc54102/sources/boardsupport/sensacq_i2c.c
+++ b/embedded/projects/osp-lpc54102/sources/boardsupport/sensacq_i2c.c
@@ -271,7 +271,7 @@ void dev_i2c_delay(unsigned int msec)
 	/* Will WFI for the entire sleep duration. If an interrupt
 	   occurs that wakes the device, the sleep handler will
 	   automatically re-enter WFI until the duration has expired. */
-	os_dly_wait(MSEC_TO_TICS(msec)); /* Allow startup time for sensors */
+	osDelay(msec); /* Allow startup time for sensors */
 }
 
 /**
diff --git a/embedded/projects/osp-lpc54102/sources/lpcopen/applications/lpc5410x/startup/keil_startup_lpc5410x.s b/embedded/projects/osp-lpc54102/sources/lpcopen/applications/lpc5410x/startup/keil_startup_lpc5410x.s
index 942981f..ca75a9e 100644
--- a/embedded/projects/osp-lpc54102/sources/lpcopen/applications/lpc5410x/startup/keil_startup_lpc5410x.s
+++ b/embedded/projects/osp-lpc54102/sources/lpcopen/applications/lpc5410x/startup/keil_startup_lpc5410x.s
@@ -35,7 +35,7 @@
 
 ; original stack size was set to 0x200
 				EXPORT  Stack_Size
-Stack_Size      EQU     0x00000400
+Stack_Size      EQU     0x00000200
 	
 				EXPORT  Stack_Mem
                 AREA    STACK, NOINIT, READWRITE, ALIGN=3
@@ -48,7 +48,7 @@ __initial_sp
 ; </h>
 
 ; Original heap size was set to 0x100
-Heap_Size       EQU     0x00006000
+Heap_Size       EQU     0x100
 
                 AREA    HEAP, NOINIT, READWRITE, ALIGN=3
 __heap_base