diff --git a/scripts/Calibration/tofpd/diagnostics.py b/scripts/Calibration/tofpd/diagnostics.py
index ed2b8d0663462b45951a3b660e366500ddaa8548..e7b0d48eb5e884790022230246226188a2b14c05 100644
--- a/scripts/Calibration/tofpd/diagnostics.py
+++ b/scripts/Calibration/tofpd/diagnostics.py
@@ -1,3 +1,10 @@
+# Mantid Repository : https://github.com/mantidproject/mantid
+#
+# Copyright © 2021 ISIS Rutherford Appleton Laboratory UKRI,
+#   NScD Oak Ridge National Laboratory, European Spallation Source,
+#   Institut Laue - Langevin & CSNS, Institute of High Energy Physics, CAS
+# SPDX - License - Identifier: GPL - 3.0 +
+
 from mantid.plots.resampling_image.samplingimage import imshow_sampling
 from mantid.plots.datafunctions import get_axes_labels
 from mantid.simpleapi import CalculateDIFC, LoadDiffCal, mtd
@@ -374,9 +381,10 @@ def extract_peak_info(wksp, outputname: str, peak_position: float):
     x = single.dataX(0)
     y = single.dataY(0)
     e = single.dataE(0)
+    start_detid = np.searchsorted(detids, 0)
     for wksp_index in range(numSpec):
         if have_detids:
-            x[wksp_index] = detids[wksp_index]
+            x[wksp_index] = detids[start_detid + wksp_index]
         else:
             x[wksp_index] = wksp_index
         y[wksp_index] = wksp.readY(wksp_index)[peak_index]
diff --git a/scripts/test/Calibration/CMakeLists.txt b/scripts/test/Calibration/CMakeLists.txt
index 158676dd10b944018704daf0547d6d904abc6315..924639b734f33e235e226ffd79f76971c752dbe3 100644
--- a/scripts/test/Calibration/CMakeLists.txt
+++ b/scripts/test/Calibration/CMakeLists.txt
@@ -4,9 +4,10 @@ set(TEST_PY_FILES
     test_ideal_tube.py
     test_tube.py
     test_tube_calib.py
-    tofpd/test_diagnostics.py
     )
 
+add_subdirectory(tofpd)
+
 check_tests_valid(${CMAKE_CURRENT_SOURCE_DIR} ${TEST_PY_FILES})
 
 pyunittest_add_test(${CMAKE_CURRENT_SOURCE_DIR} python.Calibration
diff --git a/scripts/test/Calibration/tofpd/CMakeLists.txt b/scripts/test/Calibration/tofpd/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..69a680c7c955109833c88d2d6c1cf1e4aa1adeac
--- /dev/null
+++ b/scripts/test/Calibration/tofpd/CMakeLists.txt
@@ -0,0 +1,10 @@
+# Unit tests for Calibration.tofpd
+
+set(TEST_PY_FILES
+    test_diagnostics.py
+    )
+
+check_tests_valid(${CMAKE_CURRENT_SOURCE_DIR} ${TEST_PY_FILES})
+
+pyunittest_add_test(${CMAKE_CURRENT_SOURCE_DIR} python.Calibration.tofpd
+                    ${TEST_PY_FILES})
diff --git a/scripts/test/Calibration/tofpd/test_diagnostics.py b/scripts/test/Calibration/tofpd/test_diagnostics.py
index 59b98a7aaa8b9d22e8313723c799062c2a0d8240..8b4eea2014ccb78aabbf5c7d0edf1152e5eacba2 100644
--- a/scripts/test/Calibration/tofpd/test_diagnostics.py
+++ b/scripts/test/Calibration/tofpd/test_diagnostics.py
@@ -30,20 +30,24 @@ class TestDiagnostics(unittest.TestCase):
         test_strain = diagnostics.collect_peaks('diag_dspacing', 'test_strain', infotype='strain')
         result = CompareWorkspaces(test_strain, "strain", Tolerance=1e-6)
         self.assertTrue(result)
+        DeleteWorkspaces(test_strain)
 
     def test_collect_peaks_diff(self):
         test_diff = diagnostics.collect_peaks('diag_dspacing', 'test_diff', infotype='difference')
         result = CompareWorkspaces(test_diff, "difference", Tolerance=1e-6)
         self.assertTrue(result)
+        DeleteWorkspaces(test_diff)
 
     def test_extract_peak_info(self):
         test_single_strain = diagnostics.extract_peak_info('strain', 'test_single_strain', self.PEAK)
         result = CompareWorkspaces(test_single_strain, "single_strain", Tolerance=1e-6)
         self.assertTrue(result)
+        DeleteWorkspaces(test_single_strain)
 
         test_single_diff = diagnostics.extract_peak_info('difference', 'test_single_diff', self.PEAK)
         result = CompareWorkspaces(test_single_diff, "single_diff", Tolerance=1e-6)
         self.assertTrue(result)
+        DeleteWorkspaces(test_single_diff)
 
 
 if __name__ == '__main__':