Make G92 work as expected based on current WCS position

[clinton/Smoothieware.git] / src / modules / robot / Conveyor.cpp

diff --git a/src/modules/robot/Conveyor.cpp b/src/modules/robot/Conveyor.cpp
index 9ec7561..f8b6873 100644 (file)
--- a/src/modules/robot/Conveyor.cpp
+++ b/src/modules/robot/Conveyor.cpp
@@ -55,25 +55,35 @@ using namespace std;
 Conveyor::Conveyor(){
     gc_pending = queue.tail_i;
     running = false;
+    flush = false;
+    halted= false;
 }
 
 void Conveyor::on_module_loaded(){
     register_for_event(ON_IDLE);
     register_for_event(ON_MAIN_LOOP);
-    register_for_event(ON_CONFIG_RELOAD);
+    register_for_event(ON_HALT);
 
     on_config_reload(this);
 }
 
+void Conveyor::on_halt(void* argument){
+    if(argument == nullptr) {
+        halted= true;
+        flush_queue();
+    }else{
+        halted= false;
+    }
+}
+
 // Delete blocks here, because they can't be deleted in interrupt context ( see Block.cpp:release )
 // note that blocks get cleaned as they come off the tail, so head ALWAYS points to a cleaned block.
 void Conveyor::on_idle(void* argument){
     if (queue.tail_i != gc_pending)
     {
-        if (queue.is_empty())
+        if (queue.is_empty()) {
             __debugbreak();
-        else
-        {
+        }else{
             // Cleanly delete block
             Block* block = queue.tail_ref();
 //             block->debug();
@@ -126,7 +136,6 @@ void Conveyor::on_config_reload(void* argument)
 
 void Conveyor::append_gcode(Gcode* gcode)
 {
-    gcode->mark_as_taken();
     queue.head_ref()->append_gcode(gcode);
 }
 
@@ -138,6 +147,13 @@ void Conveyor::on_block_end(void* block)
 
     gc_pending = queue.next(gc_pending);
 
+    // mark entire queue for GC if flush flag is asserted
+    if (flush){
+        while (gc_pending != queue.head_i) {
+            gc_pending = queue.next(gc_pending);
+        }
+    }
+
     // Return if queue is empty
     if (gc_pending == queue.head_i)
     {
@@ -154,8 +170,7 @@ void Conveyor::on_block_end(void* block)
 // Wait for the queue to be empty
 void Conveyor::wait_for_empty_queue()
 {
-    while (!queue.is_empty())
-    {
+    while (!queue.is_empty()) {
         ensure_running();
         THEKERNEL->call_event(ON_IDLE, this);
     }
@@ -166,14 +181,21 @@ void Conveyor::wait_for_empty_queue()
  */
 void Conveyor::queue_head_block()
 {
-    while (queue.is_full())
-    {
+    // upstream caller will block on this until there is room in the queue
+    while (queue.is_full()) {
         ensure_running();
         THEKERNEL->call_event(ON_IDLE, this);
     }
 
-    queue.head_ref()->ready();
-    queue.produce_head();
+    if(halted) {
+        // we do not want to stick more stuff on the queue if we are in halt state
+        // clear and release the block on the head
+        queue.head_ref()->clear();
+
+    }else{
+        queue.head_ref()->ready();
+        queue.produce_head();
+    }
 }
 
 void Conveyor::ensure_running()
@@ -188,6 +210,23 @@ void Conveyor::ensure_running()
     }
 }
 
+/*
+
+    In most cases this will not totally flush the queue, as when streaming
+    gcode there is one stalled waiting for space in the queue, in
+    queue_head_block() so after this flush, once main_loop runs again one more
+    gcode gets stuck in the queue, this is bad. Current work around is to call
+    this when the queue in not full and streaming has stopped
+
+*/
+
+void Conveyor::flush_queue()
+{
+    flush = true;
+    wait_for_empty_queue();
+    flush = false;
+}
+
 // Debug function
 void Conveyor::dump_queue()
 {