Index: firmware/App/Modes/OperationModes.c
===================================================================
diff -u -r50a01b77ced16a3ceaec0ea3828502c7a2a286d4 -rdcd360fb4dc37db2dcbeb7fb14fb327fe68235f4
--- firmware/App/Modes/OperationModes.c	(.../OperationModes.c)	(revision 50a01b77ced16a3ceaec0ea3828502c7a2a286d4)
+++ firmware/App/Modes/OperationModes.c	(.../OperationModes.c)	(revision dcd360fb4dc37db2dcbeb7fb14fb327fe68235f4)
@@ -1,22 +1,23 @@
 /**************************************************************************
 *
-* Copyright (c) 2019-2020 Diality Inc. - All Rights Reserved.
+* Copyright (c) 2019-2021 Diality Inc. - All Rights Reserved.
 *
 * THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN
 * WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER.
 *
-* @file OperationModes.c
+* @file    OperationModes.c
 *
-* @author (last) Quang Nguyen
-* @date (last) 24-Aug-2020
+* @author  (last)      Dara Navaei
+* @date    (last)      06-Nov-2021
 *
-* @author (original) Dara Navaei
-* @date (original) 05-Nov-2019
+* @author  (original)  Dara Navaei
+* @date    (original)  05-Nov-2019
 *
 ***************************************************************************/
 
 #include "gio.h"
 
+#include "MessageSupport.h"
 #include "ModeChemicalDisinfect.h"
 #include "ModeDrain.h"
 #include "ModeFault.h"
@@ -50,6 +51,7 @@
 /// DG operation mode data publish interval.
 static OVERRIDE_U32_T dgOpModePublishInterval = { BROADCAST_DG_OP_MODE_INTERVAL, BROADCAST_DG_OP_MODE_INTERVAL, 0, 0 };
 static U32 broadcastModeIntervalCtr = 11;                                           ///< Interval counter used to determine when to broadcast operation mode.  Initialize to 11 to stagger broadcast.
+static U32 priorSubMode = 0;                                                        ///< The prior submode state.
 
 /// This matrix determines legal transitions from one mode to another.
 static const DG_OP_MODE_T MODE_TRANSITION_TABLE[ NUM_OF_DG_MODES - 1 ][ NUM_OF_DG_MODES - 1 ] =
@@ -96,6 +98,7 @@
     // start in init mode
     currentMode = DG_MODE_INIT;
     currentSubMode = 0;
+    priorSubMode = 0;
     transitionToNewOperationMode( DG_MODE_INIT );
 
     // call initializers for the individual modes
@@ -124,6 +127,8 @@
     DG_OP_MODE_T newMode;
     U32 priorSubMode = currentSubMode;
 
+    priorSubMode = currentSubMode;
+
     // any new mode requests?
     newMode = arbitrateModeRequest();   // will return current mode if no pending requests
     newMode = MODE_TRANSITION_TABLE[ currentMode ][ newMode ];
@@ -204,6 +209,7 @@
     {
         SEND_EVENT_WITH_2_U32_DATA( DG_EVENT_SUB_MODE_CHANGE, priorSubMode, currentSubMode )
     }
+
     priorSubMode = currentSubMode;
 
     // publish op mode on interval
@@ -311,42 +317,46 @@
     switch ( newMode )
     {
         case DG_MODE_FAUL:
-            transitionToFaultMode();
+            currentSubMode = transitionToFaultMode();
             break;
         case DG_MODE_SERV:
-            transitionToServiceMode();
+            currentSubMode = transitionToServiceMode();
             break;
         case DG_MODE_INIT:
-            transitionToInitAndPOSTMode();
+            currentSubMode = transitionToInitAndPOSTMode();
             break;
         case DG_MODE_STAN:
-            transitionToStandbyMode();
+            currentSubMode = transitionToStandbyMode();
             break;
         case DG_MODE_SOLO:
-            transitionToSoloMode();
+            currentSubMode = transitionToSoloMode();
             break;
         case DG_MODE_GENE:
-            transitionToGenIdleMode();
+            currentSubMode = transitionToGenIdleMode();
             break;
         case DG_MODE_FILL:
-            transitionToFillMode();
+            currentSubMode = transitionToFillMode();
             break;
         case DG_MODE_DRAI:
-            transitionToDrainMode();
+            currentSubMode = transitionToDrainMode();
             break;
         case DG_MODE_FLUS:
-            transitionToFlushMode();
+            currentSubMode = transitionToFlushMode();
             break;
         case DG_MODE_HEAT:
-            transitionToHeatDisinfectMode();
+            currentSubMode = transitionToHeatDisinfectMode();
             break;
         case DG_MODE_CHEM:
-            transitionToChemicalDisinfectMode();
+            currentSubMode = transitionToChemicalDisinfectMode();
             break;
         default:
             SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_OP_MODES_INVALID_MODE_TO_TRANSITION_TO, (U32)newMode )
             break;
     }
+
+    SEND_EVENT_WITH_2_U32_DATA( DG_EVENT_SUB_MODE_CHANGE, priorSubMode, currentSubMode )
+
+    priorSubMode = currentSubMode;
 }
 
 /*********************************************************************//**
@@ -361,8 +371,14 @@
 {
     if ( ++broadcastModeIntervalCtr >= getDGOpModePublishInterval() )
     {
+        OP_MODES_DATA_T data;
+
+        data.currentMode    = (U32)currentMode;
+        data.currentSubMode = currentSubMode;
+
+        broadcastData( MSG_ID_DG_OP_MODE, COMM_BUFFER_OUT_CAN_DG_BROADCAST, (U08*)&data, sizeof( OP_MODES_DATA_T ) );
+
         broadcastModeIntervalCtr = 0;
-        broadcastDGOperationMode( (U32)currentMode, currentSubMode );
     }
 }