Improve implementation

src/application.cpp

@@ -32,7 +32,7 @@ Application application;
 
 Application::Application()
     : lastCommandProcessedMs(0)
-    , currentCommand(&logic::hwSanity)
+    , currentCommand(&logic::noCommand)
     , currentCommandRq(mp::RequestMsgCodes::Reset, 0) {}
 
 void Application::CheckManualOperation() {

src/hal/tmc2130.cpp

@@ -7,6 +7,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;
@@ -27,7 +29,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
@@ -73,6 +75,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

@@ -88,6 +88,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);
 
@@ -104,6 +107,12 @@ 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

src/logic/error_codes.h

@@ -70,13 +70,6 @@ enum class ErrorCode : uint_fast16_t {
     /// - E34240 All 3 TMC driver
     TMC_RESET = 0x8400,
 
-    /// 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.
-    /// TODO: DRRacer - Separate codes per channel.
-    TMC_PINS_UNRELIABLE = 0x8600,
-
     /// not enough current for the TMC, NOT RECOVERABLE
     /// - E34880 Pulley TMC driver
     /// - E34944 Selector TMC driver
@@ -107,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

@@ -4,7 +4,6 @@
 #include "../modules/motion.h"
 #include "../modules/leds.h"
 #include "../modules/timebase.h"
-#include "config/axis.h"
 
 namespace logic {
 
@@ -23,13 +22,6 @@ static_assert(TEST_PASSES < 32); // Would overflow counters
 
 HWSanity hwSanity;
 
-uint8_t HWSanity::test_step = 0;
-uint8_t HWSanity::fault_masks[] = { 0 };
-uint16_t HWSanity::wait_start = 0;
-ml::Mode das_blinken_state = ml::off;
-Axis HWSanity::axis;
-ProgressCode HWSanity::next_state = ProgressCode::HWTestBegin;
-
 bool HWSanity::Reset(uint8_t param) {
     state = ProgressCode::HWTestBegin;
     error = ErrorCode::RUNNING;
@@ -62,12 +54,18 @@ void HWSanity::SetFaultDisplay(uint8_t slot, uint8_t mask) {
     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:
         //auto& driver = mm::motion.DriverForAxis(config::Axis::Pulley);
         test_step = 0;
-        // Todo - set TOFF so the output bridge is disabled.
         state = ProgressCode::HWTestIdler;
         break;
     case ProgressCode::HWTestIdler:
@@ -75,18 +73,21 @@ bool HWSanity::StepInner() {
         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:
@@ -103,16 +104,15 @@ bool HWSanity::StepInner() {
         /* FALLTHRU */
     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;
-            //auto* driver = &mm::motion.DriverForAxis(axis);
             // The order of the bits here is roughly the same as that of IOIN.
-            mm::motion.DriverForAxis(axis).SetDir(params, set_state & BIT_DIR);
+            driver.SetDir(params, set_state & BIT_DIR);
-            mm::motion.DriverForAxis(axis).SetStep(params, set_state & BIT_STEP);
+            driver.SetStep(params, set_state & BIT_STEP);
-            ml::leds.SetPairButOffOthers(3, ml::on, ml::off);
+            driver.SetEnabled(params, set_state & BIT_ENA);
-            //mm::motion.DriverForAxis(axis).SetEnabled(params, set_state & BIT_ENA);
+            uint32_t drv_ioin = driver.ReadRegister(params, hal::tmc2130::TMC2130::Registers::IOIN);
-            uint32_t drv_ioin = const_cast<hal::tmc2130::TMC2130 &>(mm::motion.DriverForAxis(axis)).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);
@@ -131,6 +131,7 @@ bool HWSanity::StepInner() {
             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;
         }
@@ -139,20 +140,17 @@ bool HWSanity::StepInner() {
         if (fault_masks[0] || fault_masks[1] || fault_masks[2]) {
             // error, display it and return the code.
             state = ProgressCode::ErrHwTestFailed;
-            error = ErrorCode::TMC_PINS_UNRELIABLE;
+            error = ErrorCode::MMU_SOLDERING_NEEDS_ATTENTION;
             uint8_t mask = fault_masks[Axis::Idler];
             if (mask) {
-                error |= ErrorCode::TMC_IDLER_BIT;
                 SetFaultDisplay(0, mask);
             }
             mask = fault_masks[Axis::Pulley];
             if (mask) {
-                error |= ErrorCode::TMC_PULLEY_BIT;
                 SetFaultDisplay(2, mask);
             }
             mask = fault_masks[Axis::Selector];
             if (mask) {
-                error |= ErrorCode::TMC_SELECTOR_BIT;
                 SetFaultDisplay(1, mask);
             }
             ml::leds.SetMode(3, ml::red, ml::off);
@@ -161,7 +159,7 @@ bool HWSanity::StepInner() {
             ml::leds.SetMode(4, ml::green, ml::off);
             return true;
         } else {
-            //TODO: Re-enable TOFF here
+            ml::leds.SetPairButOffOthers(0, ml::off, ml::off);
             FinishedOK();
         }
     case ProgressCode::OK:

src/logic/hw_sanity.h

@@ -2,7 +2,8 @@
 #pragma once
 #include <stdint.h>
 #include "command_base.h"
-#include "config/axis.h"
+#include "../config/axis.h"
+#include "../modules/leds.h"
 
 namespace logic {
 
@@ -38,13 +39,18 @@ public:
     bool StepInner() override;
 
 private:
+    // Shared code fault display setup for each axis/slot
     static void SetFaultDisplay(uint8_t slot, uint8_t mask);
 
-    static uint8_t test_step;
+    // Prepares an axis for testing by initializing it and turning off the output.
-    static config::Axis axis;
+    static void PrepareAxis(config::Axis axis);
-    static uint8_t fault_masks[3];
+
-    static ProgressCode next_state;
+    uint8_t test_step = 0;
-    static uint16_t wait_start;
+    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

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

@@ -340,6 +340,13 @@ public:
         return axisData[axis].drv;
     }
 
+    /// @returns the (non-const) TMC213 driver associated with the particular axis.
+    /// Do not use unless you know exactly what you're doing, 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;