Merge pull request #203 from prusa3d/vintagepc-check-hardware

Add module to check for sketchy electronics

src/application.cpp

@@ -13,6 +13,7 @@
 #include "logic/cut_filament.h"
 #include "logic/eject_filament.h"
 #include "logic/home.h"
+#include "logic/hw_sanity.h"
 #include "logic/load_filament.h"
 #include "logic/move_selector.h"
 #include "logic/no_command.h"

src/hal/tmc2130.cpp

@@ -6,6 +6,8 @@
 namespace hal {
 namespace tmc2130 {
 
+static constexpr uint8_t TOFF_DEFAULT = 3U, TOFF_MASK = 0xFU;
+
 bool TMC2130::Init(const MotorParams &params, const MotorCurrents &currents, MotorMode mode) {
     // sg_filter_threshold = (1 << (8 - params.mRes));
     sg_filter_threshold = 2;
@@ -26,7 +28,7 @@ bool TMC2130::Init(const MotorParams &params, const MotorCurrents &currents, Mot
     errorFlags.reset_flag = false;
 
     ///apply chopper parameters
-    const uint32_t chopconf = (uint32_t)(3U & 0x0FU) << 0U //toff
+    const uint32_t chopconf = (uint32_t)(TOFF_DEFAULT & TOFF_MASK) << 0U //toff
         | (uint32_t)(5U & 0x07U) << 4U //hstrt
         | (uint32_t)(1U & 0x0FU) << 7U //hend
         | (uint32_t)(2U & 0x03U) << 15U //tbl
@@ -71,6 +73,15 @@ void TMC2130::SetMode(const MotorParams &params, MotorMode mode) {
     WriteRegister(params, Registers::TPWMTHRS, (mode == Stealth) ? 70 : 0xFFFF0); // @@TODO should be configurable
 }
 
+void TMC2130::SetBridgeOutput(const MotorParams &params, bool bOn) {
+    uint32_t chopconf = ReadRegister(params, Registers::CHOPCONF);
+    chopconf &= ~((uint32_t)TOFF_MASK);
+    if (bOn) {
+        chopconf |= TOFF_DEFAULT;
+    }
+    WriteRegister(params, Registers::CHOPCONF, chopconf);
+}
+
 void TMC2130::SetCurrents(const MotorParams &params, const MotorCurrents &currents) {
     uint32_t ihold_irun = (uint32_t)(currents.iHold & 0x1F) << 0 //ihold
         | (uint32_t)(currents.iRun & 0x1F) << 8 //irun

src/hal/tmc2130.h

@@ -89,6 +89,9 @@ public:
     /// Set the current motor mode
     void SetMode(const MotorParams &params, MotorMode mode);
 
+    /// Disables the output by setting or clearing CHOPCONF's TOFF.
+    void SetBridgeOutput(const MotorParams &params, bool on);
+
     /// Set the current motor currents
     void SetCurrents(const MotorParams &params, const MotorCurrents &currents);
 
@@ -111,11 +114,22 @@ public:
         hal::shr16::shr16.SetTMCDir(params.idx, dir ^ params.dirOn);
     }
 
+    /// Set direction, raw value (i.e. ignore Params).
+    static inline void SetRawDir(const MotorParams &params, bool dir) {
+        // Also cancels the inversion in SetTMCDir
+        hal::shr16::shr16.SetTMCDir(params.idx, !dir);
+    }
+
     /// Step the motor
     static inline void Step(const MotorParams &params) {
         gpio::TogglePin(params.stepPin); // assumes DEDGE
     }
 
+    /// Set step to an explicit state
+    static inline void SetStep(const MotorParams &params, bool state) {
+        gpio::WritePin(params.stepPin, (state ? gpio::Level::high : gpio::Level::low));
+    }
+
     /// Return SG state
     static inline bool SampleDiag(const MotorParams &params) {
         return gpio::ReadPin(params.sgPin) == gpio::Level::low;

src/logic/CMakeLists.txt

@@ -6,6 +6,7 @@ target_sources(
           feed_to_bondtech.cpp
           feed_to_finda.cpp
           home.cpp
+          hw_sanity.cpp
           load_filament.cpp
           move_selector.cpp
           no_command.cpp

src/logic/error_codes.h

@@ -100,5 +100,12 @@ enum class ErrorCode : uint_fast16_t {
     /// - E49280 Selector TMC driver
     /// - E49408 Idler TMC driver
     /// - E49600 All 3 TMC driver
-    TMC_OVER_TEMPERATURE_ERROR = 0xC000
+    TMC_OVER_TEMPERATURE_ERROR = 0xC000,
+
+    /// TMC driver - IO pins are unreliable. While in theory it's recoverable, in practice it most likely
+    /// means your hardware is borked (we can't command the drivers reliably via STEP/EN/DIR due to electrical
+    /// issues or hardware fault. Possible "fixable" cause is undervoltage on the 5v logic line.
+    /// Unfixable possible cause: bad or cracked solder joints on the PCB, failed shift register, failed driver.
+    MMU_SOLDERING_NEEDS_ATTENTION = 0xC200,
+
 };

src/logic/hw_sanity.cpp

@@ -0,0 +1,175 @@
+/// @file hw_sanity.cpp
+#include "hw_sanity.h"
+#include "../modules/globals.h"
+#include "../modules/motion.h"
+#include "../modules/leds.h"
+#include "../modules/timebase.h"
+
+namespace logic {
+
+// Copy-pasta from command_base, they're inline
+// so this shouldn't affect code size.
+inline ErrorCode &operator|=(ErrorCode &a, ErrorCode b) {
+    return a = (ErrorCode)((uint16_t)a | (uint16_t)b);
+}
+
+using Axis = config::Axis;
+using TMC2130 = hal::tmc2130::TMC2130;
+
+static constexpr uint8_t LED_WAIT_MS = 50U;
+static constexpr uint8_t TEST_PASSES = 3U;
+static_assert(TEST_PASSES < 32); // Would overflow counters
+
+HWSanity hwSanity;
+
+bool HWSanity::Reset(uint8_t param) {
+    state = ProgressCode::HWTestBegin;
+    error = ErrorCode::RUNNING;
+    axis = config::Axis::Idler;
+    fault_masks[0] = 0;
+    fault_masks[1] = 0;
+    fault_masks[2] = 0;
+    return true;
+}
+
+enum pin_bits {
+    BIT_STEP = 0b001,
+    BIT_DIR = 0b010,
+    BIT_ENA = 0b100,
+};
+
+void HWSanity::SetFaultDisplay(uint8_t slot, uint8_t mask) {
+    ml::Mode red_mode = ml::off, green_mode = ml::off;
+    if (mask & BIT_STEP) {
+        green_mode = ml::on;
+    }
+    if (mask & BIT_DIR) {
+        red_mode = ml::on;
+    }
+    if (mask & BIT_ENA) {
+        green_mode = green_mode ? ml::blink0 : ml::on;
+        red_mode = red_mode ? ml::blink0 : ml::on;
+    }
+    ml::leds.SetMode(slot, ml::green, green_mode);
+    ml::leds.SetMode(slot, ml::red, red_mode);
+}
+
+void HWSanity::PrepareAxis(config::Axis axis) {
+    mm::motion.InitAxis(axis);
+    mm::motion.SetMode(axis, mm::Normal);
+    // Clear TOFF so the motors don't actually step during the test.
+    mm::motion.MMU_NEEDS_ATTENTION_DriverForAxis(axis).SetBridgeOutput(mm::axisParams[axis].params, false);
+}
+
+bool HWSanity::StepInner() {
+    switch (state) {
+    case ProgressCode::HWTestBegin:
+        error = ErrorCode::RUNNING;
+        test_step = 0;
+        state = ProgressCode::HWTestIdler;
+        break;
+    case ProgressCode::HWTestIdler:
+        axis = config::Axis::Idler;
+        ml::leds.SetPairButOffOthers(3, ml::on, ml::off);
+        state = ProgressCode::HWTestExec;
+        next_state = ProgressCode::HWTestSelector;
+        PrepareAxis(axis);
+        break;
+    case ProgressCode::HWTestSelector:
+        axis = config::Axis::Selector;
+        ml::leds.SetPairButOffOthers(3, ml::off, ml::on);
+        state = ProgressCode::HWTestExec;
+        next_state = ProgressCode::HWTestPulley;
+        PrepareAxis(axis);
+        break;
+    case ProgressCode::HWTestPulley:
+        axis = config::Axis::Pulley;
+        ml::leds.SetPairButOffOthers(3, ml::on, ml::on);
+        state = ProgressCode::HWTestExec;
+        next_state = ProgressCode::HWTestCleanup;
+        PrepareAxis(axis);
+        break;
+    // The main test loop for a given axis.
+    case ProgressCode::HWTestDisplay:
+        // Hold for a few ms while we display the last step result.
+        if (!mt::timebase.Elapsed(wait_start, LED_WAIT_MS)) {
+            break;
+        } else {
+            state = ProgressCode::HWTestExec;
+            // display done, reset LEDs.
+            for (uint8_t i = 0; i < 6; i++) {
+                ml::leds.SetMode(i, ml::off);
+            }
+        }
+        [[fallthrough]];
+    case ProgressCode::HWTestExec: {
+        auto params = mm::axisParams[axis].params;
+        auto &driver = mm::motion.MMU_NEEDS_ATTENTION_DriverForAxis(axis);
+        if (test_step < (TEST_PASSES * 8)) // 8 combos per axis
+        {
+            uint8_t set_state = test_step % 8;
+            // The order of the bits here is roughly the same as that of IOIN.
+            driver.SetRawDir(params, set_state & BIT_DIR);
+            driver.SetStep(params, set_state & BIT_STEP);
+            driver.SetEnabled(params, set_state & BIT_ENA);
+            uint32_t drv_ioin = driver.ReadRegister(params, hal::tmc2130::TMC2130::Registers::IOIN);
+            // Compose IOIN to look like set_state.
+            drv_ioin = (drv_ioin & 0b11) | ((drv_ioin & 0b10000) ? 0 : 4); // Note the logic inversion for ENA readback!
+            uint8_t bit_errs = (drv_ioin ^ set_state);
+            // Set the LEDs. Note RED is index 0 in the enum, so we want  the expression FALSE if there's an error.
+            ml::leds.SetMode(0, static_cast<ml::Color>((bit_errs & BIT_STEP) == 0), ml::on);
+            ml::leds.SetMode(1, static_cast<ml::Color>((bit_errs & BIT_DIR) == 0), ml::on);
+            ml::leds.SetMode(2, static_cast<ml::Color>((bit_errs & BIT_ENA) == 0), ml::on);
+            // Capture the error for later.
+            fault_masks[axis] |= bit_errs;
+            // Enter the wait state:
+            wait_start = mt::timebase.Millis();
+            das_blinken_state = das_blinken_state ? ml::off : ml::on;
+            ml::leds.SetMode(4, ml::green, das_blinken_state);
+            state = ProgressCode::HWTestDisplay;
+            // Next iteration.
+            test_step++;
+        } else {
+            // This pass is complete. Move on to the next motor or cleanup.
+            driver.SetBridgeOutput(params, true);
+            test_step = 0;
+            state = next_state;
+        }
+    } break;
+    case ProgressCode::HWTestCleanup:
+        if (fault_masks[0] || fault_masks[1] || fault_masks[2]) {
+            // error, display it and return the code.
+            state = ProgressCode::ErrHwTestFailed;
+            error = ErrorCode::MMU_SOLDERING_NEEDS_ATTENTION;
+            uint8_t mask = fault_masks[Axis::Idler];
+            if (mask) {
+                SetFaultDisplay(0, mask);
+            }
+            mask = fault_masks[Axis::Pulley];
+            if (mask) {
+                SetFaultDisplay(2, mask);
+            }
+            mask = fault_masks[Axis::Selector];
+            if (mask) {
+                SetFaultDisplay(1, mask);
+            }
+            ml::leds.SetMode(3, ml::red, ml::off);
+            ml::leds.SetMode(3, ml::green, ml::off);
+            ml::leds.SetMode(4, ml::red, ml::on);
+            ml::leds.SetMode(4, ml::green, ml::off);
+            return true;
+        } else {
+            ml::leds.SetPairButOffOthers(0, ml::off, ml::off);
+            FinishedOK();
+        }
+    case ProgressCode::OK:
+        return true;
+    default: // we got into an unhandled state, better report it
+        state = ProgressCode::ERRInternal;
+        error = ErrorCode::INTERNAL;
+        return true;
+    }
+    return false;
+}
+
+} // namespace logic

src/logic/hw_sanity.h

@@ -0,0 +1,57 @@
+/// @file hw_sanity.h
+#pragma once
+#include <stdint.h>
+#include "command_base.h"
+#include "../config/axis.h"
+#include "../modules/leds.h"
+
+namespace logic {
+
+/// @brief Performs a sanity check of the hardware at reset/boot. Checks the following:
+/// - TMC drivers using their IOIN registers (DIR/STEP/DRV_EN)
+/// - ...
+/// - Profit!
+
+class HWSanity : public CommandBase {
+public:
+    inline HWSanity()
+        : CommandBase() {}
+
+    /// Restart the automaton
+    bool Reset(uint8_t param) override;
+
+    /// @returns true if the state machine finished its job, false otherwise.
+    /// LED indicators during the test execution:
+    /// Slots 1-3: Pin states for STEP, DIR, and ENA - G:  Set value matches readback, R: Readback disagrees.
+    /// Slot 4: Axis under test - G: Idler, R: Selector, RG: Pully.
+    /// Slot 5: G: Blinking to indicate test progression. R: Solid to indicate completed test w/ fault.
+    /// Indicators at test end (fault condition):
+    /// Slots 1-3 now indicate pins for Idler/Selector/Pulley, respectively:
+    /// - Off: No faults detected.
+    /// - G:   STEP fault
+    /// - R:   DIR fault
+    /// - RG:  EN fault.
+    /// - Blinking R/G: Multiple fault, e.g both an EN fault together with STEP and/or DIR.
+    /// Slot 4: Reserved
+    /// Slot 5: R: Solid
+    bool StepInner() override;
+
+private:
+    // Shared code fault display setup for each axis/slot
+    static void SetFaultDisplay(uint8_t slot, uint8_t mask);
+
+    // Prepares an axis for testing by initializing it and turning off the output.
+    static void PrepareAxis(config::Axis axis);
+
+    uint8_t test_step = 0;
+    config::Axis axis;
+    uint8_t fault_masks[3] = { 0 };
+    ProgressCode next_state = ProgressCode::HWTestBegin;
+    uint16_t wait_start = 0;
+    ml::Mode das_blinken_state = ml::off;
+};
+
+/// The one and only instance of hwSanity state machine in the FW
+extern HWSanity hwSanity;
+
+} // namespace logic

src/logic/progress_codes.h

@@ -41,5 +41,14 @@ enum class ProgressCode : uint_fast8_t {
 
     FeedingToFSensor, // P28
 
+    HWTestBegin, // P29
+    HWTestIdler, // P30
+    HWTestSelector, // P31
+    HWTestPulley, // P32
+    HWTestCleanup, // P33
+    HWTestExec, // P34
+    HWTestDisplay, // P35
+    ErrHwTestFailed, // P36
+
     Empty = 0xff // dummy empty state
 };

src/main.cpp

@@ -27,6 +27,7 @@
 
 #include "application.h"
 
+#include "logic/hw_sanity.h"
 #include "logic/no_command.h"
 
 /// One-time setup of HW and SW components
@@ -112,6 +113,19 @@ static void setup2() {
     }
     ml::leds.Step();
 
+    // Prep hardware sanity:
+    logic::hwSanity.Reset(0);
+
+    while (!logic::hwSanity.StepInner()) {
+        ml::leds.Step();
+    }
+
+    if (logic::hwSanity.Error() != ErrorCode::OK) {
+        // forward the issue to the logic startup handler.
+        logic::noCommand.SetInitError(logic::hwSanity.Error());
+        return;
+    }
+
     // Idler and Selector decide whether homing is possible/safe
     mi::idler.Init();
     ms::selector.Init();
@@ -141,14 +155,17 @@ void Panic(ErrorCode ec) {
 /// The idea behind the Step* routines is to keep each automaton non-blocking allowing for some “concurrency”.
 /// Some FW components will leverage ISR to do their stuff (UART, motor stepping?, etc.)
 void loop() {
+
     mb::buttons.Step();
     ml::leds.Step();
+    if (logic::hwSanity.Error() == ErrorCode::OK) {
         mf::finda.Step();
         mfs::fsensor.Step();
         mi::idler.Step();
         ms::selector.Step();
         mpu::pulley.Step();
         mui::userInput.Step();
+    }
     hal::cpu::Step();
     mu::cdc.Step();
 

src/modules/motion.h

@@ -344,6 +344,13 @@ public:
         return axisData[axis].drv;
     }
 
+    /// @returns the (non-const) TMC2130 driver associated with the particular axis.
+    /// Do not use unless you know exactly what you're doing, (i.e., nothing else can possibly be using
+    /// the axis. Currently the only valid usage is in the hw sanity module.
+    inline hal::tmc2130::TMC2130 &MMU_NEEDS_ATTENTION_DriverForAxis(Axis axis) {
+        return axisData[axis].drv;
+    }
+
     /// @returns the controller associated with the particular axis
     inline const pulse_gen::PulseGen &CtrlForAxis(Axis axis) const {
         return axisData[axis].ctrl;