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)