Index: firmware/App/Controllers/BloodLeak.c
===================================================================
diff -u -r66dda999c5736ecec046197ff1c843c01f6aecc5 -r242ad36c7a1ec4ee5012c9f009899f9e0bd87628
--- firmware/App/Controllers/BloodLeak.c	(.../BloodLeak.c)	(revision 66dda999c5736ecec046197ff1c843c01f6aecc5)
+++ firmware/App/Controllers/BloodLeak.c	(.../BloodLeak.c)	(revision 242ad36c7a1ec4ee5012c9f009899f9e0bd87628)
@@ -26,6 +26,7 @@
 #include "SystemCommMessages.h"
 #include "TaskPriority.h"
 #include "Timers.h"
+#include "Utilities.h"
 
 /**
  *  @addtogroup BloodLeak
@@ -79,7 +80,7 @@
 #define BLOOD_LEAK_EMB_MODE_STOP_WRITE_INDEX  3                                                  ///< Blood leak embedded mode stop write to FIFO index.
 #define BLOOD_LEAK_EMB_MODE_ACTIVE_HIGH_INDEX 4                                                  ///< Blood leak embedded mode active high command index.
 #define BLOOD_LEAK_EMB_MODE_REQUEST_RX_INDEX  0                                                  ///< Blood leak embedded mode request Rx to read index.
-#define BLOOD_LEAK_EMB_MODE_CMD_Q_MAX_SIZE    10                                                 ///< Blood leak embedded mode command queue maximum size.
+#define BLOOD_LEAK_EMB_MODE_CMD_Q_MAX_SIZE    12                                                 ///< Blood leak embedded mode command queue maximum size.
 #define BLOOD_LEAK_EMB_MODE_ZERO_CMD_RQRD_Q   5                                                  ///< Blood leak embedded mode zero command required queue count.
 #define BLOOD_LEAK_EMB_MODE_NUM_OF_RETRIES    3                                                  ///< Blood leak embedded mode number of retries to enqueue.
 
@@ -265,6 +266,13 @@
         getNVRecord2Driver( GET_CAL_BLOOD_LEAK_SENSOR, (U08*)&bloodLeakCalRecord, length, 0, ALARM_ID_HD_BLOOD_LEAK_INVALID_CAL_RECORD );
     }
 
+    if ( bloodLeakState >= BLOOD_LEAK_INIT_STATE )
+    {
+        enqueueEmbModeCmd( I_EMB_MODE_CMD );
+        enqueueEmbModeCmd( V_EMB_MODE_CMD );
+        enqueueEmbModeCmd( D_EMB_MODE_CMD );
+    }
+
     switch( bloodLeakState )
     {
         case BLOOD_LEAK_WAIT_FOR_POST_STATE:
@@ -771,6 +779,10 @@
                 // Clear the response buffer to able to receive fresh data
                 memset( bloodLeakEmbModeRespBuffer, 0x0, BLOOD_LEAK_EMB_MODE_RESP_BUFFER_LEN );
                 break;
+
+            default:
+                SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_BLOOD_LEAK_INVALID_EMB_MODE_CMD_SELECTED, bloodLeakEmbModeRqstedCmd )
+                break;
         }
     }
 
@@ -830,6 +842,10 @@
             }
             bloodLeakUARTCmdIndex++;
             break;
+
+        default:
+            SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_BLOOD_LEAK_INVALID_EMB_MODE_CMD_SELECTED, bloodLeakEmbModeRqstedCmd )
+            break;
     }
 
     if ( ( bloodLeakEmbModeRqstedCmd != CS_EMB_MODE_CMD ) && ( bloodLeakUARTCmdIndex - 1 > BLOOD_LEAK_EMB_MODE_RESET_INDEX ) &&
@@ -1186,16 +1202,11 @@
     {
         BLOOD_LEAK_DATA_T data;
 
-        data.bloodLeakStatus            = (U32)getBloodLeakStatus();
-        data.bloodLeakState             = (U32)bloodLeakState;
-        data.bloodLeakZeroStatusCounter = (U32)getFPGABloodLeakZeroStatusCounter();
-        data.bloodLeakCounter           = (U32)getFPGABloodLeakCounter();
-        data.bloodLeakZeroedStatus      = (U32)getFPGABloodLeakZeroedStatus();
-        data.bloodLeakDetectSetPoint    = (U32)getFPGABloodLeakDetectSetPoint();
-        data.bloodLeakDetectLevel       = (U32)getFPGABloodLeakDetectLevel();
-        data.bloodLeakStCount           = (U32)getFPGABloodLeakStCount();
-        data.bloodLeakLEDIntesity       = (U32)getFPGABloodLeakLEDIntensity();
-        data.bloodLeakRegisterCounter   = (U32)getFPGABloodLeakRegisterCounter();
+        data.bloodLeakStatus         = (U32)getBloodLeakStatus();
+        data.bloodLeakState          = (U32)bloodLeakState;
+        data.bloodLeakDetectSetPoint = bloodLeakEmbModeCmd[ D_EMB_MODE_CMD ].commandResp;
+        data.bloodLeakDetectLevel    = bloodLeakEmbModeCmd[ V_EMB_MODE_CMD ].commandResp;
+        data.bloodLeakLEDIntesity    = bloodLeakEmbModeCmd[ I_EMB_MODE_CMD ].commandResp;
 
         broadcastData( MSG_ID_HD_BLOOD_LEAK_DATA, COMM_BUFFER_OUT_CAN_HD_BROADCAST, (U08*)&data, sizeof( BLOOD_LEAK_DATA_T ) );
         bloodLeakDataPublicationTimerCounter = 0;