Unload now rechecks the position of filament after recovery

This solves a number of issues - if FINDA or FSensor failed,
the unload was never "complete" - filament was stuck in the selector
blocking it from normal operation.

Now, after all errors have been resolved, filament is explicitly FED
into FINDA and then RETRACTED to Pulley.

src/config/config.h

@@ -83,15 +83,17 @@ static constexpr U_mm couplerToBowden = 3.5_mm; /// 3.5_mm /// FINDA Coupler scr
 // just another piece of PLA (probably having more resistance in the tubes)
 // and we are at least 40mm off! It looks like this really depends on the exact position
 // We'll probably need to check for stallguard while pushing the filament to avoid ginding the filament
-static constexpr U_mm defaultBowdenLength = 427.0_mm; /// ~427.0_mm /// Default Bowden length. @TODO Should be stored in EEPROM. 392 a 784
+static constexpr U_mm defaultBowdenLength = 427.0_mm; ///< ~427.0_mm - Default Bowden length. @TODO Should be stored in EEPROM. 392 a 784
-static constexpr U_mm minimumBowdenLength = 341.0_mm; /// ~341.0_mm /// Minimum bowden length. @TODO Should be stored in EEPROM.
+static constexpr U_mm minimumBowdenLength = 341.0_mm; ///< ~341.0_mm - Minimum bowden length. @TODO Should be stored in EEPROM.
-static constexpr U_mm maximumBowdenLength = 792.0_mm; /// ~792.0_mm /// Maximum bowden length. @TODO Should be stored in EEPROM.
+static constexpr U_mm maximumBowdenLength = 792.0_mm; ///< ~792.0_mm - Maximum bowden length. @TODO Should be stored in EEPROM.
 static constexpr U_mm feedToFinda = cuttingEdgeToFindaMidpoint + filamentMinLoadedToMMU;
 static constexpr U_mm maximumFeedToFinda = feedToFinda + 20.0_mm; ///< allow for some safety margin to load to FINDA
-static constexpr U_mm pulleyHelperMove = 10.0_mm; /// Helper move for Load/Unload error states - when the MMU should slowly move the filament a bit
+static constexpr U_mm pulleyHelperMove = 10.0_mm; ///< Helper move for Load/Unload error states - when the MMU should slowly move the filament a bit
 static constexpr U_mm cutLength = 8.0_mm;
-static constexpr U_mm fsensorToNozzle = 20.0_mm; /// ~20mm from MK4's filament sensor through extruder gears into nozzle
+static constexpr U_mm fsensorToNozzle = 20.0_mm; ///< ~20mm from MK4's filament sensor through extruder gears into nozzle
 static constexpr U_mm fsensorToNozzleAvoidGrind = 5.0_mm;
+/// Check the state of FSensor after this amount of filament got (hopefully) pulled out while unloading.
+static constexpr U_mm fsensorUnloadCheckDistance = 20.0_mm;
 
 /// Begin: Pulley axis configuration
 static constexpr AxisConfig pulley = {

src/logic/unload_filament.cpp

@@ -31,7 +31,7 @@ void UnloadFilament::Reset(uint8_t /*param*/) {
     ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::off, ml::off);
 }
 
-void logic::UnloadFilament::FinishedCorrectly() {
+void UnloadFilament::FinishedCorrectly() {
     state = ProgressCode::OK;
     error = ErrorCode::OK;
     mm::motion.Disable(mm::Pulley);
@@ -39,21 +39,31 @@ void logic::UnloadFilament::FinishedCorrectly() {
     ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::off, ml::off);
 }
 
+void UnloadFilament::GoToRetractingFromFinda() {
+    state = ProgressCode::RetractingFromFinda;
+    retract.Reset();
+}
+
+void UnloadFilament::GoToRecheckFilamentAgainstFINDA() {
+    state = ProgressCode::FeedingToFinda;
+    error = ErrorCode::RUNNING;
+    feed.Reset(true, true);
+}
+
 bool UnloadFilament::StepInner() {
     switch (state) {
     // state 1 engage idler - will be done by the Unload to FINDA state machine
     case ProgressCode::UnloadingToFinda: // state 2 rotate pulley as long as the FINDA is on
         if (unl.Step()) {
-            if (unl.State() == UnloadToFinda::Failed) {
+            if (unl.State() == UnloadToFinda::FailedFINDA) {
                 // couldn't unload to FINDA, report error and wait for user to resolve it
                 GoToErrDisengagingIdler(ErrorCode::FINDA_DIDNT_SWITCH_OFF);
-            } else if (mfs::fsensor.Pressed()) {
+            } else if (unl.State() == UnloadToFinda::FailedFSensor) {
                 // fsensor still pressed - that smells bad - a piece of filament may still be present in the heatsink
                 // and that would cause serious problems while loading another filament
                 GoToErrDisengagingIdler(ErrorCode::FSENSOR_DIDNT_SWITCH_OFF);
             } else {
-                state = ProgressCode::RetractingFromFinda;
+                GoToRetractingFromFinda();
-                retract.Reset();
             }
         }
         return false;
@@ -83,11 +93,17 @@ bool UnloadFilament::StepInner() {
             GoToErrEngagingIdler();
             break;
         case mui::Event::Middle: // try again the whole sequence
+            // First invalidate homing flags as the user may have moved the Idler or Selector accidentally
+            InvalidateHoming();
             if (mf::finda.Pressed()) {
-                Reset(0);
+                Reset(0); // filament is present in FINDA (regardless of FSensor) - assume we need to pull the filament to FINDA first
+            } else if (!mf::finda.Pressed() && mfs::fsensor.Pressed()) {
+                // a piece of filament is stuck in the extruder - keep waiting for the user to fix it
             } else {
-                state = ProgressCode::DisengagingIdler;
+                // filament is not present in FINDA and not in FSensor
-                mi::idler.Disengage();
+                // - that means the filament can still be behind FINDA and blocking the selector
+                // Ideally push it to FINDA and then back to verify the whole situation
+                GoToRecheckFilamentAgainstFINDA();
             }
             break;
         case mui::Event::Right: // problem resolved - the user pulled the fillament by hand
@@ -104,8 +120,8 @@ bool UnloadFilament::StepInner() {
                 error = ErrorCode::FINDA_DIDNT_SWITCH_OFF;
                 state = ProgressCode::ERRWaitingForUser; // stand still
             } else {
-                // all sensors are ok
+                // all sensors are ok, but re-check the position of the filament against FINDA
-                FinishedCorrectly();
+                GoToRecheckFilamentAgainstFINDA();
             }
             break;
         default:
@@ -129,6 +145,17 @@ bool UnloadFilament::StepInner() {
             GoToErrDisengagingIdler(ErrorCode::FINDA_DIDNT_SWITCH_OFF);
         }
         return false;
+    case ProgressCode::FeedingToFinda:
+        // recovery mode - we assume the filament is somewhere between the idle position and FINDA - thus blocking the selector
+        if (feed.Step()) {
+            if (feed.State() == FeedToFinda::Failed) {
+                GoToErrDisengagingIdler(ErrorCode::FINDA_DIDNT_SWITCH_ON);
+            } else {
+                state = ProgressCode::RetractingFromFinda;
+                retract.Reset();
+            }
+        }
+        break;
     case ProgressCode::OK:
         return true; // successfully finished
     default: // we got into an unhandled state, better report it

src/logic/unload_filament.h

@@ -2,6 +2,7 @@
 #pragma once
 #include <stdint.h>
 #include "command_base.h"
+#include "feed_to_finda.h"
 #include "unload_to_finda.h"
 #include "retract_from_finda.h"
 
@@ -25,8 +26,11 @@ private:
 
     /// Common code for a correct completion of UnloadFilament
     void FinishedCorrectly();
+    void GoToRetractingFromFinda();
+    void GoToRecheckFilamentAgainstFINDA();
 
     UnloadToFinda unl;
+    FeedToFinda feed;
     RetractFromFinda retract;
 };
 

src/logic/unload_to_finda.cpp

@@ -1,6 +1,7 @@
 /// @file unload_to_finda.cpp
 #include "unload_to_finda.h"
 #include "../modules/finda.h"
+#include "../modules/fsensor.h"
 #include "../modules/globals.h"
 #include "../modules/idler.h"
 #include "../modules/leds.h"
@@ -21,6 +22,11 @@ void UnloadToFinda::Reset(uint8_t maxTries) {
     }
 }
 
+// @@TODO this may end up somewhere else as more code may need to check the distance traveled by the filament
+int32_t CurrentPositionPulley_mm() {
+    return mm::stepsToUnit<mm::P_pos_t>(mm::P_pos_t({ mm::motion.CurPosition(mm::Pulley) }));
+}
+
 bool UnloadToFinda::Step() {
     switch (state) {
     case EngagingIdler:
@@ -29,18 +35,26 @@ bool UnloadToFinda::Step() {
             mm::motion.InitAxis(mm::Pulley);
             ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::blink0);
         } else {
-            state = Failed;
+            state = FailedFINDA;
         }
         return false;
     case UnloadingToFinda:
         if (mi::idler.Engaged()) {
             state = WaitingForFINDA;
             mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InSelector);
+            unloadStart_mm = CurrentPositionPulley_mm();
             mm::motion.PlanMove<mm::Pulley>(-config::defaultBowdenLength - config::feedToFinda - config::filamentMinLoadedToMMU, config::pulleyUnloadFeedrate);
         }
         return false;
-    case WaitingForFINDA:
+    case WaitingForFINDA: {
-        if (!mf::finda.Pressed()) {
+        int32_t currentPulley_mm = CurrentPositionPulley_mm();
+        if ((abs(unloadStart_mm - currentPulley_mm) > config::fsensorUnloadCheckDistance.v) && mfs::fsensor.Pressed()) {
+            // fsensor didn't trigger within the first fsensorUnloadCheckDistance mm -> stop pulling, something failed, report an error
+            // This scenario should not be tried again - repeating it may cause more damage to filament + potentially more collateral damage
+            state = FailedFSensor;
+            mm::motion.AbortPlannedMoves(); // stop rotating the pulley
+            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::off);
+        } else if (!mf::finda.Pressed()) {
             // detected end of filament
             state = OK;
             mm::motion.AbortPlannedMoves(); // stop rotating the pulley
@@ -51,12 +65,14 @@ bool UnloadToFinda::Step() {
             if (--maxTries) {
                 Reset(maxTries); // try again
             } else {
-                state = Failed;
+                state = FailedFINDA;
             }
         }
+    }
         return false;
     case OK:
-    case Failed:
+    case FailedFINDA:
+    case FailedFSensor:
     default:
         return true;
     }

src/logic/unload_to_finda.h

@@ -18,7 +18,8 @@ struct UnloadToFinda {
         UnloadingToFinda,
         WaitingForFINDA,
         OK,
-        Failed
+        FailedFINDA,
+        FailedFSensor
     };
     inline UnloadToFinda()
         : maxTries(3) {}
@@ -36,6 +37,7 @@ struct UnloadToFinda {
 private:
     uint8_t state;
     uint8_t maxTries;
+    int32_t unloadStart_mm; // intentionally trying to avoid using U_mm because it is a float (reps. long double)
 };
 
 } // namespace logic

src/modules/axisunit.h

@@ -112,13 +112,30 @@ static constexpr AU unitToAxisUnit(U v) {
     return { (typename AU::type_t)(v.v * axisScale[AU::axis].stepsPerUnit) };
 }
 
-/// Convert an unit::Unit to a steps type (pos_t or steps_t).
+/// Convert an AxisUnit to unit::Unit.
+/// The scaling factor is stored with the pair config::AxisConfig::uSteps and
+/// config::AxisConfig::stepsPerUnit (one per-axis).
+template <typename U, typename AU>
+static constexpr typename U::type_t axisUnitToUnit(AU v) {
+    static_assert(AU::unit == U::unit, "incorrect unit type conversion");
+    //static_assert(U::base == axisScale[AU::axis].base, "incorrect unit base conversion");
+    return { (typename U::type_t)(v.v / axisScale[AU::axis].stepsPerUnit) };
+}
+
+/// Convert a unit::Unit to a steps type (pos_t or steps_t).
 /// Extract the raw step count from an AxisUnit with type checking.
 template <typename AU, typename U>
 static constexpr typename AU::type_t unitToSteps(U v) {
     return unitToAxisUnit<AU>(v).v;
 }
 
+/// Convert a steps type (pos_t or steps_t) to a unit::Unit.
+/// Extract the raw step count from an AxisUnit with type checking.
+template <typename U, typename AU>
+static constexpr typename U::type_t stepsToUnit(AU pos) {
+    return axisUnitToUnit<U, AU>(pos);
+}
+
 // Pulley
 typedef AxisUnit<pos_t, Pulley, Lenght> P_pos_t; ///< Pulley position type (steps)
 typedef AxisUnit<steps_t, Pulley, Speed> P_speed_t; ///< Pulley speed type (steps/s)

src/modules/usb_cdc.cpp

@@ -82,7 +82,7 @@ void EVENT_CDC_Device_LineEncodingChanged(USB_ClassInfo_CDC_Device_t *const CDCI
         hal::watchdog::Enable(hal::watchdog::configuration::compute(250));
     }
 }
-}
+} // extern "C"
 
 namespace modules {
 namespace usb {

tests/unit/logic/stubs/main_loop_stub.cpp

@@ -141,3 +141,18 @@ void SetFINDAStateAndDebounce(bool press) {
     for (size_t i = 0; i < config::findaDebounceMs + 1; ++i)
         main_loop();
 }
+
+// The idea is to set fsOff and findaOff to some reasonable values (like 10 and 1000)
+// for normal situations.
+// For errorneous situations set fsOff or findaOff to some number higher than the number of steps
+// the testing routine is allowed to do -> thus effectively blocking the corresponding moment for fsensor
+// and finda switching off
+bool SimulateUnloadToFINDA(uint32_t step, uint32_t fsOff, uint32_t findaOff) {
+    if (step == fsOff) { // make FSensor switch off
+        mfs::fsensor.ProcessMessage(false);
+        return true;
+    } else if (step == findaOff) { // make FINDA switch off
+        hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::low);
+    }
+    return mf::finda.Pressed();
+}

tests/unit/logic/stubs/main_loop_stub.h

@@ -27,6 +27,7 @@ void SetFINDAStateAndDebounce(bool press);
 void SimulateIdlerHoming();
 void SimulateSelectorHoming();
 void SimulateIdlerAndSelectorHoming();
+bool SimulateUnloadToFINDA(uint32_t step, uint32_t fsOff, uint32_t findaOff);
 
 // these are recommended max steps for simulated movement of the idler and selector
 // - roughly the amount of motion steps from one end to the other + some margin

tests/unit/logic/stubs/stub_motion.cpp

@@ -48,6 +48,7 @@ pos_t Motion::Position(Axis axis) const {
 
 void Motion::SetPosition(Axis axis, pos_t x) {
     axes[axis].pos = x;
+    axisData[axis].ctrl.SetPosition(axes[axis].pos);
 }
 
 void Motion::SetMode(Axis axis, hal::tmc2130::MotorMode mode) {
@@ -58,6 +59,7 @@ st_timer_t Motion::Step() {
         if (axes[i].pos != axes[i].targetPos) {
             int8_t dirInc = (axes[i].pos < axes[i].targetPos) ? 1 : -1;
             axes[i].pos += dirInc;
+            axisData[i].ctrl.SetPosition(axes[i].pos);
         }
     }
     return 0;
@@ -83,6 +85,7 @@ void Motion::AbortPlannedMoves(bool halt) {
 
 void Motion::AbortPlannedMoves(config::Axis i, bool) {
     axes[i].targetPos = axes[i].pos; // leave the axis where it was at the time of abort
+    axisData[i].ctrl.SetPosition(axes[i].pos);
 }
 
 void ReinitMotion() {

tests/unit/logic/unload_filament/test_unload_filament.cpp

@@ -18,6 +18,7 @@
 #include "../stubs/stub_motion.h"
 
 using Catch::Matchers::Equals;
+using namespace std::placeholders;
 
 #include "../helpers/helpers.ipp"
 
@@ -38,7 +39,7 @@ void RegularUnloadFromSlot04Init(uint8_t slot, logic::UnloadFilament &uf) {
     uf.Reset(slot);
 }
 
-void RegularUnloadFromSlot04(uint8_t slot, logic::UnloadFilament &uf) {
+void RegularUnloadFromSlot04(uint8_t slot, logic::UnloadFilament &uf, uint8_t entryIdlerSlotIndex, bool selectorShallHomeAtEnd) {
     // Stage 0 - verify state just after Reset()
     // we still think we have filament loaded at this stage
     // idler should have been activated by the underlying automaton
@@ -46,7 +47,7 @@ void RegularUnloadFromSlot04(uint8_t slot, logic::UnloadFilament &uf) {
     // FINDA on
     // green LED should blink, red off
     REQUIRE(VerifyState(uf, (mg::FilamentLoadState)(mg::FilamentLoadState::InNozzle | mg::FilamentLoadState::InSelector),
-        mi::Idler::IdleSlotIndex(), slot, true, true, ml::off, ml::off, ErrorCode::RUNNING, ProgressCode::UnloadingToFinda));
+        entryIdlerSlotIndex, slot, true, true, ml::off, ml::off, ErrorCode::RUNNING, ProgressCode::UnloadingToFinda));
 
     // run the automaton
     // Stage 1 - unloading to FINDA
@@ -72,6 +73,10 @@ void RegularUnloadFromSlot04(uint8_t slot, logic::UnloadFilament &uf) {
     // Stage 3 - idler was engaged, disengage it
     REQUIRE(WhileTopState(uf, ProgressCode::DisengagingIdler, idlerEngageDisengageMaxSteps));
 
+    if (selectorShallHomeAtEnd) {
+        SimulateSelectorHoming();
+    }
+
     // filament unloaded
     // idler should have been disengaged
     // no change in selector's position
@@ -90,7 +95,7 @@ TEST_CASE("unload_filament::regular_unload_from_slot_0-4", "[unload_filament]")
     for (uint8_t slot = 0; slot < config::toolCount; ++slot) {
         logic::UnloadFilament uf;
         RegularUnloadFromSlot04Init(slot, uf);
-        RegularUnloadFromSlot04(slot, uf);
+        RegularUnloadFromSlot04(slot, uf, mi::Idler::IdleSlotIndex(), false);
     }
 }
 
@@ -125,7 +130,13 @@ void FindaDidntTriggerCommonSetup(uint8_t slot, logic::UnloadFilament &uf) {
     // run the automaton
     // Stage 1 - unloading to FINDA - do NOT let it trigger - keep it pressed, the automaton should finish all moves with the pulley
     // without reaching the FINDA and report an error
-    REQUIRE(WhileTopState(uf, ProgressCode::UnloadingToFinda, 500000));
+    REQUIRE(WhileCondition(
+        uf,
+        [&](uint32_t step) {
+            SimulateUnloadToFINDA(step, 10, 1'000'000);
+            return uf.TopLevelState() == ProgressCode::UnloadingToFinda;
+        },
+        200'000));
 
     // we still think we have filament loaded at this stage
     // idler should have been activated by the underlying automaton
@@ -243,6 +254,9 @@ void FindaDidntTriggerResolveTryAgain(uint8_t slot, logic::UnloadFilament &uf) {
     // FINDA still on
     // red LED should blink, green LED should be off
     REQUIRE(VerifyState(uf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, true, true, ml::off, ml::off, ErrorCode::RUNNING, ProgressCode::UnloadingToFinda));
+
+    // Assume, the Idler homed (homing is invalidated after pressing the recovery button)
+    SimulateIdlerHoming();
 }
 
 TEST_CASE("unload_filament::finda_didnt_trigger_resolve_try_again", "[unload_filament]") {
@@ -250,7 +264,7 @@ TEST_CASE("unload_filament::finda_didnt_trigger_resolve_try_again", "[unload_fil
         logic::UnloadFilament uf;
         FindaDidntTriggerCommonSetup(slot, uf);
         FindaDidntTriggerResolveTryAgain(slot, uf);
-        RegularUnloadFromSlot04(slot, uf);
+        RegularUnloadFromSlot04(slot, uf, slot, true);
     }
 }
 
@@ -281,6 +295,19 @@ void FailedUnloadResolveManual(uint8_t slot, logic::UnloadFilament &uf) {
     hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::low);
     PressButtonAndDebounce(uf, mb::Right);
 
+    REQUIRE(VerifyState(uf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, false, false, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::FeedingToFinda));
+
+    // we still need to feed to FINDA and back to verify the position of the filament
+    SimulateIdlerHoming();
+
+    REQUIRE(WhileTopState(uf, ProgressCode::FeedingToFinda, 5000));
+
+    REQUIRE(WhileTopState(uf, ProgressCode::RetractingFromFinda, idlerEngageDisengageMaxSteps));
+
+    REQUIRE(WhileTopState(uf, ProgressCode::DisengagingIdler, idlerEngageDisengageMaxSteps));
+
+    SimulateSelectorHoming();
+
     REQUIRE(VerifyState(uf, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), slot, false, false, ml::off, ml::off, ErrorCode::OK, ProgressCode::OK));
 }
 

tests/unit/logic/unload_to_finda/test_unload_to_finda.cpp

@@ -1,5 +1,7 @@
 #include "catch2/catch.hpp"
 
+#include <functional>
+
 #include "../../../../src/modules/buttons.h"
 #include "../../../../src/modules/finda.h"
 #include "../../../../src/modules/fsensor.h"
@@ -18,6 +20,7 @@
 #include "../stubs/stub_motion.h"
 
 using Catch::Matchers::Equals;
+using namespace std::placeholders;
 
 namespace ha = hal::adc;
 
@@ -27,6 +30,8 @@ TEST_CASE("unload_to_finda::regular_unload", "[unload_to_finda]") {
 
     // we need finda ON
     SetFINDAStateAndDebounce(true);
+    // fsensor should be ON
+    mfs::fsensor.ProcessMessage(true);
     // and MMU "thinks" it has the filament loaded
     mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InNozzle);
 
@@ -51,11 +56,7 @@ TEST_CASE("unload_to_finda::regular_unload", "[unload_to_finda]") {
 
     // now pulling the filament until finda triggers
     REQUIRE(ff.State() == logic::UnloadToFinda::WaitingForFINDA);
-    hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::low);
+    REQUIRE(WhileCondition(ff, std::bind(SimulateUnloadToFINDA, _1, 10, 1000), 1100));
-    REQUIRE(WhileCondition(
-        ff,
-        [&](uint32_t) { return mf::finda.Pressed(); },
-        50000));
 
     REQUIRE(ff.State() == logic::UnloadToFinda::OK);
     REQUIRE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::InSelector);
@@ -81,6 +82,8 @@ TEST_CASE("unload_to_finda::unload_without_FINDA_trigger", "[unload_to_finda]")
 
     // we need finda ON
     SetFINDAStateAndDebounce(true);
+    // fsensor should be ON
+    mfs::fsensor.ProcessMessage(true);
     // and MMU "thinks" it has the filament loaded
     mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InNozzle);
 
@@ -107,11 +110,50 @@ TEST_CASE("unload_to_finda::unload_without_FINDA_trigger", "[unload_to_finda]")
     REQUIRE(ff.State() == logic::UnloadToFinda::WaitingForFINDA);
 
     // no changes to FINDA during unload - we'll pretend it never triggers
-    REQUIRE_FALSE(WhileCondition(
+    // but set FSensor correctly
-        ff,
+    REQUIRE_FALSE(WhileCondition(ff, std::bind(SimulateUnloadToFINDA, _1, 10, 150000), 50000));
-        [&](uint32_t) { return mf::finda.Pressed(); },
-        50000));
 
-    REQUIRE(ff.State() == logic::UnloadToFinda::Failed);
+    REQUIRE(ff.State() == logic::UnloadToFinda::FailedFINDA);
+    REQUIRE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::InSelector);
+}
+
+TEST_CASE("unload_to_finda::unload_without_FSensor_trigger", "[unload_to_finda]") {
+    ForceReinitAllAutomata();
+    EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley);
+
+    // we need finda ON
+    SetFINDAStateAndDebounce(true);
+    // fsensor should be ON
+    mfs::fsensor.ProcessMessage(true);
+    // and MMU "thinks" it has the filament loaded
+    mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InNozzle);
+
+    logic::UnloadToFinda ff;
+
+    // restart the automaton - just 1 attempt
+    ff.Reset(1);
+
+    REQUIRE(ff.State() == logic::UnloadToFinda::EngagingIdler);
+
+    // it should have instructed the selector and idler to move to slot 1
+    // check if the idler and selector have the right command
+    CHECK(mm::axes[mm::Idler].targetPos == mi::Idler::SlotPosition(0).v);
+    CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0).v);
+    CHECK(mm::axes[mm::Idler].enabled == true);
+
+    // engaging idler
+    REQUIRE(WhileCondition(
+        ff,
+        [&](uint32_t) { return !mi::idler.Engaged(); },
+        5000));
+
+    // now pulling the filament until finda triggers
+    REQUIRE(ff.State() == logic::UnloadToFinda::WaitingForFINDA);
+
+    // no changes to FSensor during unload - we'll pretend it never triggers
+    // but set FINDA correctly
+    REQUIRE(WhileCondition(ff, std::bind(SimulateUnloadToFINDA, _1, 150000, 10000), 50000));
+
+    REQUIRE(ff.State() == logic::UnloadToFinda::FailedFSensor);
     REQUIRE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::InSelector);
 }