From c2a7d7db5decb4daa2d95c4d7be0e475806f79bf Mon Sep 17 00:00:00 2001
From: Alex Voinea <voinea.dragos.alexandru@gmail.com>
Date: Tue, 27 Jul 2021 19:07:38 +0300
Subject: [PATCH] tmc: Error flags

---
 src/hal/avr/tmc2130.cpp | 16 ++++++++++++++--
 src/hal/tmc2130.h       | 14 ++++++++++++--
 2 files changed, 26 insertions(+), 4 deletions(-)

diff --git a/src/hal/avr/tmc2130.cpp b/src/hal/avr/tmc2130.cpp
index 15546e2..8e63fde 100644
--- a/src/hal/avr/tmc2130.cpp
+++ b/src/hal/avr/tmc2130.cpp
@@ -94,6 +94,18 @@ void TMC2130::ClearStallguard(const MotorParams &params) {
     sg_counter = 4 * (1 << (8 - params.uSteps)) - 1; /// one electrical full step (4 steps when fullstepping)
 }
 
+bool TMC2130::CheckForErrors(const MotorParams &params) {
+    uint32_t GSTAT = ReadRegister(params, Registers::GSTAT);
+    uint32_t DRV_STATUS = ReadRegister(params, Registers::DRV_STATUS);
+    errorFlags.reset_flag |= GSTAT & (1 << 0);
+    errorFlags.uv_cp = GSTAT & (1 << 2);
+    errorFlags.s2g = DRV_STATUS & (3ul << 27);
+    errorFlags.otpw = DRV_STATUS & (1ul << 26);
+    errorFlags.ot = DRV_STATUS & (1ul << 25);
+
+    return GSTAT || errorFlags.reset_flag; //any bit in gstat is an error
+}
+
 uint32_t TMC2130::ReadRegister(const MotorParams &params, Registers reg) {
     uint8_t pData[5] = { (uint8_t)reg };
     _spi_tx_rx(params, pData);
@@ -129,8 +141,8 @@ void TMC2130::_spi_tx_rx(const MotorParams &params, uint8_t (&pData)[5]) {
 }
 
 void TMC2130::_handle_spi_status(const MotorParams &params, uint8_t status) {
-    errorFlags.reset_flag |= status & (1 << 0);
-    errorFlags.driver_error |= status & (1 << 1);
+    // errorFlags.reset_flag |= status & (1 << 0);
+    // errorFlags.driver_error |= status & (1 << 1);
 }
 
 } // namespace tmc2130
diff --git a/src/hal/tmc2130.h b/src/hal/tmc2130.h
index add8419..cd1c9e4 100644
--- a/src/hal/tmc2130.h
+++ b/src/hal/tmc2130.h
@@ -35,9 +35,12 @@ struct MotorCurrents {
 class TMC2130 {
     MotorMode mode;
     MotorCurrents currents;
-    struct __attribute__((packed)) {
+    struct __attribute__((packed)) ErrorFlags {
         uint8_t reset_flag : 1;
-        uint8_t driver_error : 1;
+        uint8_t uv_cp : 1;
+        uint8_t s2g : 1;
+        uint8_t otpw : 1;
+        uint8_t ot : 1;
     } errorFlags;
     bool enabled = false;
     uint8_t sg_counter;
@@ -118,6 +121,13 @@ public:
 
     void ClearStallguard(const MotorParams &params);
 
+    /// Should be called periodically from main loop. Maybe not all the time. Once every 10 ms is probably enough
+    bool CheckForErrors(const MotorParams &params);
+
+    inline ErrorFlags GetErrorFlags() const {
+        return errorFlags;
+    }
+
     /// Reads a driver register and updates the status flags
     uint32_t ReadRegister(const MotorParams &params, Registers reg);