diff --git a/Core/Inc/FreeRTOSConfig.h b/Core/Inc/FreeRTOSConfig.h
index 11bf23c..4e6a824 100644
--- a/Core/Inc/FreeRTOSConfig.h
+++ b/Core/Inc/FreeRTOSConfig.h
@@ -68,7 +68,7 @@
 #define configTICK_RATE_HZ                       ((TickType_t)1000)
 #define configMAX_PRIORITIES                     ( 7 )
 #define configMINIMAL_STACK_SIZE                 ((uint16_t)512)
-#define configTOTAL_HEAP_SIZE                    ((size_t)40000)
+#define configTOTAL_HEAP_SIZE                    ((size_t)50000)
 #define configMAX_TASK_NAME_LEN                  ( 16 )
 #define configGENERATE_RUN_TIME_STATS            1
 #define configUSE_TRACE_FACILITY                 1
diff --git a/sdk/components/tmc/ic/ztmc5130.cpp b/sdk/components/tmc/ic/ztmc5130.cpp
index 0bf9828..f510dd8 100644
--- a/sdk/components/tmc/ic/ztmc5130.cpp
+++ b/sdk/components/tmc/ic/ztmc5130.cpp
@@ -47,7 +47,6 @@ void TMC51X0::initialize(cfg_t *cfg) {
 }
 
 void TMC51X0::setMRES(mres_type_t value) {
-  PRV_FIELD_WRITE(TMC5130_CHOPCONF, TMC5130_MRES_MASK, TMC5130_MRES_SHIFT, value);
 
   if (value == kmres_256) {
     m_onecirclepulse = 51200;
@@ -68,11 +67,12 @@ void TMC51X0::setMRES(mres_type_t value) {
   } else if (value == kmres_1) {
     m_onecirclepulse = 200;
   } else {
-    ZASSERT(false);
     m_onecirclepulse = 51200;
     value            = kmres_256;
   }
   m_MRES = value;
+  PRV_FIELD_WRITE(TMC5130_CHOPCONF, TMC5130_MRES_MASK, TMC5130_MRES_SHIFT, value);
+
 }
 
 void TMC51X0::setNoAccLimit(bool enable) {