Split step function into two parts to reduce the amount of time that locks must be held

firmware/jerk_control/src/executor.rs

@@ -32,10 +32,9 @@ impl CommandQueue {
         self.segments[self.cur_segment as usize].delta.iter().any(|d| d.0 != 0)
     }
 
-    // Returns true if step signal should be high
-    pub fn step(&mut self) -> bool {
-        let next_seg_idx = (self.cur_segment + 1) % self.segments.len() as u8;
-        let next_seg_start = self.segments[next_seg_idx as usize].start_time;
+    // Returns true if step signal should be high. This does the first, constant time part of stepping.
+    // Note that step_late needs to be called as well to advance the segment.
+    pub fn step_early(&mut self) -> bool {
 
         let cur_segment = &mut self.segments[self.cur_segment as usize];
 
@@ -44,16 +43,29 @@ impl CommandQueue {
         cur_segment.delta[0] += cur_segment.delta[1];
         cur_segment.delta[1] += cur_segment.delta[2];
 
+        // return state of step line
+        return self.p.0 > self.step_size / 2;
+    }
+
+    // Perform the variable-time part of stepping. This should be called after step_early.
+    pub fn step_late(&mut self) {
         // Advance the segment if necessary.
+        let next_seg_idx = (self.cur_segment + 1) % self.segments.len() as u8;
+        let next_seg_start = self.segments[next_seg_idx as usize].start_time;
+
         self.cur_time += Wrapping(1);
         if self.cur_time.0 >= next_seg_start && next_seg_start != 0 {
+            let cur_segment = &mut self.segments[self.cur_segment as usize];
             self.cur_segment = next_seg_idx;
             cur_segment.start_time = 0;
         }
+    }
 
-
-        // return state of step line
-        return self.p.0 < self.step_size / 2;
+    // Shortcut to calling both step_early and step_late, for non-demanding applications.
+    pub fn step(&mut self) -> bool {
+        let result = self.step_early();
+        self.step_late();
+        result
     }
 
     pub fn direction(& self) -> bool {