From d7874d5336877efefffde50d74167ab06d65c767 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Tue, 6 Jul 2021 09:12:47 +0200
Subject: [PATCH] PulseGen: implement AbortPlannedMoves

---
 src/modules/pulse_gen.cpp | 18 +++++++++++++++++-
 src/modules/pulse_gen.h   |  3 +++
 2 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp
index 3d38784..2cea500 100644
--- a/src/modules/pulse_gen.cpp
+++ b/src/modules/pulse_gen.cpp
@@ -96,7 +96,7 @@ void PulseGen::Move(pos_t target, steps_t feed_rate) {
         return;
 
     // Direction and speed for this block
-    block->direction = (target > position);
+    block->direction = (target >= position);
     block->nominal_rate = feed_rate;
 
     // Acceleration of the segment, in steps/sec^2
@@ -111,5 +111,21 @@ void PulseGen::Move(pos_t target, steps_t feed_rate) {
     position = target;
 }
 
+void PulseGen::AbortPlannedMoves() {
+    if (!current_block)
+        return;
+
+    // update position
+    steps_t steps_missing = (current_block->steps - steps_completed);
+    if (current_block->direction)
+        position -= steps_missing;
+    else
+        position += steps_missing;
+
+    // destroy the block
+    current_block = nullptr;
+    block_index.pop();
+}
+
 } // namespace motor
 } // namespace modules
diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h
index fb143f8..03d02c3 100644
--- a/src/modules/pulse_gen.h
+++ b/src/modules/pulse_gen.h
@@ -32,6 +32,9 @@ public:
     /// Plan a single move (can only be executed when !Full())
     void Move(pos_t x, steps_t feed_rate);
 
+    /// stop whatever moves are being done
+    void AbortPlannedMoves();
+
     /// @returns the current position of the axis
     pos_t Position() const { return position; }