From c4e9e624f588be9383b5cd55023fdeec79ac6488 Mon Sep 17 00:00:00 2001
From: Robert Pelnar <robert.pelnar@seznam.cz>
Date: Mon, 18 Sep 2017 19:36:18 +0200
Subject: [PATCH] cmd queue and planner - functions for calculating sd
 position: cmdqueue_calc_sd_length planner_calc_sd_length

---
 Firmware/cmdqueue.cpp | 17 +++++++++++++++++
 Firmware/cmdqueue.h   |  1 +
 Firmware/planner.cpp  | 20 ++++++++++++++++++--
 Firmware/planner.h    |  8 ++++++--
 4 files changed, 42 insertions(+), 4 deletions(-)

diff --git a/Firmware/cmdqueue.cpp b/Firmware/cmdqueue.cpp
index 9f134c71..cd0a86b9 100644
--- a/Firmware/cmdqueue.cpp
+++ b/Firmware/cmdqueue.cpp
@@ -601,3 +601,20 @@ void get_command()
 
   #endif //SDSUPPORT
 }
+
+uint16_t cmdqueue_calc_sd_length()
+{
+	int _buflen = buflen;
+	int _bufindr = bufindr;
+	uint16_t sdlen = 0;
+	while (_buflen--)
+	{
+		if (cmdbuffer[_bufindr] == CMDBUFFER_CURRENT_TYPE_SDCARD)
+			sdlen += cmdbuffer[_bufindr + 1];
+		//skip header, skip command
+		for (_bufindr += CMDHDRSIZE; cmdbuffer[_bufindr] != 0; ++ _bufindr) ;
+		//skip zeros
+		for (++ _bufindr; _bufindr < sizeof(cmdbuffer) && cmdbuffer[_bufindr] == 0; ++ _bufindr) ;
+	}
+    return sdlen;
+}
diff --git a/Firmware/cmdqueue.h b/Firmware/cmdqueue.h
index f029d43a..adeaf6b5 100644
--- a/Firmware/cmdqueue.h
+++ b/Firmware/cmdqueue.h
@@ -62,6 +62,7 @@ extern void enquecommand_front(const char *cmd, bool from_progmem);
 extern void repeatcommand_front();
 extern bool is_buffer_empty();
 extern void get_command();
+extern uint16_t cmdqueue_calc_sd_length();
 
 // Return True if a character was found
 static inline bool    code_seen(char code) { return (strchr_pointer = strchr(CMDBUFFER_CURRENT_STRING, code)) != NULL; }
diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp
index f7e61802..341f3e5f 100644
--- a/Firmware/planner.cpp
+++ b/Firmware/planner.cpp
@@ -593,7 +593,7 @@ float junction_deviation = 0.1;
 // Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in 
 // mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
 // calculation the caller must also provide the physical length of the line in millimeters.
-void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder)
+void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder, uint8_t sdlen)
 {
     // Calculate the buffer head after we push this byte
   int next_buffer_head = next_block_index(block_buffer_head);
@@ -723,6 +723,9 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate
   // Prepare to set up new block
   block_t *block = &block_buffer[block_buffer_head];
 
+  // Set sdlen for calculating sd position
+  block->sdlen = sdlen;
+
   // Mark block as not busy (Not executed by the stepper interrupt, could be still tinkered with.)
   block->busy = false;
 
@@ -1295,4 +1298,17 @@ void planner_queue_min_reset()
 {
   g_cntr_planner_queue_min = moves_planned();
 }
-#endif /* PLANNER_DIAGNOSTICS */
\ No newline at end of file
+#endif /* PLANNER_DIAGNOSTICS */
+
+uint16_t planner_calc_sd_length()
+{
+	unsigned char _block_buffer_head = block_buffer_head;
+	unsigned char _block_buffer_tail = block_buffer_tail;
+	uint16_t sdlen = 0;
+	while (_block_buffer_head != _block_buffer_tail)
+	{
+		sdlen += block_buffer[_block_buffer_tail].sdlen;
+	    _block_buffer_tail = (_block_buffer_tail + 1) & (BLOCK_BUFFER_SIZE - 1);  
+	}
+	return sdlen;
+}
diff --git a/Firmware/planner.h b/Firmware/planner.h
index 96ab83bc..12b1931c 100644
--- a/Firmware/planner.h
+++ b/Firmware/planner.h
@@ -93,6 +93,8 @@ typedef struct {
   bool use_advance_lead;
   unsigned long abs_adv_steps_multiplier8; // Factorised by 2^8 to avoid float
 #endif
+
+  uint8_t sdlen;
 } block_t;
 
 #ifdef LIN_ADVANCE
@@ -111,12 +113,12 @@ void plan_init();
 // millimaters. Feed rate specifies the speed of the motion.
 
 #ifdef ENABLE_AUTO_BED_LEVELING
-void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder);
+void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder, uint8_t sdlen = 0);
 
 // Get the position applying the bed level matrix if enabled
 vector_3 plan_get_position();
 #else
-void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder);
+void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder, uint8_t sdlen = 0);
 //void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder);
 #endif // ENABLE_AUTO_BED_LEVELING
 
@@ -217,3 +219,5 @@ extern uint8_t planner_queue_min();
 // Diagnostic function: Reset the minimum planner segments.
 extern void planner_queue_min_reset();
 #endif /* PLANNER_DIAGNOSTICS */
+
+extern uint16_t planner_calc_sd_length();