From 03235c8aab64aaffd22844949cd407bf9c4ec7b6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Gu=C3=B0ni=20M=C3=A1r=20Gilbert?= <gudni.m.g@gmail.com>
Date: Sat, 19 Nov 2022 14:46:50 +0000
Subject: [PATCH] PFW-1432 restore toolchange load testing

---
 Firmware/mmu2.cpp | 37 ++++++++++++++++++++++++++++++++++++-
 Firmware/mmu2.h   |  8 ++++++++
 2 files changed, 44 insertions(+), 1 deletion(-)

diff --git a/Firmware/mmu2.cpp b/Firmware/mmu2.cpp
index c36943a06..42442212c 100644
--- a/Firmware/mmu2.cpp
+++ b/Firmware/mmu2.cpp
@@ -306,9 +306,43 @@ void MMU2::DecrementRetryAttempts() {
     }
 }
 
+bool MMU2::FSensorCalibrationCheck()
+{
+    st_synchronize();
+
+    if (!fsensor.getFilamentPresent()) return false;
+
+    uint8_t fsensorState = 0;
+    // MMU has finished its load, move filament back by ExtraLoadDistance
+    // If Fsensor reads 0 at any moment, then report FAILURE
+    current_position[E_AXIS] -= logic.ExtraLoadDistance();
+    plan_buffer_line_curposXYZE(MMU2_LOAD_TO_NOZZLE_FEED_RATE);
+
+    while(blocks_queued())
+    {
+        // Wait for move to finish and monitor the fsensor the entire time
+        // a single 0 (1) reading is enough to fail the test
+        fsensorState |= !fsensor.getFilamentPresent();
+    }
+
+    if (fsensorState)
+    {
+        return false;
+    } else {
+        // else, happy printing! :)
+        // Revert the movements
+        current_position[E_AXIS] += logic.ExtraLoadDistance();
+        plan_buffer_line_curposXYZE(MMU2_LOAD_TO_NOZZLE_FEED_RATE);
+        st_synchronize();
+        return true;
+    }
+}
+
 void MMU2::ToolChangeCommon(uint8_t slot){
+    bool calibrationCheckResult = true;
     tool_change_extruder = slot;
     do {
+        if (!calibrationCheckResult) unload(); // TODO cut filament
         for(;;) {
             logic.ToolChange(slot); // let the MMU pull the filament out and push a new one in
             if( manage_response(true, true) )
@@ -324,7 +358,8 @@ void MMU2::ToolChangeCommon(uint8_t slot){
         }
         // reset current position to whatever the planner thinks it is
         plan_set_e_position(current_position[E_AXIS]);
-    } while (0); // while not successfully fed into etruder's PTFE tube
+        calibrationCheckResult = FSensorCalibrationCheck();
+    } while (!calibrationCheckResult); // while not successfully fed into etruder's PTFE tube
     // when we run out of feeding retries, we should call an unload + cut before trying again.
     // + we need some error screen report
 
diff --git a/Firmware/mmu2.h b/Firmware/mmu2.h
index 323b869f3..522a6e1b1 100644
--- a/Firmware/mmu2.h
+++ b/Firmware/mmu2.h
@@ -280,6 +280,14 @@ private:
     /// @returns false if the MMU is not ready to perform the command (for whatever reason)
     bool WaitForMMUReady();
 
+    /// Redundancy test. After MMU completes a tool-change command
+    /// the printer will retract the filament by a distance set by the
+    //  Extra Loading Distance MMU register. If the Fsensor untriggers
+    /// at any moment the test fails. Else test passes, and the E-motor retraction
+    /// is reverted.
+    /// @returns false if test fails, true otherwise
+    bool FSensorCalibrationCheck();
+
     /// Common processing of pushing filament into the extruder - shared by tool_change, load_to_nozzle and probably others
     void ToolChangeCommon(uint8_t slot);