From 94f11642e091da55dddb9674df79a0785f3a8325 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Tue, 6 Jul 2021 16:10:16 +0200
Subject: [PATCH] PulseGen: perform an extra queue check in Move()

Since scheduling a move on a block which is being executed will jolt the
motors, be extra-safe and perform an extra lower-level check before
committing even if the caller is responsible.

Return the status, which can be useful to build a simple busy loop.
---
 src/modules/pulse_gen.cpp | 14 +++++++++-----
 src/modules/pulse_gen.h   |  5 +++--
 2 files changed, 12 insertions(+), 7 deletions(-)

diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp
index 2cea500..6fe2ba1 100644
--- a/src/modules/pulse_gen.cpp
+++ b/src/modules/pulse_gen.cpp
@@ -85,15 +85,18 @@ void PulseGen::CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t e
     block->final_rate = final_rate;
 }
 
-void PulseGen::Move(pos_t target, steps_t feed_rate) {
+bool PulseGen::Move(pos_t target, steps_t feed_rate) {
     // Prepare to set up new block
+    if (block_index.full())
+        return false;
     block_t *block = &block_buffer[block_index.back()];
 
-    block->steps = abs(target - position);
-
     // Bail if this is a zero-length block
-    if (block->steps <= config::dropSegments)
-        return;
+    block->steps = abs(target - position);
+    if (block->steps <= config::dropSegments) {
+        // behave as-if the block as been scheduled
+        return true;
+    }
 
     // Direction and speed for this block
     block->direction = (target >= position);
@@ -109,6 +112,7 @@ void PulseGen::Move(pos_t target, steps_t feed_rate) {
     // Move forward
     block_index.push();
     position = target;
+    return true;
 }
 
 void PulseGen::AbortPlannedMoves() {
diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h
index f65268a..219acbd 100644
--- a/src/modules/pulse_gen.h
+++ b/src/modules/pulse_gen.h
@@ -28,8 +28,9 @@ public:
     /// Set acceleration for the axis
     void SetAcceleration(steps_t accel) { acceleration = accel; }
 
-    /// Plan a single move (can only be executed when !Full())
-    void Move(pos_t x, steps_t feed_rate);
+    /// Plan a single move (can only be executed when not Full())
+    /// @returns True if the move has been planned
+    bool Move(pos_t x, steps_t feed_rate);
 
     /// stop whatever moves are being done
     void AbortPlannedMoves();