From 6f518f1ed268ed664409b5e823a805f71abfa1db Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Tue, 6 Jul 2021 08:55:02 +0200
Subject: [PATCH] PulseGen: complete implementation

---
 src/config/config.h       |  3 +++
 src/modules/pulse_gen.cpp | 25 ++++++++++++-------------
 src/modules/pulse_gen.h   | 11 ++++++-----
 3 files changed, 21 insertions(+), 18 deletions(-)

diff --git a/src/config/config.h b/src/config/config.h
index b5c139c..66080d9 100644
--- a/src/config/config.h
+++ b/src/config/config.h
@@ -41,6 +41,9 @@ static constexpr uint16_t maxStepFrequency = 40000;
 /// Minimum stepping rate 120Hz
 static constexpr uint16_t minStepRate = 120;
 
+/// Size for the motion planner block buffer size
+static constexpr uint8_t blockBufferSize = 2;
+
 /// Idler configuration
 static constexpr AxisConfig idler = {
     .dirOn = true,
diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp
index 0c94e35..09e4aef 100644
--- a/src/modules/pulse_gen.cpp
+++ b/src/modules/pulse_gen.cpp
@@ -16,7 +16,6 @@ PulseGen::PulseGen(steps_t max_jerk, steps_t acceleration) {
     this->acceleration = acceleration;
 
     // Block buffer
-    block_buffer_head = block_buffer_tail = 0;
     current_block = nullptr;
 }
 
@@ -94,7 +93,7 @@ void PulseGen::CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t e
 
 void PulseGen::Move(pos_t target, steps_t feed_rate) {
     // Prepare to set up new block
-    block_t *block = &block_buffer[block_buffer_head];
+    block_t *block = &block_buffer[block_index.back()];
 
     block->steps = abs(target - position);
 
@@ -113,17 +112,16 @@ void PulseGen::Move(pos_t target, steps_t feed_rate) {
     // Perform the trapezoid calculations
     CalculateTrapezoid(block, max_jerk, max_jerk);
 
-    // TODO: Move the buffer head
-    //block_buffer_head++;
-
+    // Move forward
+    block_index.push();
     position = target;
 }
 
 st_timer_t PulseGen::Step(const MotorParams &motorParams) {
     if (!current_block) {
-        // TODO: fetch next block
-        if (!block_buffer_head)
-            current_block = &block_buffer[block_buffer_head++];
+        // fetch next block
+        if (!block_index.empty())
+            current_block = &block_buffer[block_index.front()];
         if (!current_block)
             return 0;
 
@@ -136,9 +134,9 @@ st_timer_t PulseGen::Step(const MotorParams &motorParams) {
         acceleration_time = calc_timer(acc_step_rate, step_loops);
         steps_completed = 0;
 
-        // Set the nominal step loops to zero to indicate, that the timer value is not known yet.
-        // That means, delay the initialization of nominal step rate and step loops until the steady
-        // state is reached.
+        // Set the nominal step loops to zero to indicate, that the timer value is not
+        // known yet. That means, delay the initialization of nominal step rate and step
+        // loops until the steady state is reached.
         step_loops_nominal = 0;
     }
 
@@ -181,8 +179,8 @@ st_timer_t PulseGen::Step(const MotorParams &motorParams) {
         deceleration_time += timer;
     } else {
         if (!step_loops_nominal) {
-            // Calculation of the steady state timer rate has been delayed to the 1st tick of the steady state to lower
-            // the initial interrupt blocking.
+            // Calculation of the steady state timer rate has been delayed to the 1st tick
+            // of the steady state to lower the initial interrupt blocking.
             timer_nominal = calc_timer(uint16_t(current_block->nominal_rate), step_loops);
             step_loops_nominal = step_loops;
         }
@@ -192,6 +190,7 @@ st_timer_t PulseGen::Step(const MotorParams &motorParams) {
     // If current block is finished, reset pointer
     if (steps_completed >= current_block->steps) {
         current_block = nullptr;
+        block_index.pop();
     }
 
     return timer;
diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h
index 5485dec..201dd30 100644
--- a/src/modules/pulse_gen.h
+++ b/src/modules/pulse_gen.h
@@ -2,12 +2,14 @@
 #include <stdint.h>
 #include "speed_table.h"
 #include "../hal/tmc2130.h"
+#include "../hal/circular_buffer.h"
 
 namespace modules {
 
 /// Acceleration ramp and stepper pulse generator
 namespace pulse_gen {
 
+using config::blockBufferSize;
 using speed_table::st_timer_t;
 typedef uint32_t steps_t; ///< Absolute step units
 typedef uint32_t rate_t; ///< Type for step rates
@@ -33,10 +35,10 @@ public:
     void SetPosition(pos_t x) { position = x; }
 
     /// @returns true if all planned moves have been finished
-    bool QueueEmpty() const;
+    bool QueueEmpty() const { return block_index.empty(); }
 
     /// @returns false if new moves can still be planned
-    bool Full() const;
+    bool Full() const { return block_index.full(); }
 
     /// Single-step the axis
     /// @returns the interval for the next tick
@@ -60,10 +62,9 @@ private:
     };
 
     // Block buffer parameters
-    block_t block_buffer[2];
+    block_t block_buffer[blockBufferSize];
+    CircularIndex<uint8_t, blockBufferSize> block_index;
     block_t *current_block;
-    uint8_t block_buffer_head;
-    uint8_t block_buffer_tail;
 
     // Axis data
     pos_t position; ///< Current axis position