Index: firmware/App/Services/SystemCommMessages.c
===================================================================
diff -u -r83b1a65b576994e1f8e1476629d8f1277cd67720 -rfd0e359b0ee4b3efdf25ac1fd619f85fffef67b7
--- firmware/App/Services/SystemCommMessages.c	(.../SystemCommMessages.c)	(revision 83b1a65b576994e1f8e1476629d8f1277cd67720)
+++ firmware/App/Services/SystemCommMessages.c	(.../SystemCommMessages.c)	(revision fd0e359b0ee4b3efdf25ac1fd619f85fffef67b7)
@@ -26,6 +26,7 @@
 #include "SystemCommMessages.h"
 #include "SystemComm.h"
 #include "CPLD.h"
+#include "OperationModes.h"
 
 // ********** private definitions **********
 
@@ -158,10 +159,26 @@
  *************************************************************************/
 void handleDGFillStartStopMessages( MESSAGE_T *message )
 {
+    # define STOP  0
+    # define START 1
+
     #ifdef RM46_EVAL_BOARD_TARGET
-    toggleUserLED();
+    //toggleUserLED();
     #endif
 
+    // If we start therapy
+    if (message->cargo[0] == START && getCurrentOperationMode() == MODE_STAN) {
+        requestNewOperationMode(MODE_FILL);
+        sendTestAckResponseMsg((MSG_ID_T)message->hdr.msgID, TRUE);
+    } // We can only stop fill if we are in fill mode
+    else if (message->cargo[0] == STOP && getCurrentOperationMode() == MODE_FILL) {
+        requestNewOperationMode(MODE_STAN);
+        sendTestAckResponseMsg((MSG_ID_T)message->hdr.msgID, TRUE);
+    } else {
+        // TODO: Print to log that either start was called when t he machine was not
+        // in standby or that stop was called when the machine was not in fill
+        sendTestAckResponseMsg((MSG_ID_T)message->hdr.msgID, FALSE);
+    }
 }
 
 
@@ -247,7 +264,7 @@
     msgSize = serializeMessage( msg, data );
 
     // add serialized message data to appropriate comm buffer
-    result = addToCommBuffer( COMM_BUFFER_OUT_UART_PC, data, msgSize );
+    result = addToCommBuffer( COMM_BUFFER_OUT_CAN_HD_2_DG, data, msgSize );
 
     return result;
 }