From 83433cb071729ba3068c5e0750b3fd91888edd2b Mon Sep 17 00:00:00 2001
From: Markus Hitter <mah@jump-ing.de>
Date: Sun, 27 Oct 2013 10:19:49 +0100
Subject: [PATCH] look-ahead: take F_start into account on acceleration
 calculations.

---
 dda.c           | 19 ++++++++++++-------
 dda.h           |  1 +
 dda_lookahead.c |  4 +++-
 3 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/dda.c b/dda.c
index 11e7c01..c48e644 100644
--- a/dda.c
+++ b/dda.c
@@ -96,12 +96,13 @@ void dda_create(DDA *dda, TARGET *target, DDA *prev_dda) {
 	memcpy(&(dda->endpoint), target, sizeof(TARGET));
 
   #ifdef LOOKAHEAD
-  // Set the start and stop speeds to zero for now = full stops between
-  // moves. Also fallback if lookahead calculations fail to finish in time.
-  dda->F_start = 0;
-  dda->F_end = 0;
-  // Give this move an identifier.
-  dda->id = idcnt++;
+    // Set the start and stop speeds to zero for now = full stops between
+    // moves. Also fallback if lookahead calculations fail to finish in time.
+    dda->F_start = 0;
+    dda->start_steps = 0;
+    dda->F_end = 0;
+    // Give this move an identifier.
+    dda->id = idcnt++;
   #endif
 
 // TODO TODO: We should really make up a loop for all axes.
@@ -787,7 +788,11 @@ void dda_clock() {
 
     recalc_speed = 0;
     if (move_step_no < dda->rampup_steps) {
-      dda->n = move_step_no;
+      #ifdef LOOKAHEAD
+        dda->n = dda->start_steps + move_step_no;
+      #else
+        dda->n = move_step_no;
+      #endif
       recalc_speed = 1;
     }
     else if (move_step_no >= dda->rampdown_steps) {
diff --git a/dda.h b/dda.h
index de7c2ac..f77a5ca 100644
--- a/dda.h
+++ b/dda.h
@@ -147,6 +147,7 @@ typedef struct {
   // movement between G1 moves. These variables keep track of the entry and
   // exit speeds between moves.
   uint32_t          F_start;
+  uint32_t          start_steps; ///< steps to reach F_start
   uint32_t          F_end;
   // Displacement vector, in um, based between the difference of the starting
   // point and the target. Required to obtain the jerk between 2 moves.
diff --git a/dda_lookahead.c b/dda_lookahead.c
index 3e20a63..a053033 100644
--- a/dda_lookahead.c
+++ b/dda_lookahead.c
@@ -177,7 +177,7 @@ void dda_join_moves(DDA *prev, DDA *current) {
   // until then, we do not want to touch the current move settings.
   // Note: we assume 'current' will not be dispatched while this function runs, so we do not to
   // back up the move settings: they will remain constant.
-  uint32_t this_F_start, this_rampup, this_rampdown;
+  uint32_t this_F_start, this_start, this_rampup, this_rampdown;
   int32_t jerk, jerk_e;       // Expresses the forces if we would change directions at full speed
   static uint32_t la_cnt = 0;     // Counter: how many moves did we join?
   #ifdef LOOKAHEAD_DEBUG
@@ -400,6 +400,7 @@ void dda_join_moves(DDA *prev, DDA *current) {
     this_rampup = up;
     this_rampdown = current->total_steps - down;
     this_F_start = crossF;
+    this_start = ACCELERATE_RAMP_LEN(this_F_start);
     serprintf(PSTR("Actual crossing speed: %lu\r\n"), crossF);
 
     // Potential reverse processing:
@@ -468,6 +469,7 @@ void dda_join_moves(DDA *prev, DDA *current) {
         current->rampdown_steps = this_rampdown;
         current->F_end = 0;
         current->F_start = this_F_start;
+        current->start_steps = this_start;
         la_cnt++;
       } else
         timeout = 1;