From ecc0d6e3ef9ef354b9803e4d05533d8067d34caa Mon Sep 17 00:00:00 2001
From: Michael Moon <triffid.hunter@gmail.com>
Date: Sun, 19 Dec 2010 07:09:19 +1100
Subject: [PATCH] fix waitfor temperature, for M109 and friends

---
 dda.c       | 42 +++++++++++++++++++++---------------------
 dda_queue.c | 12 +++++++++++-
 2 files changed, 32 insertions(+), 22 deletions(-)

diff --git a/dda.c b/dda.c
index 3e97eee..0bdedaf 100644
--- a/dda.c
+++ b/dda.c
@@ -330,32 +330,32 @@ void dda_start(DDA *dda) {
 		// keep dda->live = 0
 	}
 	else {
-		if (dda->waitfor_temp) {
+/*		if (dda->waitfor_temp) {
 			#ifndef	REPRAP_HOST_COMPATIBILITY
 				serial_writestr_P(PSTR("Waiting for target temp\n"));
 			#endif
 		}
-		else {
-			// ensure steppers are ready to go
-			steptimeout = 0;
-			power_on();
-			x_enable();
-			y_enable();
-			if (dda->z_delta)
-				z_enable();
+		else {*/
+		// ensure steppers are ready to go
+		steptimeout = 0;
+		power_on();
+		x_enable();
+		y_enable();
+		if (dda->z_delta)
+			z_enable();
 
-			// set direction outputs
-			x_direction(dda->x_direction);
-			y_direction(dda->y_direction);
-			z_direction(dda->z_direction);
-			e_direction(dda->e_direction);
-			
-			#ifdef	DC_EXTRUDER
-			if (dda->e_delta)
-				heater_set(DC_EXTRUDER, DC_EXTRUDER_PWM);
-			#endif
-			
-		}
+		// set direction outputs
+		x_direction(dda->x_direction);
+		y_direction(dda->y_direction);
+		z_direction(dda->z_direction);
+		e_direction(dda->e_direction);
+
+		#ifdef	DC_EXTRUDER
+		if (dda->e_delta)
+			heater_set(DC_EXTRUDER, DC_EXTRUDER_PWM);
+		#endif
+
+// 		}
 
 		// ensure this dda starts
 		dda->live = 1;
diff --git a/dda_queue.c b/dda_queue.c
index ad33992..3b4ab66 100644
--- a/dda_queue.c
+++ b/dda_queue.c
@@ -32,6 +32,7 @@ void queue_step() {
 	// do our next step
 	if (movebuffer[mb_tail].live) {
 		if (movebuffer[mb_tail].waitfor_temp) {
+			setTimer(movebuffer[mb_tail].c >> 8);
 			if (temp_achieved()) {
 				movebuffer[mb_tail].live = movebuffer[mb_tail].waitfor_temp = 0;
 				serial_writestr_P(PSTR("Temp achieved\n"));
@@ -90,7 +91,16 @@ void next_move() {
 		// next item
 		uint8_t t = mb_tail + 1;
 		t &= (MOVEBUFFER_SIZE - 1);
-		dda_start(&movebuffer[t]);
+		if (movebuffer[t].waitfor_temp) {
+			#ifndef	REPRAP_HOST_COMPATIBILITY
+				serial_writestr_P(PSTR("Waiting for target temp\n"));
+			#endif
+			movebuffer[t].live = 1;
+			setTimer(movebuffer[t].c >> 8);
+		}
+		else {
+			dda_start(&movebuffer[t]);
+		}
 		mb_tail = t;
 	}
 	else