MK3 Add Gcode to wait for minimum PINDA temp

The PINDA temperature compensation is defined for values above 35C. To achieve an optimal first layer consistently it is vital to start the print with a temperature of >= 35C on the pinda probe.
When doing a manual pinda temperature calibration it is necessary to begin homing and mesh bed leveling at an exact temperature. This gcode is perfect for this.

Example startup code:

G28 W ; home all without mesh bed level
G0 Z50 ; raise Z to not heat PINDA before bed is warm
M104 S215 ; set extruder temp
M140 S60 ; set bed temp
M190 S60 ; wait for bed temp
M109 S215 ; wait for extruder temp
G0 X50 Y50 Z0.15 ; this is a good PINDA heating position
M666 S35 ; the new code - wait until PINDA is >= 35C
G28 W ; home all without mesh bed level
G80 ; mesh bed leveling

See my forum post later for more explaination on my manual temperature calibration procedure. I will link it then.

Firmware/Marlin_main.cpp

@@ -226,6 +226,7 @@
 // M540 - Use S[0|1] to enable or disable the stop SD card print on endstop hit (requires ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
 // M600 - Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
 // M605 - Set dual x-carriage movement mode: S<mode> [ X<duplication x-offset> R<duplication temp offset> ]
+// M666 - Wait for PINDA thermistor to reach target temperature.
 // M900 - Set LIN_ADVANCE options, if enabled. See Configuration_adv.h for details.
 // M907 - Set digital trimpot motor current using axis codes.
 // M908 - Control digital trimpot directly.
@@ -4806,6 +4807,48 @@ Sigma_Exit:
     #endif
         break;
 
+#ifdef PINDA_THERMISTOR
+case 666: // M666 - Wait for PINDA thermistor to reach target temperature.
+  {
+    int setTargetPinda = 0;
+  
+    if (code_seen('S')) {
+      setTargetPinda =  code_value();
+    } else {
+      break;
+    }
+
+    LCD_MESSAGERPGM(MSG_PLEASE_WAIT);
+
+    SERIAL_PROTOCOLPGM("Wait for PINDA target temperature:");
+    SERIAL_PROTOCOL(setTargetPinda);
+    SERIAL_PROTOCOLLN("");
+
+    codenum = millis();
+    cancel_heatup = false;
+
+    KEEPALIVE_STATE(NOT_BUSY);
+
+    while ( (!cancel_heatup) && current_temperature_pinda < setTargetPinda) {
+      if(( millis() - codenum) > 1000 ) //Print Temp Reading every 1 second while waiting.
+      {
+        SERIAL_PROTOCOLPGM("P:");
+        SERIAL_PROTOCOL_F(current_temperature_pinda,1);
+        SERIAL_PROTOCOLPGM("/");
+        SERIAL_PROTOCOL(setTargetPinda);
+        SERIAL_PROTOCOLLN("");
+        codenum = millis();
+      }
+      manage_heater();
+      manage_inactivity();
+      lcd_update();
+    }
+    LCD_MESSAGERPGM(MSG_OK);
+  
+    break;
+  }
+#endif //PINDA_THERMISTOR
+
     #if defined(FAN_PIN) && FAN_PIN > -1
       case 106: //M106 Fan On
         if (code_seen('S')){