Index: firmware/App/Controllers/BloodLeak.c
===================================================================
diff -u -r050984357442e3ee0d6d5b21e274c1306643c598 -r6e86723c766c0097c9867af984c0c7e82802537a
--- firmware/App/Controllers/BloodLeak.c	(.../BloodLeak.c)	(revision 050984357442e3ee0d6d5b21e274c1306643c598)
+++ firmware/App/Controllers/BloodLeak.c	(.../BloodLeak.c)	(revision 6e86723c766c0097c9867af984c0c7e82802537a)
@@ -984,9 +984,9 @@
 
                     bloodLeakEmbModeRespIndex++;
                 }
-                // Check if the buffer index is > 0 so the first char has been inserted and the rest is data
-                // For instance, V has been inserted and XXX is inserted that is followed by V. So V123.
-                else if ( bloodLeakEmbModeRespIndex > 0 )
+                // Check if the buffer index is > 0 so the first char has been inserted and the rest is data but it is not the echo of the command
+                // For instance, V has been inserted and XXXX is inserted that is followed by V. So V123 and not VVV12.
+                else if ( ( bloodLeakEmbModeRespIndex > 0 ) && ( data != expChar1 ) )
                 {
                     bloodLeakEmbModeRespBuffer[ bloodLeakEmbModeRespIndex ] = data;
 
@@ -1047,8 +1047,6 @@
                 }
             }
         }
-
-        BOOL test = FALSE; // TODO for testing remove
     }
 
     // If wait for the receive FIFO has timed out or all there is no buffer left in the Rx FIFO transition back to wait for command state for the next command
@@ -1068,21 +1066,25 @@
 
             for ( i = 0; i < length; i++ )
             {
+                // Loop through the elements of the buffer and make sure none of the elements are not NULL.
+                // The elements are checked until the specified length that is expected for a command
                 isNull |= ( bloodLeakEmbModeRespBuffer[ i ] != NU ? FALSE : TRUE );
             }
 
             // Check if the failed command is Control S which is switch to embedded mode and if
             // it failed set the embedded mode request to false so the other commands cannot be sent again
             bloodLeakSignalEmbModeReq = ( ( CS == bloodLeakEmbModeRqstedCmd ) && ( NU == bloodLeakEmbModeRespBuffer[ 0 ] ) ? FALSE : TRUE );
 
-            if ( ( TRUE == isNull ) && ( bloodLeakEmbModeNumOfCmdTries < BLOOD_LEAK_EMB_MODE_MAX_NUM_CMD_TRIES ) )
+            if ( ( TRUE == isNull ) && ( ++bloodLeakEmbModeNumOfCmdTries < BLOOD_LEAK_EMB_MODE_MAX_NUM_CMD_TRIES ) )
             {
                 state = BLOOD_LEAK_CAL_SEND_COMMAND_STATE;
             }
             else
             {
                 bloodLeakEmbModeRqstedCmd = NU;
                 state                     = BLOOD_LEAK_CAL_WAIT_FOR_COMAND_STATE;
+
+                handleSendBloodLeakEmbeddedModeCommandResponse( length, bloodLeakEmbModeRespBuffer );
             }
         }
     }
@@ -1217,15 +1219,15 @@
 
     // Get self test command
     bloodLeakEmbModeCmd[ G ].commandASCII  = 71;      // ASCII for G
-    bloodLeakEmbModeCmd[ G ].expChar1      = NU;
+    bloodLeakEmbModeCmd[ G ].expChar1      = 71;
     bloodLeakEmbModeCmd[ G ].expChar2      = NU;
-    bloodLeakEmbModeCmd[ G ].length        = 4;
+    bloodLeakEmbModeCmd[ G ].length        = 5;
 
     // Intensity command
     bloodLeakEmbModeCmd[ I ].commandASCII  = 73;      // ASCII for I
-    bloodLeakEmbModeCmd[ I ].expChar1      = NU;
+    bloodLeakEmbModeCmd[ I ].expChar1      = 73;
     bloodLeakEmbModeCmd[ I ].expChar2      = NU;
-    bloodLeakEmbModeCmd[ I ].length        = 4;
+    bloodLeakEmbModeCmd[ I ].length        = 5;
 
     // Blood detection command
     bloodLeakEmbModeCmd[ V ].commandASCII  = 86;      // ASCII for V