Implement triffid hunters flush queue code in Conveyor

[clinton/Smoothieware.git] / src / modules / communication / GcodeDispatch.cpp

diff --git a/src/modules/communication/GcodeDispatch.cpp b/src/modules/communication/GcodeDispatch.cpp
index 5ba6b07..7aa1dfd 100644 (file)
--- a/src/modules/communication/GcodeDispatch.cpp
+++ b/src/modules/communication/GcodeDispatch.cpp
@@ -22,16 +22,28 @@ using std::string;
 #include "checksumm.h"
 #include "ConfigValue.h"
 
-GcodeDispatch::GcodeDispatch() {}
+#define return_error_on_unhandled_gcode_checksum    CHECKSUM("return_error_on_unhandled_gcode")
+
+GcodeDispatch::GcodeDispatch()
+{
+    halted= false;
+    uploading = false;
+    currentline = -1;
+    last_g= 255;
+}
 
 // Called when the module has just been loaded
 void GcodeDispatch::on_module_loaded()
 {
     return_error_on_unhandled_gcode = THEKERNEL->config->value( return_error_on_unhandled_gcode_checksum )->by_default(false)->as_bool();
     this->register_for_event(ON_CONSOLE_LINE_RECEIVED);
-    currentline = -1;
-    uploading = false;
-    last_g= 255;
+    this->register_for_event(ON_HALT);
+}
+
+void GcodeDispatch::on_halt(void *arg)
+{
+    // set halt stream and ignore everything until M999
+    this->halted= true;
 }
 
 // When a command is received, if it is a Gcode, dispatch it as an object via an event
@@ -108,13 +120,27 @@ try_again:
                     possible_command = possible_command.substr(nextcmd);
                 }
 
+
                 if(!uploading) {
                     //Prepare gcode for dispatch
                     Gcode *gcode = new Gcode(single_command, new_message.stream);
 
+                    if(halted) {
+                        // we ignore all commands until M999
+                        if(gcode->has_m && gcode->m == 999) {
+                            halted= false;
+                            new_message.stream->printf("ok\r\n");
+                        }else{
+                            new_message.stream->printf("!!\r\n");
+                        }
+                        delete gcode;
+                        continue;
+                    }
+
                     if(gcode->has_g) {
                         last_g= gcode->g;
                     }
+
                     if(gcode->has_m) {
                         switch (gcode->m) {
                             case 28: // start upload command
@@ -134,11 +160,10 @@ try_again:
 
                             case 112: // emergency stop, do the best we can with this
                                 // TODO this really needs to be handled out-of-band
-                                // stops block queue
-                                THEKERNEL->pauser->take();
-                                // disables heaters and motors
+                                // disables heaters and motors, ignores further incoming Gcode and clears block queue
                                 THEKERNEL->call_event(ON_HALT);
-                                THEKERNEL->streams->printf("ok Emergency Stop Requested - reset required to continue\r\n");
+                                THEKERNEL->streams->printf("ok Emergency Stop Requested - reset or M999 required to continue\r\n");
+                                delete gcode;
                                 return;
 
                             case 500: // M500 save volatile settings to config-override