Index: firmware/App/Services/SystemCommMessages.c
===================================================================
diff -u -r0bce81180c0ba2b25f5d501ed6aa6b2f9a8b2500 -r2cb4ff0f8a0ad74826c5c9b4d0e84c6334c960d6
--- firmware/App/Services/SystemCommMessages.c	(.../SystemCommMessages.c)	(revision 0bce81180c0ba2b25f5d501ed6aa6b2f9a8b2500)
+++ firmware/App/Services/SystemCommMessages.c	(.../SystemCommMessages.c)	(revision 2cb4ff0f8a0ad74826c5c9b4d0e84c6334c960d6)
@@ -7,8 +7,8 @@
 *
 * @file    SystemCommMessages.c
 *
-* @author  (last)      Bill Bracken
-* @date    (last)      22-Aug-2022
+* @author  (last)      Michael Garthwaite
+* @date    (last)      07-Sep-2022
 *
 * @author  (original)  Dara Navaei
 * @date    (original)  05-Nov-2019
@@ -43,7 +43,10 @@
 #include "Valves.h"
 #include "WatchdogMgmt.h"
 #include "HDDefs.h"
+#include "TaskPriority.h"
 
+#include "TaskPriority.h"
+
 /**
  *  @addtogroup SystemCommMessages
  *  @{
@@ -62,9 +65,9 @@
 #pragma pack(pop)
 
 // ********** private data **********
-
 static BOOL testerLoggedIn = FALSE;                         ///< Flag indicates whether an external tester (connected PC) has sent a valid login message.
 static volatile U16 nextSeqNo = 1;                          ///< Value of sequence number to use for next transmitted message.
+
 /// List of message IDs that are requested not to be transmitted.
 static BLOCKED_MSGS_DATA_T blockedMessagesForXmit = { 0, 0, 0, 0, 0, 0, 0, 0 };
 
@@ -889,24 +892,26 @@
 
 /*********************************************************************//**
  * @brief
- * The handleSetHDStandbyDisinfectSubmode function handles setting the
+ * The handleSetHDStandbyDisinfectSubmodeRequest function handles setting the
  * standby submode to wait for disisnfect state.
- * @details Inputs: none
+ * @details Inputs: 1=initiate, 0=cancel
  * @details Outputs: message handled
  * @param message a pointer to the message to handle
  * @return none
  *************************************************************************/
 void handleSetHDStandbyDisinfectSubmodeRequest( MESSAGE_T *message )
 {
-    BOOL result = FALSE;
+    U32 cmd;
 
-    // The payload should be 0 in this case because there is mode in this command
-    if ( 0 == message->hdr.payloadLen )
+    if ( sizeof( U32 ) == message->hdr.payloadLen )
     {
-        signalInitiateStandbyDisinfectSubmode();
+        memcpy( &cmd, &message->payload[0], sizeof( U32 ) );
+        signalInitiateStandbyDisinfectSubmode( cmd );
     }
-
-    sendAckResponseMsg( (MSG_ID_T)message->hdr.msgID, COMM_BUFFER_OUT_CAN_HD_2_UI, result );
+    else
+    {
+        handleSetHDStandbyDisinfectSubmodeResponse( FALSE, REQUEST_REJECT_REASON_INVALID_REQUEST_FORMAT );
+    }
 }
 
 /*********************************************************************//**
@@ -2349,13 +2354,13 @@
     {
         TEMPERATURE_SENSORS_DATA_T payload;
 
-        memcpy( &payload, message->payload, sizeof( TEMPERATURE_SENSORS_DATA_T ) );
-        setDialysateTemperatureReadings( payload.inletDialysate, payload.outletRedundant );
-    }
-    // TODO - what to do if invalid payload length?
-    // TODO - how to know if DG stops sending these?
-}
-
+        memcpy( &payload, message->payload, sizeof( TEMPERATURE_SENSORS_DATA_T ) );
+        setDialysateTemperatureReadings( payload.inletDialysate, payload.outletRedundant );
+    }
+    // TODO - what to do if invalid payload length?
+    // TODO - how to know if DG stops sending these?
+}
+
 /*********************************************************************//**
  * @brief
  * The handleDialysateFlowData function handles dialysate flow data broadcast
@@ -5148,6 +5153,7 @@
     if ( sizeof( TEST_OVERRIDE_PAYLOAD_T ) == message->hdr.payloadLen )
     {
         memcpy( &payload, message->payload, sizeof( TEST_OVERRIDE_PAYLOAD_T ) );
+
         if ( FALSE == payload.reset )
         {
             result = testSetBatteryRemainingPercentOverride( payload.state.u32 );
@@ -7023,15 +7029,26 @@
 {
     BOOL status = FALSE;
     HD_OP_MODE_T currentMode = getCurrentOperationMode();
+    DG_OP_MODE_T currentDGMode = getDGOpMode();
     REQUEST_REJECT_REASON_CODE_T reject;
 
     if ( 0 == message->hdr.payloadLen )
     {
         if ( ( MODE_STAN == currentMode ) || ( MODE_FAUL == currentMode ) )
         {
-            status = TRUE;
+
             requestNewOperationMode( MODE_SERV );
-            reject = REQUEST_REJECT_REASON_NONE;
+
+            if ( (DG_MODE_STAN == currentDGMode) || (DG_MODE_FAUL == currentDGMode) )
+            {
+                status = TRUE;
+                cmdSetDGToServiceMode();
+                reject = REQUEST_REJECT_REASON_NONE;
+            }
+            else
+            {
+                reject = REQUEST_REJECT_REASON_DG_NOT_IN_STANDBY_IDLE_STATE;
+            }
         }
         else
         {
@@ -7399,4 +7416,28 @@
     sendTestAckResponseMsg( (MSG_ID_T)message->hdr.msgID, result );
 }
 
+/*********************************************************************//**
+ * @brief
+ * The sendDGServiceModeRequest function constructs a service mode request msg
+ * to the DG and queues the msg for transmit on the appropriate CAN channel.
+ * @details Inputs: none
+ * @details Outputs: DG Service mode request msg constructed and queued.
+ * @return TRUE if msg successfully queued for transmit, FALSE if not
+ *************************************************************************/
+BOOL sendDGServiceModeRequest()
+{
+    BOOL result;
+    MESSAGE_T msg;
+
+    // Create a message record
+    blankMessage( &msg );
+    msg.hdr.msgID = MSG_ID_HD_REQUEST_DG_SERVICE_MODE;
+    msg.hdr.payloadLen = 0;
+
+    // Serialize the message (w/ sync, CRC, and appropriate CAN padding) and add serialized message data to appropriate comm buffer
+    result = serializeMessage( msg, COMM_BUFFER_OUT_CAN_HD_2_DG, ACK_REQUIRED );
+
+    return result;
+}
+
 /**@}*/