add flush queue for emergency stop

dda_queue.c

@@ -106,3 +106,18 @@ void print_queue() {
 		serial_writechar('E');
 	serial_writechar('\n');
 }
+
+void queue_flush() {
+	// save interrupt flag
+	uint8_t sreg = SREG;
+
+	// disable interrupts
+	cli();
+
+	// flush queue
+	mb_tail = mb_head;
+	movebuffer[mb_head].live = 0;
+
+	// restore interrupt flag
+	SREG = sreg;
+}

dda_queue.h

@@ -33,4 +33,7 @@ void next_move(void) __attribute__ ((hot));
 // print queue status
 void print_queue(void);
 
+// flush the queue for eg; emergency stop
+void queue_flush(void);
+
 #endif	/* _DDA_QUEUE */

gcode_process.c

@@ -249,6 +249,7 @@ void process_gcode_command() {
 				// M112- immediate stop
 			case 112:
 				disableTimerInterrupt();
+				queue_flush();
 				power_off();
 				break;
 				// M113- extruder PWM