From 8149853fc90cd0553e85a3a6a6f6201a677def36 Mon Sep 17 00:00:00 2001
From: Alex Voinea <voinea.dragos.alexandru@gmail.com>
Date: Thu, 22 Jun 2023 13:41:02 +0200
Subject: [PATCH] Optimize `st_set_position()` for speed and size

flash: -186
SRAM: 0
time: x0.45
---
 Firmware/planner.cpp |  4 ++--
 Firmware/stepper.cpp | 11 ++++-------
 Firmware/stepper.h   |  2 +-
 3 files changed, 7 insertions(+), 10 deletions(-)

diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp
index 001b2137b..9406fe839 100644
--- a/Firmware/planner.cpp
+++ b/Firmware/planner.cpp
@@ -1330,7 +1330,7 @@ void plan_set_position(float x, float y, float z, const float &e)
   position_float[Z_AXIS] = z;
   position_float[E_AXIS] = e;
   #endif
-  st_set_position(position[X_AXIS], position[Y_AXIS], position[Z_AXIS], position[E_AXIS]);
+  st_set_position(position);
   previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
   memset(previous_speed, 0, sizeof(previous_speed));
 }
@@ -1342,7 +1342,7 @@ void plan_set_z_position(const float &z)
   position_float[Z_AXIS] = z;
   #endif
   position[Z_AXIS] = lround(z*cs.axis_steps_per_unit[Z_AXIS]);
-  st_set_position(position[X_AXIS], position[Y_AXIS], position[Z_AXIS], position[E_AXIS]);
+  st_set_position(position);
 }
 
 void plan_set_e_position(const float &e)
diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp
index 3e7e4c72c..b3ea244af 100644
--- a/Firmware/stepper.cpp
+++ b/Firmware/stepper.cpp
@@ -1313,18 +1313,15 @@ void st_synchronize()
 	}
 }
 
-void st_set_position(const long &x, const long &y, const long &z, const long &e)
+void st_set_position(const long *pos)
 {
   CRITICAL_SECTION_START;
   // Copy 4x4B.
-  // This block locks the interrupts globally for 4.56 us,
-  // which corresponds to a maximum repeat frequency of 219.18 kHz.
+  // This block locks the interrupts globally for 2.06 us,
+  // which corresponds to a maximum repeat frequency of ~484kHz.
   // This blocking is safe in the context of a 10kHz stepper driver interrupt
   // or a 115200 Bd serial line receive interrupt, which will not trigger faster than 12kHz.
-  count_position[X_AXIS] = x;
-  count_position[Y_AXIS] = y;
-  count_position[Z_AXIS] = z;
-  count_position[E_AXIS] = e;
+  memcpy((uint8_t *)count_position, pos, sizeof(count_position));
   CRITICAL_SECTION_END;
 }
 
diff --git a/Firmware/stepper.h b/Firmware/stepper.h
index 944403fe4..72ea5294a 100644
--- a/Firmware/stepper.h
+++ b/Firmware/stepper.h
@@ -41,7 +41,7 @@ void isr();
 void st_synchronize();
 
 // Set current position in steps
-void st_set_position(const long &x, const long &y, const long &z, const long &e);
+void st_set_position(const long *pos);
 void st_set_e_position(const long &e);
 
 // Get current position in steps