From 5ca8d51e123463004626e853a930e2f88febd66d Mon Sep 17 00:00:00 2001
From: Robby Candra <robbycandra.mail@gmail.com>
Date: Sat, 8 Jun 2019 17:23:53 +0700
Subject: [PATCH] Ability to insert G-code in front of queue (#14229)
---
Marlin/src/Marlin.cpp | 2 +-
Marlin/src/gcode/queue.cpp | 65 ++++++++++++++++++++++++++++++++-----
Marlin/src/gcode/queue.h | 13 +++++++-
Marlin/src/lcd/ultralcd.cpp | 2 +-
4 files changed, 71 insertions(+), 11 deletions(-)
diff --git a/Marlin/src/Marlin.cpp b/Marlin/src/Marlin.cpp
index 4d1c66d6f7..fdc6649799 100644
--- a/Marlin/src/Marlin.cpp
+++ b/Marlin/src/Marlin.cpp
@@ -381,7 +381,7 @@ void disable_all_steppers() {
#endif // HOST_ACTION_COMMANDS
if (run_runout_script)
- enqueue_and_echo_commands_P(PSTR(FILAMENT_RUNOUT_SCRIPT));
+ enqueue_and_echo_commands_front_P(PSTR(FILAMENT_RUNOUT_SCRIPT));
}
#endif // HAS_FILAMENT_SENSOR
diff --git a/Marlin/src/gcode/queue.cpp b/Marlin/src/gcode/queue.cpp
index d8bd91c5f3..afc76bbe1f 100644
--- a/Marlin/src/gcode/queue.cpp
+++ b/Marlin/src/gcode/queue.cpp
@@ -132,6 +132,7 @@ inline bool _enqueuecommand(const char* cmd, bool say_ok=false
/**
* Enqueue with Serial Echo
+ * Return true if the command was consumed
*/
bool enqueue_and_echo_command(const char* cmd) {
@@ -139,10 +140,7 @@ bool enqueue_and_echo_command(const char* cmd) {
//SERIAL_ECHO(cmd);
//SERIAL_ECHOPGM("\") \n");
- if (*cmd == 0 || *cmd == '\n' || *cmd == '\r') {
- //SERIAL_ECHOLNPGM("Null command found... Did not queue!");
- return true;
- }
+ if (*cmd == 0 || *cmd == '\n' || *cmd == '\r') return true;
if (_enqueuecommand(cmd)) {
SERIAL_ECHO_START();
@@ -152,30 +150,81 @@ bool enqueue_and_echo_command(const char* cmd) {
return false;
}
+#if HAS_QUEUE_FRONT
+
+ bool early_cmd; // = false
+
+ /**
+ * Insert a high Priority command from RAM into the main command buffer.
+ * Return true if the command was consumed
+ * Return false for a full buffer, or if the 'command' is a comment.
+ */
+ inline bool _enqueuecommand_front(const char* cmd) {
+ if (*cmd == 0 || *cmd == '\n' || *cmd == '\r') return true;
+ if (*cmd == ';' || commands_in_queue >= BUFSIZE) return false;
+ if (cmd_queue_index_r == 0) cmd_queue_index_r = BUFSIZE;
+ --cmd_queue_index_r;
+ strcpy(command_queue[cmd_queue_index_r], cmd);
+ send_ok[cmd_queue_index_r] = false;
+ #if NUM_SERIAL > 1
+ command_queue_port[cmd_queue_index_r] = -1;
+ #endif
+ commands_in_queue++;
+ return true;
+ }
+
+ /**
+ * Insert in the front of queue, one or many commands to run from program memory.
+ * Aborts the current queue, if any.
+ * Note: drain_injected_commands_P() must be called repeatedly to drain the commands afterwards
+ */
+ void enqueue_and_echo_commands_front_P(PGM_P const pgcode) {
+ early_cmd = true;
+ enqueue_and_echo_commands_P(pgcode);
+ }
+
+#endif
+
/**
* Inject the next "immediate" command, when possible, onto the front of the queue.
* Return true if any immediate commands remain to inject.
+ * Do not inject a comment or use leading space!.
*/
static bool drain_injected_commands_P() {
- if (injected_commands_P != nullptr) {
+ while (injected_commands_P != nullptr) {
size_t i = 0;
char c, cmd[60];
strncpy_P(cmd, injected_commands_P, sizeof(cmd) - 1);
cmd[sizeof(cmd) - 1] = '\0';
while ((c = cmd[i]) && c != '\n') i++; // find the end of this gcode command
cmd[i] = '\0';
- if (enqueue_and_echo_command(cmd)) // success?
+
+ if (
+ #if HAS_QUEUE_FRONT
+ early_cmd ? _enqueuecommand_front(cmd) :
+ #endif
+ enqueue_and_echo_command(cmd)
+ ) {
injected_commands_P = c ? injected_commands_P + i + 1 : nullptr; // next command or done
+ #if HAS_QUEUE_FRONT
+ if (!c) early_cmd = false;
+ #endif
+ }
+ else
+ return true; // buffer is full (or command is comment);
}
- return (injected_commands_P != nullptr); // return whether any more remain
+ return false; // return whether any more remain
}
/**
- * Record one or many commands to run from program memory.
+ * Enqueue one or many commands to run from program memory.
* Aborts the current queue, if any.
* Note: drain_injected_commands_P() must be called repeatedly to drain the commands afterwards
*/
void enqueue_and_echo_commands_P(PGM_P const pgcode) {
+ #if HAS_QUEUE_FRONT
+ early_cmd = false;
+ #endif
injected_commands_P = pgcode;
(void)drain_injected_commands_P(); // first command executed asap (when possible)
}
diff --git a/Marlin/src/gcode/queue.h b/Marlin/src/gcode/queue.h
index ac519bc77b..07e164c5f5 100644
--- a/Marlin/src/gcode/queue.h
+++ b/Marlin/src/gcode/queue.h
@@ -83,8 +83,17 @@ void flush_and_request_resend();
*/
void ok_to_send();
+#if ENABLED(ADVANCED_PAUSE_FEATURE)
+ /**
+ * Insert in the front of queue, one or many commands to run from program memory.
+ * Aborts the current queue, if any.
+ * Note: drain_injected_commands_P() must be called repeatedly to drain the commands afterwards
+ */
+ void enqueue_and_echo_commands_front_P(PGM_P const pgcode);
+#endif
+
/**
- * Record one or many commands to run from program memory.
+ * Enqueue one or many commands to run from program memory.
* Aborts the current queue, if any.
* Note: drain_injected_commands_P() must be called repeatedly to drain the commands afterwards
*/
@@ -92,11 +101,13 @@ void enqueue_and_echo_commands_P(PGM_P const pgcode);
/**
* Enqueue with Serial Echo
+ * Return true on success
*/
bool enqueue_and_echo_command(const char* cmd);
#define HAS_LCD_QUEUE_NOW (ENABLED(MALYAN_LCD) || (HAS_LCD_MENU && ANY(AUTO_BED_LEVELING_UBL, PID_AUTOTUNE_MENU, ADVANCED_PAUSE_FEATURE)))
#define HAS_QUEUE_NOW (ENABLED(SDSUPPORT) || HAS_LCD_QUEUE_NOW)
+#define HAS_QUEUE_FRONT ENABLED(ADVANCED_PAUSE_FEATURE)
#if HAS_QUEUE_NOW
/**
diff --git a/Marlin/src/lcd/ultralcd.cpp b/Marlin/src/lcd/ultralcd.cpp
index 9cdcfbace6..855be5bce2 100644
--- a/Marlin/src/lcd/ultralcd.cpp
+++ b/Marlin/src/lcd/ultralcd.cpp
@@ -1398,7 +1398,7 @@ void MarlinUI::update() {
#if HAS_SPI_LCD
lcd_pause_show_message(PAUSE_MESSAGE_PAUSING, PAUSE_MODE_PAUSE_PRINT); // Show message immediately to let user know about pause in progress
#endif
- enqueue_and_echo_commands_P(PSTR("M25 P\nM24"));
+ enqueue_and_echo_commands_front_P(PSTR("M25 P\nM24"));
#elif ENABLED(SDSUPPORT)
enqueue_and_echo_commands_P(PSTR("M25"));
#elif defined(ACTION_ON_PAUSE)
--
GitLab