Index: firmware/App/Services/Reservoirs.c
===================================================================
diff -u -r59357d3831aa60f17ccdfbe0eef1a005935b9a58 -r09cde48e11d32be58fe9e1e7c88ada33cff664f5
--- firmware/App/Services/Reservoirs.c	(.../Reservoirs.c)	(revision 59357d3831aa60f17ccdfbe0eef1a005935b9a58)
+++ firmware/App/Services/Reservoirs.c	(.../Reservoirs.c)	(revision 09cde48e11d32be58fe9e1e7c88ada33cff664f5)
@@ -7,8 +7,8 @@
 *
 * @file    Reservoirs.c
 *
-* @author  (last)      Dara Navaei
-* @date    (last)      19-May-2023
+* @author  (last)      Michael Garthwaite
+* @date    (last)      18-Aug-2023
 *
 * @author  (original)  Sean
 * @date    (original)  18-Mar-2020
@@ -104,6 +104,8 @@
 static OVERRIDE_U32_T activeReservoir = { 0, 0, 0, 0 };                                           ///< The active reservoir that the DG is filling/draining/etc.
 static OVERRIDE_U32_T fillVolumeTargetMl = { 0, 0, 0, 0 };                                        ///< The target reservoir fill volume (in mL).
 static OVERRIDE_U32_T drainVolumeTargetMl = { 0, 0, 0, 0 };                                       ///< The target reservoir drain volume (in mL).
+static OVERRIDE_U32_T reservoirDataPublishInterval = { RESERVOIR_DATA_PUB_INTERVAL,
+                                                       RESERVOIR_DATA_PUB_INTERVAL, 0, 0 };       ///< Interval (in ms) at which to publish reservoir data to CAN bus.
 
 /// The reservoirs' associate load cell.
 static LOAD_CELL_ID_T associatedLoadCell[ NUM_OF_DG_RESERVOIRS ] = { LOAD_CELL_RESERVOIR_1_PRIMARY, LOAD_CELL_RESERVOIR_2_PRIMARY };
@@ -124,6 +126,10 @@
 static HD_MODE_SUB_MODE_T hdModes;                                                                ///< HD operations mode.
 static NV_OPS_T nvOps;                                                                            ///< Non-volatile memory operations.
 
+// ********** private function prototypes **********
+
+static void publishReservoirData( void );
+
 /*********************************************************************//**
  * @brief
  * The initReservoirs function initializes the Reservoirs module.
@@ -191,27 +197,7 @@
         }
     }
 
-    // publish active reservoir, fill/drain volume targets at 1 Hz.
-    if ( ++dataPublishCounter >= RESERVOIR_DATA_PUB_INTERVAL )
-    {
-        RESERVOIR_DATA_T data;
-
-        data.activeReservoir            = getU32OverrideValue( &activeReservoir );
-        data.fillToVolumeMl             = getU32OverrideValue( &fillVolumeTargetMl );
-        data.drainToVolumeMl            = getU32OverrideValue( &drainVolumeTargetMl );
-        data.timeReservoirCycleMS       = heatersTempCalc.timeReservoirCycleMS;
-        data.timeReservoirFill2SwitchMS = heatersTempCalc.timeReservoirFill2SwitchMS;
-        data.timeUFDecayMS              = heatersTempCalc.timeUFDecayMS;
-        data.tempUFFill                 = heatersTempCalc.tempUFFill;
-        data.tempReservoirUseActual     = getReservoirCurrentTemperature();
-        data.tempReservoirEndFill       = heatersTempCalc.tempReservoirEndFill;
-        data.tempAvgFill                = getAvgFillTemperature();
-        data.tempLastFill               = getLastFillTemperature();
-        data.timereservoirFill          = heatersTempCalc.timeReservoirFillMS;
-
-        broadcastData( MSG_ID_DG_RESERVOIRS_DATA, COMM_BUFFER_OUT_CAN_DG_BROADCAST, (U08*)&data, sizeof( RESERVOIR_DATA_T ) );
-        dataPublishCounter = 0;
-    }
+    publishReservoirData();
 }
 
 /*********************************************************************//**
@@ -513,8 +499,12 @@
  *************************************************************************/
 void setHDOperationMode( U32 mode, U32 subMode )
 {
+    // thread protection for queue operations
+    _disable_IRQ();
     hdModes.hdMode    = (HD_OP_MODE_T)mode;
     hdModes.hdSubMode = subMode;
+    // release thread protection
+    _enable_IRQ();
 }
 
 /*********************************************************************//**
@@ -528,7 +518,8 @@
  *************************************************************************/
 void getHDOperationMode( HD_MODE_SUB_MODE_T* mode )
 {
-    memcpy( mode, &hdModes, sizeof( HD_MODE_SUB_MODE_T ) );
+    mode->hdMode    = hdModes.hdMode;
+    mode->hdSubMode = hdModes.hdSubMode;
 }
 
 /*********************************************************************//**
@@ -1025,7 +1016,40 @@
     reservoirPreviousStatus[ reservoirId ].previousDrainFlowML      = 0.0F;
 }
 
+/*********************************************************************//**
+ * @brief
+ * The publishReservoirData function publishes reservoir data
+ * at the set interval.
+ * @details Inputs: dataPublishCounter, reservoirDataPublishInterval
+ * @details Outputs: data
+ * @return none
+ *************************************************************************/
+static void publishReservoirData( void )
+{
+    // publish active reservoir, fill/drain volume targets at 1 Hz.
+    if ( ++dataPublishCounter >= getU32OverrideValue( &reservoirDataPublishInterval ) )
+    {
+        RESERVOIR_DATA_T data;
 
+        data.activeReservoir            = getU32OverrideValue( &activeReservoir );
+        data.fillToVolumeMl             = getU32OverrideValue( &fillVolumeTargetMl );
+        data.drainToVolumeMl            = getU32OverrideValue( &drainVolumeTargetMl );
+        data.timeReservoirCycleMS       = heatersTempCalc.timeReservoirCycleMS;
+        data.timeReservoirFill2SwitchMS = heatersTempCalc.timeReservoirFill2SwitchMS;
+        data.timeUFDecayMS              = heatersTempCalc.timeUFDecayMS;
+        data.tempUFFill                 = heatersTempCalc.tempUFFill;
+        data.tempReservoirUseActual     = getReservoirCurrentTemperature();
+        data.tempReservoirEndFill       = heatersTempCalc.tempReservoirEndFill;
+        data.tempAvgFill                = getAvgFillTemperature();
+        data.tempLastFill               = getLastFillTemperature();
+        data.timereservoirFill          = heatersTempCalc.timeReservoirFillMS;
+
+        broadcastData( MSG_ID_DG_RESERVOIRS_DATA, COMM_BUFFER_OUT_CAN_DG_BROADCAST, (U08*)&data, sizeof( RESERVOIR_DATA_T ) );
+        dataPublishCounter = 0;
+    }
+}
+
+
 /*************************************************************************
  *                       TEST SUPPORT FUNCTIONS
  *************************************************************************/
@@ -1216,4 +1240,50 @@
     return result;
 }
 
+/*********************************************************************//**
+ * @brief
+ * The testSetReservoirDataPublishIntervalOverride function overrides the
+ * reservoir data publish interval.
+ * @details Inputs: reservoirDataPublishInterval
+ * @details Outputs: reservoirDataPublishInterval
+ * @param: value override reservoir data publish interval with (in ms)
+ * @return TRUE if override successful, FALSE if not
+ *************************************************************************/
+BOOL testSetReservoirDataPublishIntervalOverride( U32 value )
+{
+    BOOL result = FALSE;
+
+    if ( TRUE == isTestingActivated() )
+    {
+        U32 intvl                             = value / TASK_GENERAL_INTERVAL;
+        reservoirDataPublishInterval.ovData   = intvl;
+        reservoirDataPublishInterval.override = OVERRIDE_KEY;
+        result                                = TRUE;
+    }
+
+    return result;
+}
+
+/*********************************************************************//**
+ * @brief
+ * The testResetReservoirDataPublishIntervalOverride function resets the
+ * override of the reservoir publish interval.
+ * @details Inputs: reservoirDataPublishInterval
+ * @details Outputs: reservoirDataPublishInterval
+ * @return TRUE if override reset successful, FALSE if not
+ *************************************************************************/
+BOOL testResetReservoirDataPublishIntervalOverride( void )
+{
+    BOOL result = FALSE;
+
+    if ( TRUE == isTestingActivated() )
+    {
+        reservoirDataPublishInterval.override = OVERRIDE_RESET;
+        reservoirDataPublishInterval.ovData   = reservoirDataPublishInterval.ovInitData;
+        result                                = TRUE;
+    }
+
+    return result;
+}
+
 /**@}*/