diff --git a/config.h.dist b/config.h.dist
index e7c6a6d..b671b31 100644
--- a/config.h.dist
+++ b/config.h.dist
@@ -273,7 +273,10 @@ struct {
 
 // number of heaters- for GEN3, set to zero as extruder manages the heater by itself
 #define	NUM_HEATERS				0
-#define BED_HEATER			3  //Set to 0 if no bed heater
+
+// index of bed heater for M105 and M140 commands. set to 255 if no bed heater
+#define BED_HEATER				1
+
 // check if heater responds to changes in target temperature, disable and spit errors if not
 // #define	HEATER_SANITY_CHECK
 
diff --git a/gcode_process.c b/gcode_process.c
index a435b54..3e04e96 100644
--- a/gcode_process.c
+++ b/gcode_process.c
@@ -263,12 +263,8 @@ void process_gcode_command() {
 				// M104- set temperature
 			case 104:
 				temp_set(next_target.P, next_target.S);
-				if (next_target.S) {
+				if (next_target.S)
 					power_on();
-				}
-				else {
-					disable_heater();
-				}
 				break;
 				
 				// M105- get temperature
@@ -363,12 +359,8 @@ void process_gcode_command() {
 
 			case 140: //Set heated bed temperature
 				temp_set(BED_HEATER, next_target.S);
-				if (next_target.S) {
+				if (next_target.S)
 					power_on();
-				}
-				else {
-					disable_heater();
-				}
 				break;
 			#endif	/* NUM_HEATERS > 0 */
 				
diff --git a/temp.c b/temp.c
index ae30dac..b9d36ca 100644
--- a/temp.c
+++ b/temp.c
@@ -285,11 +285,11 @@ void temp_print(uint8_t index) {
 	
 	c = (temp_sensors_runtime[index].last_read_temp & 3) * 25;
 
-#if BED_HEATER > 0
+#if BED_HEATER < NUM_HEATERS
 	uint8_t b = 0;
-	b = (temp_sensors_runtime[BED_HEATER - 1].last_read_temp & 3) * 25;
+	b = (temp_sensors_runtime[BED_HEATER].last_read_temp & 3) * 25;
 	
-		sersendf_P(PSTR("T:%u.%u  B:%u.%u\n"), temp_sensors_runtime[index].last_read_temp >> 2, c, temp_sensors_runtime[BED_HEATER - 1].last_read_temp >> 2 , b);
+	sersendf_P(PSTR("T:%u.%u  B:%u.%u\n"), temp_sensors_runtime[index].last_read_temp >> 2, c, temp_sensors_runtime[BED_HEATER].last_read_temp >> 2 , b);
 #else
 		sersendf_P(PSTR("T:%u.%u"), temp_sensors_runtime[index].last_read_temp >> 2, c);
 #endif