Fix setup/FINDA init state

It turned out FINDA needs running timer to perform BlockingInit() correctly.
Therefore setup() was split into setup() (no IRQ) and setup2() (IRQ enabled).

Then, finally, the check for FINDA state became reliable upon start of the FW.

src/logic/tool_change.cpp

@@ -175,6 +175,8 @@ ProgressCode ToolChange::State() const {
             return ProgressCode::FeedingToFSensor;
         case FeedToBondtech::PushingFilamentIntoNozzle:
             return ProgressCode::FeedingToNozzle;
+        case FeedToBondtech::DisengagingIdler:
+            return ProgressCode::DisengagingIdler;
         }
         // [[fallthrough]] // everything else is reported as FeedingToBondtech
     default:

src/main.cpp

@@ -32,7 +32,8 @@
 /// One-time setup of HW and SW components
 /// Called before entering the loop() function
 /// Green LEDs signalize the progress of initialization. If anything goes wrong we shall turn on a red LED
-void setup() {
+/// Executed with interrupts disabled
+static void setup() {
     hal::cpu::Init();
 
     mt::timebase.Init();
@@ -80,7 +81,10 @@ void setup() {
     ml::leds.Step();
 
     mu::cdc.Init();
+}
 
+/// Second part of setup - executed with interrupts enabled
+static void setup2() {
     // waits at least finda debounce period
     // which is abused to let the LEDs shine for ~100ms
     mf::finda.BlockingInit();
@@ -93,9 +97,7 @@ void setup() {
         // Ideally this should be signalled as an error state and displayed on the printer and recovered properly.
         mg::globals.SetFilamentLoaded(2, mg::InFSensor);
         logic::noCommand.SetInitError(ErrorCode::FINDA_VS_EEPROM_DISREPANCY);
-    }
-
-    if (!mf::finda.Pressed() && mg::globals.FilamentLoaded() >= mg::InSelector) {
+    } else if (!mf::finda.Pressed() && mg::globals.FilamentLoaded() >= mg::InSelector) {
         // Opposite situation - not so dangerous, but definitely confusing to users.
         // FINDA is not pressed but we have a record in the EEPROM.
         // It has been decided, that we shall clear such a record from EEPROM automagically
@@ -158,6 +160,7 @@ void loop() {
 int main() {
     setup();
     sei(); ///enable interrupts
+    setup2();
     for (;;) {
         loop();
     }

src/modules/finda.cpp

@@ -14,10 +14,10 @@ void FINDA::Step() {
 }
 
 void FINDA::BlockingInit() {
-    auto tgtMs = mt::timebase.Millis() + config::findaDebounceMs + 1;
-    Step(); // let FINDA settle down - we're gonna need its state for selector homing
-    while (tgtMs < mt::timebase.Millis()) {
-        mf::finda.Step();
+    uint16_t start = mt::timebase.Millis();
+    // let FINDA settle down - we're gonna need its state for selector homing
+    while (!mt::timebase.Elapsed(start, config::findaDebounceMs + 1)) {
+        Step();
     }
 }
 

src/modules/timebase.cpp

@@ -20,6 +20,12 @@ void Timebase::Isr() {
     ms++;
 }
 
+bool Timebase::Elapsed(uint16_t start, uint16_t timeout) const {
+    uint16_t ms_from_start = Millis(); // beware the uint16_t!
+    ms_from_start -= start;
+    return ms_from_start > timeout;
+}
+
 uint16_t Timebase::Millis() const {
     return ms;
 }

src/modules/timebase.h

@@ -24,6 +24,10 @@ public:
 
     void Isr();
 
+    /// @returns true if the timeout elapsed from the start
+    /// handles correctly timer counter overflows
+    bool Elapsed(uint16_t start, uint16_t timeout) const;
+
 private:
     uint16_t ms;
 };

tests/unit/modules/CMakeLists.txt

@@ -4,3 +4,4 @@ add_subdirectory(protocol)
 add_subdirectory(speed_table)
 add_subdirectory(pulse_gen)
 add_subdirectory(motion)
+add_subdirectory(timebase)

tests/unit/modules/stubs/stub_timebase.cpp

@@ -24,5 +24,11 @@ void IncMillis(uint16_t inc /* = 1*/) {
     millis += inc;
 }
 
+bool Timebase::Elapsed(uint16_t start, uint16_t timeout) const {
+    uint16_t ms_from_start = Millis(); // beware the uint16_t!
+    ms_from_start -= start;
+    return ms_from_start > timeout;
+}
+
 } // namespace time
 } // namespace modules

tests/unit/modules/timebase/CMakeLists.txt

@@ -0,0 +1,8 @@
+# define the test executable
+add_executable(timebase_tests ${MODULES_STUBS_DIR}/stub_timebase.cpp test_timebase.cpp)
+
+# define required search paths
+target_include_directories(timebase_tests PUBLIC ${CMAKE_SOURCE_DIR}/src/modules)
+
+# tell build system about the test case
+add_catch_test(timebase_tests)

tests/unit/modules/timebase/test_timebase.cpp

@@ -0,0 +1,22 @@
+#include "catch2/catch.hpp"
+#include "timebase.h"
+#include "../stubs/stub_timebase.h"
+
+using Catch::Matchers::Equals;
+
+// this is not a pure test of the real implementation (it would require splitting the timebase.cpp into 2 parts)
+// but serves the sole purpose of debugging the Elapsed() impl.
+TEST_CASE("timebase::Elapsed", "[timebase]") {
+    {
+        mt::ReinitTimebase(0);
+        uint16_t start = mt::timebase.Millis();
+        mt::IncMillis(5);
+        REQUIRE(mt::timebase.Elapsed(start, 4));
+    }
+    {
+        mt::ReinitTimebase(0xffff);
+        uint16_t start = mt::timebase.Millis();
+        mt::IncMillis(5);
+        REQUIRE(mt::timebase.Elapsed(start, 4));
+    }
+}