Browse Source

LF line endings

PavelSindler 7 years ago
parent
commit
d74c029327
15 changed files with 8013 additions and 8013 deletions
  1. 347 347
      Firmware/Marlin.h
  2. 813 813
      Firmware/Sd2Card.cpp
  3. 261 261
      Firmware/Sd2Card.h
  4. 367 367
      Firmware/Sd2PinMap.h
  5. 50 50
      Firmware/SdFatUtil.h
  6. 95 95
      Firmware/SdFile.cpp
  7. 285 285
      Firmware/SdInfo.h
  8. 404 404
      Firmware/SdVolume.cpp
  9. 283 283
      Firmware/language_it.h
  10. 2406 2406
      Firmware/mesh_bed_calibration.cpp
  11. 182 182
      Firmware/mesh_bed_calibration.h
  12. 1986 1986
      Firmware/temperature.cpp
  13. 211 211
      Firmware/temperature.h
  14. 288 288
      Firmware/util.cpp
  15. 35 35
      Firmware/util.h

+ 347 - 347
Firmware/Marlin.h

@@ -1,348 +1,348 @@
-// Tonokip RepRap firmware rewrite based off of Hydra-mmm firmware.
-// License: GPL
-
-#ifndef MARLIN_H
-#define MARLIN_H
-
-#define  FORCE_INLINE __attribute__((always_inline)) inline
-
-#include <math.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <inttypes.h>
-
-#include <util/delay.h>
-#include <avr/pgmspace.h>
-#include <avr/eeprom.h>
-#include <avr/interrupt.h>
-
-
-#include "fastio.h"
-#include "Configuration.h"
-#include "pins.h"
-
-#ifndef AT90USB
-#define  HardwareSerial_h // trick to disable the standard HWserial
-#endif
-
-#if (ARDUINO >= 100)
-# include "Arduino.h"
-#else
-# include "WProgram.h"
-#endif
-
-// Arduino < 1.0.0 does not define this, so we need to do it ourselves
-#ifndef analogInputToDigitalPin
-# define analogInputToDigitalPin(p) ((p) + A0)
-#endif
-
-#ifdef AT90USB
-#include "HardwareSerial.h"
-#endif
-
-#include "MarlinSerial.h"
-
-#ifndef cbi
-#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
-#endif
-#ifndef sbi
-#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
-#endif
-
-#include "WString.h"
-
-#ifdef AT90USB
-   #ifdef BTENABLED
-         #define MYSERIAL bt
-   #else
-         #define MYSERIAL Serial
-   #endif // BTENABLED
-#else
-  #define MYSERIAL MSerial
-#endif
-
-#define SERIAL_PROTOCOL(x) (MYSERIAL.print(x))
-#define SERIAL_PROTOCOL_F(x,y) (MYSERIAL.print(x,y))
-#define SERIAL_PROTOCOLPGM(x) (serialprintPGM(PSTR(x)))
-#define SERIAL_PROTOCOLRPGM(x) (serialprintPGM((x)))
-#define SERIAL_PROTOCOLLN(x) (MYSERIAL.print(x),MYSERIAL.write('\n'))
-#define SERIAL_PROTOCOLLNPGM(x) (serialprintPGM(PSTR(x)),MYSERIAL.write('\n'))
-#define SERIAL_PROTOCOLLNRPGM(x) (serialprintPGM((x)),MYSERIAL.write('\n'))
-
-
-extern const char errormagic[] PROGMEM;
-extern const char echomagic[] PROGMEM;
-
-#define SERIAL_ERROR_START (serialprintPGM(errormagic))
-#define SERIAL_ERROR(x) SERIAL_PROTOCOL(x)
-#define SERIAL_ERRORPGM(x) SERIAL_PROTOCOLPGM(x)
-#define SERIAL_ERRORRPGM(x) SERIAL_PROTOCOLRPGM(x)
-#define SERIAL_ERRORLN(x) SERIAL_PROTOCOLLN(x)
-#define SERIAL_ERRORLNPGM(x) SERIAL_PROTOCOLLNPGM(x)
-#define SERIAL_ERRORLNRPGM(x) SERIAL_PROTOCOLLNRPGM(x)
-
-#define SERIAL_ECHO_START (serialprintPGM(echomagic))
-#define SERIAL_ECHO(x) SERIAL_PROTOCOL(x)
-#define SERIAL_ECHOPGM(x) SERIAL_PROTOCOLPGM(x)
-#define SERIAL_ECHORPGM(x) SERIAL_PROTOCOLRPGM(x)
-#define SERIAL_ECHOLN(x) SERIAL_PROTOCOLLN(x)
-#define SERIAL_ECHOLNPGM(x) SERIAL_PROTOCOLLNPGM(x)
-#define SERIAL_ECHOLNRPGM(x) SERIAL_PROTOCOLLNRPGM(x)
-
-#define SERIAL_ECHOPAIR(name,value) (serial_echopair_P(PSTR(name),(value)))
-
-void serial_echopair_P(const char *s_P, float v);
-void serial_echopair_P(const char *s_P, double v);
-void serial_echopair_P(const char *s_P, unsigned long v);
-
-
-//Things to write to serial from Program memory. Saves 400 to 2k of RAM.
-FORCE_INLINE void serialprintPGM(const char *str)
-{
-  char ch=pgm_read_byte(str);
-  while(ch)
-  {
-    MYSERIAL.write(ch);
-    ch=pgm_read_byte(++str);
-  }
-}
-
-bool is_buffer_empty();
-void get_command();
-void process_commands();
-void ramming();
-
-void manage_inactivity(bool ignore_stepper_queue=false);
-
-#if defined(X_ENABLE_PIN) && X_ENABLE_PIN > -1
-  #define  enable_x() WRITE(X_ENABLE_PIN, X_ENABLE_ON)
-  #define disable_x() { WRITE(X_ENABLE_PIN,!X_ENABLE_ON); axis_known_position[X_AXIS] = false; }
-#else
-  #define enable_x() ;
-  #define disable_x() ;
-#endif
-
-#if defined(Y_ENABLE_PIN) && Y_ENABLE_PIN > -1
-  #ifdef Y_DUAL_STEPPER_DRIVERS
-    #define  enable_y() { WRITE(Y_ENABLE_PIN, Y_ENABLE_ON); WRITE(Y2_ENABLE_PIN,  Y_ENABLE_ON); }
-    #define disable_y() { WRITE(Y_ENABLE_PIN,!Y_ENABLE_ON); WRITE(Y2_ENABLE_PIN, !Y_ENABLE_ON); axis_known_position[Y_AXIS] = false; }
-  #else
-    #define  enable_y() WRITE(Y_ENABLE_PIN, Y_ENABLE_ON)
-    #define disable_y() { WRITE(Y_ENABLE_PIN,!Y_ENABLE_ON); axis_known_position[Y_AXIS] = false; }
-  #endif
-#else
-  #define enable_y() ;
-  #define disable_y() ;
-#endif
-
-#if defined(Z_ENABLE_PIN) && Z_ENABLE_PIN > -1 
-	#if defined(Z_AXIS_ALWAYS_ON)
-		  #ifdef Z_DUAL_STEPPER_DRIVERS
-			#define  enable_z() { WRITE(Z_ENABLE_PIN, Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN, Z_ENABLE_ON); }
-			#define disable_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
-		  #else
-			#define  enable_z() WRITE(Z_ENABLE_PIN, Z_ENABLE_ON)
-			#define  disable_z() ;
-		  #endif
-	#else
-		#ifdef Z_DUAL_STEPPER_DRIVERS
-			#define  enable_z() { WRITE(Z_ENABLE_PIN, Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN, Z_ENABLE_ON); }
-			#define disable_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
-		#else
-			#define  enable_z() WRITE(Z_ENABLE_PIN, Z_ENABLE_ON)
-			#define disable_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
-		#endif
-	#endif
-#else
-  #define enable_z() ;
-  #define disable_z() ;
-#endif
-
-
-
-
-//#if defined(Z_ENABLE_PIN) && Z_ENABLE_PIN > -1
-//#ifdef Z_DUAL_STEPPER_DRIVERS
-//#define  enable_z() { WRITE(Z_ENABLE_PIN, Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN, Z_ENABLE_ON); }
-//#define disable_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
-//#else
-//#define  enable_z() WRITE(Z_ENABLE_PIN, Z_ENABLE_ON)
-//#define disable_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
-//#endif
-//#else
-//#define enable_z() ;
-//#define disable_z() ;
-//#endif
-
-
-#if defined(E0_ENABLE_PIN) && (E0_ENABLE_PIN > -1)
-  #define enable_e0() WRITE(E0_ENABLE_PIN, E_ENABLE_ON)
-  #define disable_e0() WRITE(E0_ENABLE_PIN,!E_ENABLE_ON)
-#else
-  #define enable_e0()  /* nothing */
-  #define disable_e0() /* nothing */
-#endif
-
-#if (EXTRUDERS > 1) && defined(E1_ENABLE_PIN) && (E1_ENABLE_PIN > -1)
-  #define enable_e1() WRITE(E1_ENABLE_PIN, E_ENABLE_ON)
-  #define disable_e1() WRITE(E1_ENABLE_PIN,!E_ENABLE_ON)
-#else
-  #define enable_e1()  /* nothing */
-  #define disable_e1() /* nothing */
-#endif
-
-#if (EXTRUDERS > 2) && defined(E2_ENABLE_PIN) && (E2_ENABLE_PIN > -1)
-  #define enable_e2() WRITE(E2_ENABLE_PIN, E_ENABLE_ON)
-  #define disable_e2() WRITE(E2_ENABLE_PIN,!E_ENABLE_ON)
-#else
-  #define enable_e2()  /* nothing */
-  #define disable_e2() /* nothing */
-#endif
-
-
-enum AxisEnum {X_AXIS=0, Y_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5};
-
-
-void FlushSerialRequestResend();
-void ClearToSend();
-
-void get_coordinates();
-void prepare_move();
-void kill(const char *full_screen_message = NULL);
-void Stop();
-
-bool IsStopped();
-
-//put an ASCII command at the end of the current buffer.
-void enquecommand(const char *cmd, bool from_progmem = false);
-//put an ASCII command at the end of the current buffer, read from flash
-#define enquecommand_P(cmd) enquecommand(cmd, true)
-void enquecommand_front(const char *cmd, bool from_progmem = false);
-//put an ASCII command at the end of the current buffer, read from flash
-#define enquecommand_P(cmd) enquecommand(cmd, true)
-#define enquecommand_front_P(cmd) enquecommand_front(cmd, true)
-void repeatcommand_front();
-// Remove all lines from the command queue.
-void cmdqueue_reset();
-
-void prepare_arc_move(char isclockwise);
-void clamp_to_software_endstops(float target[3]);
-
-void refresh_cmd_timeout(void);
-
-#ifdef FAST_PWM_FAN
-void setPwmFrequency(uint8_t pin, int val);
-#endif
-
-#ifndef CRITICAL_SECTION_START
-  #define CRITICAL_SECTION_START  unsigned char _sreg = SREG; cli();
-  #define CRITICAL_SECTION_END    SREG = _sreg;
-#endif //CRITICAL_SECTION_START
-
-extern float homing_feedrate[];
-extern bool axis_relative_modes[];
-extern int feedmultiply;
-extern int extrudemultiply; // Sets extrude multiply factor (in percent) for all extruders
-extern bool volumetric_enabled;
-extern int extruder_multiply[EXTRUDERS]; // sets extrude multiply factor (in percent) for each extruder individually
-extern float filament_size[EXTRUDERS]; // cross-sectional area of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder.
-extern float volumetric_multiplier[EXTRUDERS]; // reciprocal of cross-sectional area of filament (in square millimeters), stored this way to reduce computational burden in planner
-extern float current_position[NUM_AXIS] ;
-extern float destination[NUM_AXIS] ;
-extern float add_homing[3];
-extern float min_pos[3];
-extern float max_pos[3];
-extern bool axis_known_position[3];
-extern float zprobe_zoffset;
-extern int fanSpeed;
-extern void homeaxis(int axis);
-
-
-#ifdef FAN_SOFT_PWM
-extern unsigned char fanSpeedSoftPwm;
-#endif
-
-#ifdef FILAMENT_SENSOR
-  extern float filament_width_nominal;  //holds the theoretical filament diameter ie., 3.00 or 1.75
-  extern bool filament_sensor;  //indicates that filament sensor readings should control extrusion
-  extern float filament_width_meas; //holds the filament diameter as accurately measured
-  extern signed char measurement_delay[];  //ring buffer to delay measurement
-  extern int delay_index1, delay_index2;  //index into ring buffer
-  extern float delay_dist; //delay distance counter
-  extern int meas_delay_cm; //delay distance
-#endif
-
-#ifdef FWRETRACT
-extern bool autoretract_enabled;
-extern bool retracted[EXTRUDERS];
-extern float retract_length, retract_length_swap, retract_feedrate, retract_zlift;
-extern float retract_recover_length, retract_recover_length_swap, retract_recover_feedrate;
-#endif
-
-extern unsigned long starttime;
-extern unsigned long stoptime;
-extern int bowden_length[4];
-extern bool is_usb_printing;
-extern bool homing_flag;
-extern bool temp_cal_active;
-extern bool loading_flag;
-extern unsigned int usb_printing_counter;
-
-extern unsigned long kicktime;
-
-extern unsigned long total_filament_used;
-void save_statistics(unsigned long _total_filament_used, unsigned long _total_print_time);
-extern unsigned int heating_status;
-extern unsigned int status_number;
-extern unsigned int heating_status_counter;
-extern bool custom_message;
-extern unsigned int custom_message_type;
-extern unsigned int custom_message_state;
-extern char snmm_filaments_used;
-extern unsigned long PingTime;
-
-
-// Handling multiple extruders pins
-extern uint8_t active_extruder;
-
-#ifdef DIGIPOT_I2C
-extern void digipot_i2c_set_current( int channel, float current );
-extern void digipot_i2c_init();
-#endif
-
-#endif
-
-//Long pause
-extern int saved_feedmultiply;
-extern float HotendTempBckp;
-extern int fanSpeedBckp;
-extern float pause_lastpos[4];
-extern unsigned long pause_time;
-extern unsigned long start_pause_print;
-
-extern bool mesh_bed_leveling_flag;
-extern bool mesh_bed_run_from_menu;
-
-extern void calculate_volumetric_multipliers();
-
-// Similar to the default Arduino delay function, 
-// but it keeps the background tasks running.
-extern void delay_keep_alive(unsigned int ms);
-
-extern void check_babystep();
-
-extern void long_pause();
-
-#ifdef DIS
-
-void d_setup();
-float d_ReadData();
-void bed_analysis(float x_dimension, float y_dimension, int x_points_num, int y_points_num, float shift_x, float shift_y);
-
-#endif
-float temp_comp_interpolation(float temperature);
-void temp_compensation_apply();
-void temp_compensation_start();
-void wait_for_heater(long codenum);
+// Tonokip RepRap firmware rewrite based off of Hydra-mmm firmware.
+// License: GPL
+
+#ifndef MARLIN_H
+#define MARLIN_H
+
+#define  FORCE_INLINE __attribute__((always_inline)) inline
+
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <inttypes.h>
+
+#include <util/delay.h>
+#include <avr/pgmspace.h>
+#include <avr/eeprom.h>
+#include <avr/interrupt.h>
+
+
+#include "fastio.h"
+#include "Configuration.h"
+#include "pins.h"
+
+#ifndef AT90USB
+#define  HardwareSerial_h // trick to disable the standard HWserial
+#endif
+
+#if (ARDUINO >= 100)
+# include "Arduino.h"
+#else
+# include "WProgram.h"
+#endif
+
+// Arduino < 1.0.0 does not define this, so we need to do it ourselves
+#ifndef analogInputToDigitalPin
+# define analogInputToDigitalPin(p) ((p) + A0)
+#endif
+
+#ifdef AT90USB
+#include "HardwareSerial.h"
+#endif
+
+#include "MarlinSerial.h"
+
+#ifndef cbi
+#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
+#endif
+#ifndef sbi
+#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
+#endif
+
+#include "WString.h"
+
+#ifdef AT90USB
+   #ifdef BTENABLED
+         #define MYSERIAL bt
+   #else
+         #define MYSERIAL Serial
+   #endif // BTENABLED
+#else
+  #define MYSERIAL MSerial
+#endif
+
+#define SERIAL_PROTOCOL(x) (MYSERIAL.print(x))
+#define SERIAL_PROTOCOL_F(x,y) (MYSERIAL.print(x,y))
+#define SERIAL_PROTOCOLPGM(x) (serialprintPGM(PSTR(x)))
+#define SERIAL_PROTOCOLRPGM(x) (serialprintPGM((x)))
+#define SERIAL_PROTOCOLLN(x) (MYSERIAL.print(x),MYSERIAL.write('\n'))
+#define SERIAL_PROTOCOLLNPGM(x) (serialprintPGM(PSTR(x)),MYSERIAL.write('\n'))
+#define SERIAL_PROTOCOLLNRPGM(x) (serialprintPGM((x)),MYSERIAL.write('\n'))
+
+
+extern const char errormagic[] PROGMEM;
+extern const char echomagic[] PROGMEM;
+
+#define SERIAL_ERROR_START (serialprintPGM(errormagic))
+#define SERIAL_ERROR(x) SERIAL_PROTOCOL(x)
+#define SERIAL_ERRORPGM(x) SERIAL_PROTOCOLPGM(x)
+#define SERIAL_ERRORRPGM(x) SERIAL_PROTOCOLRPGM(x)
+#define SERIAL_ERRORLN(x) SERIAL_PROTOCOLLN(x)
+#define SERIAL_ERRORLNPGM(x) SERIAL_PROTOCOLLNPGM(x)
+#define SERIAL_ERRORLNRPGM(x) SERIAL_PROTOCOLLNRPGM(x)
+
+#define SERIAL_ECHO_START (serialprintPGM(echomagic))
+#define SERIAL_ECHO(x) SERIAL_PROTOCOL(x)
+#define SERIAL_ECHOPGM(x) SERIAL_PROTOCOLPGM(x)
+#define SERIAL_ECHORPGM(x) SERIAL_PROTOCOLRPGM(x)
+#define SERIAL_ECHOLN(x) SERIAL_PROTOCOLLN(x)
+#define SERIAL_ECHOLNPGM(x) SERIAL_PROTOCOLLNPGM(x)
+#define SERIAL_ECHOLNRPGM(x) SERIAL_PROTOCOLLNRPGM(x)
+
+#define SERIAL_ECHOPAIR(name,value) (serial_echopair_P(PSTR(name),(value)))
+
+void serial_echopair_P(const char *s_P, float v);
+void serial_echopair_P(const char *s_P, double v);
+void serial_echopair_P(const char *s_P, unsigned long v);
+
+
+//Things to write to serial from Program memory. Saves 400 to 2k of RAM.
+FORCE_INLINE void serialprintPGM(const char *str)
+{
+  char ch=pgm_read_byte(str);
+  while(ch)
+  {
+    MYSERIAL.write(ch);
+    ch=pgm_read_byte(++str);
+  }
+}
+
+bool is_buffer_empty();
+void get_command();
+void process_commands();
+void ramming();
+
+void manage_inactivity(bool ignore_stepper_queue=false);
+
+#if defined(X_ENABLE_PIN) && X_ENABLE_PIN > -1
+  #define  enable_x() WRITE(X_ENABLE_PIN, X_ENABLE_ON)
+  #define disable_x() { WRITE(X_ENABLE_PIN,!X_ENABLE_ON); axis_known_position[X_AXIS] = false; }
+#else
+  #define enable_x() ;
+  #define disable_x() ;
+#endif
+
+#if defined(Y_ENABLE_PIN) && Y_ENABLE_PIN > -1
+  #ifdef Y_DUAL_STEPPER_DRIVERS
+    #define  enable_y() { WRITE(Y_ENABLE_PIN, Y_ENABLE_ON); WRITE(Y2_ENABLE_PIN,  Y_ENABLE_ON); }
+    #define disable_y() { WRITE(Y_ENABLE_PIN,!Y_ENABLE_ON); WRITE(Y2_ENABLE_PIN, !Y_ENABLE_ON); axis_known_position[Y_AXIS] = false; }
+  #else
+    #define  enable_y() WRITE(Y_ENABLE_PIN, Y_ENABLE_ON)
+    #define disable_y() { WRITE(Y_ENABLE_PIN,!Y_ENABLE_ON); axis_known_position[Y_AXIS] = false; }
+  #endif
+#else
+  #define enable_y() ;
+  #define disable_y() ;
+#endif
+
+#if defined(Z_ENABLE_PIN) && Z_ENABLE_PIN > -1 
+	#if defined(Z_AXIS_ALWAYS_ON)
+		  #ifdef Z_DUAL_STEPPER_DRIVERS
+			#define  enable_z() { WRITE(Z_ENABLE_PIN, Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN, Z_ENABLE_ON); }
+			#define disable_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
+		  #else
+			#define  enable_z() WRITE(Z_ENABLE_PIN, Z_ENABLE_ON)
+			#define  disable_z() ;
+		  #endif
+	#else
+		#ifdef Z_DUAL_STEPPER_DRIVERS
+			#define  enable_z() { WRITE(Z_ENABLE_PIN, Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN, Z_ENABLE_ON); }
+			#define disable_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
+		#else
+			#define  enable_z() WRITE(Z_ENABLE_PIN, Z_ENABLE_ON)
+			#define disable_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
+		#endif
+	#endif
+#else
+  #define enable_z() ;
+  #define disable_z() ;
+#endif
+
+
+
+
+//#if defined(Z_ENABLE_PIN) && Z_ENABLE_PIN > -1
+//#ifdef Z_DUAL_STEPPER_DRIVERS
+//#define  enable_z() { WRITE(Z_ENABLE_PIN, Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN, Z_ENABLE_ON); }
+//#define disable_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
+//#else
+//#define  enable_z() WRITE(Z_ENABLE_PIN, Z_ENABLE_ON)
+//#define disable_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
+//#endif
+//#else
+//#define enable_z() ;
+//#define disable_z() ;
+//#endif
+
+
+#if defined(E0_ENABLE_PIN) && (E0_ENABLE_PIN > -1)
+  #define enable_e0() WRITE(E0_ENABLE_PIN, E_ENABLE_ON)
+  #define disable_e0() WRITE(E0_ENABLE_PIN,!E_ENABLE_ON)
+#else
+  #define enable_e0()  /* nothing */
+  #define disable_e0() /* nothing */
+#endif
+
+#if (EXTRUDERS > 1) && defined(E1_ENABLE_PIN) && (E1_ENABLE_PIN > -1)
+  #define enable_e1() WRITE(E1_ENABLE_PIN, E_ENABLE_ON)
+  #define disable_e1() WRITE(E1_ENABLE_PIN,!E_ENABLE_ON)
+#else
+  #define enable_e1()  /* nothing */
+  #define disable_e1() /* nothing */
+#endif
+
+#if (EXTRUDERS > 2) && defined(E2_ENABLE_PIN) && (E2_ENABLE_PIN > -1)
+  #define enable_e2() WRITE(E2_ENABLE_PIN, E_ENABLE_ON)
+  #define disable_e2() WRITE(E2_ENABLE_PIN,!E_ENABLE_ON)
+#else
+  #define enable_e2()  /* nothing */
+  #define disable_e2() /* nothing */
+#endif
+
+
+enum AxisEnum {X_AXIS=0, Y_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5};
+
+
+void FlushSerialRequestResend();
+void ClearToSend();
+
+void get_coordinates();
+void prepare_move();
+void kill(const char *full_screen_message = NULL);
+void Stop();
+
+bool IsStopped();
+
+//put an ASCII command at the end of the current buffer.
+void enquecommand(const char *cmd, bool from_progmem = false);
+//put an ASCII command at the end of the current buffer, read from flash
+#define enquecommand_P(cmd) enquecommand(cmd, true)
+void enquecommand_front(const char *cmd, bool from_progmem = false);
+//put an ASCII command at the end of the current buffer, read from flash
+#define enquecommand_P(cmd) enquecommand(cmd, true)
+#define enquecommand_front_P(cmd) enquecommand_front(cmd, true)
+void repeatcommand_front();
+// Remove all lines from the command queue.
+void cmdqueue_reset();
+
+void prepare_arc_move(char isclockwise);
+void clamp_to_software_endstops(float target[3]);
+
+void refresh_cmd_timeout(void);
+
+#ifdef FAST_PWM_FAN
+void setPwmFrequency(uint8_t pin, int val);
+#endif
+
+#ifndef CRITICAL_SECTION_START
+  #define CRITICAL_SECTION_START  unsigned char _sreg = SREG; cli();
+  #define CRITICAL_SECTION_END    SREG = _sreg;
+#endif //CRITICAL_SECTION_START
+
+extern float homing_feedrate[];
+extern bool axis_relative_modes[];
+extern int feedmultiply;
+extern int extrudemultiply; // Sets extrude multiply factor (in percent) for all extruders
+extern bool volumetric_enabled;
+extern int extruder_multiply[EXTRUDERS]; // sets extrude multiply factor (in percent) for each extruder individually
+extern float filament_size[EXTRUDERS]; // cross-sectional area of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder.
+extern float volumetric_multiplier[EXTRUDERS]; // reciprocal of cross-sectional area of filament (in square millimeters), stored this way to reduce computational burden in planner
+extern float current_position[NUM_AXIS] ;
+extern float destination[NUM_AXIS] ;
+extern float add_homing[3];
+extern float min_pos[3];
+extern float max_pos[3];
+extern bool axis_known_position[3];
+extern float zprobe_zoffset;
+extern int fanSpeed;
+extern void homeaxis(int axis);
+
+
+#ifdef FAN_SOFT_PWM
+extern unsigned char fanSpeedSoftPwm;
+#endif
+
+#ifdef FILAMENT_SENSOR
+  extern float filament_width_nominal;  //holds the theoretical filament diameter ie., 3.00 or 1.75
+  extern bool filament_sensor;  //indicates that filament sensor readings should control extrusion
+  extern float filament_width_meas; //holds the filament diameter as accurately measured
+  extern signed char measurement_delay[];  //ring buffer to delay measurement
+  extern int delay_index1, delay_index2;  //index into ring buffer
+  extern float delay_dist; //delay distance counter
+  extern int meas_delay_cm; //delay distance
+#endif
+
+#ifdef FWRETRACT
+extern bool autoretract_enabled;
+extern bool retracted[EXTRUDERS];
+extern float retract_length, retract_length_swap, retract_feedrate, retract_zlift;
+extern float retract_recover_length, retract_recover_length_swap, retract_recover_feedrate;
+#endif
+
+extern unsigned long starttime;
+extern unsigned long stoptime;
+extern int bowden_length[4];
+extern bool is_usb_printing;
+extern bool homing_flag;
+extern bool temp_cal_active;
+extern bool loading_flag;
+extern unsigned int usb_printing_counter;
+
+extern unsigned long kicktime;
+
+extern unsigned long total_filament_used;
+void save_statistics(unsigned long _total_filament_used, unsigned long _total_print_time);
+extern unsigned int heating_status;
+extern unsigned int status_number;
+extern unsigned int heating_status_counter;
+extern bool custom_message;
+extern unsigned int custom_message_type;
+extern unsigned int custom_message_state;
+extern char snmm_filaments_used;
+extern unsigned long PingTime;
+
+
+// Handling multiple extruders pins
+extern uint8_t active_extruder;
+
+#ifdef DIGIPOT_I2C
+extern void digipot_i2c_set_current( int channel, float current );
+extern void digipot_i2c_init();
+#endif
+
+#endif
+
+//Long pause
+extern int saved_feedmultiply;
+extern float HotendTempBckp;
+extern int fanSpeedBckp;
+extern float pause_lastpos[4];
+extern unsigned long pause_time;
+extern unsigned long start_pause_print;
+
+extern bool mesh_bed_leveling_flag;
+extern bool mesh_bed_run_from_menu;
+
+extern void calculate_volumetric_multipliers();
+
+// Similar to the default Arduino delay function, 
+// but it keeps the background tasks running.
+extern void delay_keep_alive(unsigned int ms);
+
+extern void check_babystep();
+
+extern void long_pause();
+
+#ifdef DIS
+
+void d_setup();
+float d_ReadData();
+void bed_analysis(float x_dimension, float y_dimension, int x_points_num, int y_points_num, float shift_x, float shift_y);
+
+#endif
+float temp_comp_interpolation(float temperature);
+void temp_compensation_apply();
+void temp_compensation_start();
+void wait_for_heater(long codenum);
 void serialecho_temperatures();

+ 813 - 813
Firmware/Sd2Card.cpp

@@ -1,813 +1,813 @@
-/* Arduino Sd2Card Library
- * Copyright (C) 2009 by William Greiman
- *
- * This file is part of the Arduino Sd2Card Library
- *
- * This Library is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This Library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with the Arduino Sd2Card Library.  If not, see
- * <http://www.gnu.org/licenses/>.
- */
-#include "Marlin.h"
-
-#ifdef SDSUPPORT
-#include "Sd2Card.h"
-//------------------------------------------------------------------------------
-#ifndef SOFTWARE_SPI
-// functions for hardware SPI
-//------------------------------------------------------------------------------
-// make sure SPCR rate is in expected bits
-#if (SPR0 != 0 || SPR1 != 1)
-#error unexpected SPCR bits
-#endif
-/**
- * Initialize hardware SPI
- * Set SCK rate to F_CPU/pow(2, 1 + spiRate) for spiRate [0,6]
- */
-static void spiInit(uint8_t spiRate) {
-  // See avr processor documentation
-  SPCR = (1 << SPE) | (1 << MSTR) | (spiRate >> 1);
-  SPSR = spiRate & 1 || spiRate == 6 ? 0 : 1 << SPI2X;
-}
-//------------------------------------------------------------------------------
-/** SPI receive a byte */
-static uint8_t spiRec() {
-  SPDR = 0XFF;
-  while (!(SPSR & (1 << SPIF))) { /* Intentionally left empty */ }
-  return SPDR;
-}
-//------------------------------------------------------------------------------
-/** SPI read data - only one call so force inline */
-static inline __attribute__((always_inline))
-void spiRead(uint8_t* buf, uint16_t nbyte) {
-  if (nbyte-- == 0) return;
-  SPDR = 0XFF;
-  for (uint16_t i = 0; i < nbyte; i++) {
-    while (!(SPSR & (1 << SPIF))) { /* Intentionally left empty */ }
-    buf[i] = SPDR;
-    SPDR = 0XFF;
-  }
-  while (!(SPSR & (1 << SPIF))) { /* Intentionally left empty */ }
-  buf[nbyte] = SPDR;
-}
-//------------------------------------------------------------------------------
-/** SPI send a byte */
-static void spiSend(uint8_t b) {
-  SPDR = b;
-  while (!(SPSR & (1 << SPIF))) { /* Intentionally left empty */ }
-}
-//------------------------------------------------------------------------------
-/** SPI send block - only one call so force inline */
-static inline __attribute__((always_inline))
-  void spiSendBlock(uint8_t token, const uint8_t* buf) {
-  SPDR = token;
-  for (uint16_t i = 0; i < 512; i += 2) {
-    while (!(SPSR & (1 << SPIF))) { /* Intentionally left empty */ }
-    SPDR = buf[i];
-    while (!(SPSR & (1 << SPIF))) { /* Intentionally left empty */ }
-    SPDR = buf[i + 1];
-  }
-  while (!(SPSR & (1 << SPIF))) { /* Intentionally left empty */ }
-}
-//------------------------------------------------------------------------------
-#else  // SOFTWARE_SPI
-//------------------------------------------------------------------------------
-/** nop to tune soft SPI timing */
-#define nop asm volatile ("nop\n\t")
-//------------------------------------------------------------------------------
-/** Soft SPI receive byte */
-static uint8_t spiRec() {
-  uint8_t data = 0;
-  // no interrupts during byte receive - about 8 us
-  cli();
-  // output pin high - like sending 0XFF
-  fastDigitalWrite(SPI_MOSI_PIN, HIGH);
-
-  for (uint8_t i = 0; i < 8; i++) {
-    fastDigitalWrite(SPI_SCK_PIN, HIGH);
-
-    // adjust so SCK is nice
-    nop;
-    nop;
-
-    data <<= 1;
-
-    if (fastDigitalRead(SPI_MISO_PIN)) data |= 1;
-
-    fastDigitalWrite(SPI_SCK_PIN, LOW);
-  }
-  // enable interrupts
-  sei();
-  return data;
-}
-//------------------------------------------------------------------------------
-/** Soft SPI read data */
-static void spiRead(uint8_t* buf, uint16_t nbyte) {
-  for (uint16_t i = 0; i < nbyte; i++) {
-    buf[i] = spiRec();
-  }
-}
-//------------------------------------------------------------------------------
-/** Soft SPI send byte */
-static void spiSend(uint8_t data) {
-  // no interrupts during byte send - about 8 us
-  cli();
-  for (uint8_t i = 0; i < 8; i++) {
-    fastDigitalWrite(SPI_SCK_PIN, LOW);
-
-    fastDigitalWrite(SPI_MOSI_PIN, data & 0X80);
-
-    data <<= 1;
-
-    fastDigitalWrite(SPI_SCK_PIN, HIGH);
-  }
-  // hold SCK high for a few ns
-  nop;
-  nop;
-  nop;
-  nop;
-
-  fastDigitalWrite(SPI_SCK_PIN, LOW);
-  // enable interrupts
-  sei();
-}
-//------------------------------------------------------------------------------
-/** Soft SPI send block */
-  void spiSendBlock(uint8_t token, const uint8_t* buf) {
-  spiSend(token);
-  for (uint16_t i = 0; i < 512; i++) {
-    spiSend(buf[i]);
-  }
-}
-#endif  // SOFTWARE_SPI
-//------------------------------------------------------------------------------
-// send command and return error code.  Return zero for OK
-uint8_t Sd2Card::cardCommand(uint8_t cmd, uint32_t arg) {
-  // select card
-  chipSelectLow();
-
-  // wait up to 300 ms if busy
-  waitNotBusy(300);
-
-  // send command
-  spiSend(cmd | 0x40);
-
-  // send argument
-  for (int8_t s = 24; s >= 0; s -= 8) spiSend(arg >> s);
-
-  // send CRC
-  uint8_t crc = 0XFF;
-  if (cmd == CMD0) crc = 0X95;  // correct crc for CMD0 with arg 0
-  if (cmd == CMD8) crc = 0X87;  // correct crc for CMD8 with arg 0X1AA
-  spiSend(crc);
-
-  // skip stuff byte for stop read
-  if (cmd == CMD12) spiRec();
-
-  // wait for response
-  for (uint8_t i = 0; ((status_ = spiRec()) & 0X80) && i != 0XFF; i++) { /* Intentionally left empty */ }
-  return status_;
-}
-//------------------------------------------------------------------------------
-/**
- * Determine the size of an SD flash memory card.
- *
- * \return The number of 512 byte data blocks in the card
- *         or zero if an error occurs.
- */
-uint32_t Sd2Card::cardSize() {
-  csd_t csd;
-  if (!readCSD(&csd)) return 0;
-  if (csd.v1.csd_ver == 0) {
-    uint8_t read_bl_len = csd.v1.read_bl_len;
-    uint16_t c_size = (csd.v1.c_size_high << 10)
-                      | (csd.v1.c_size_mid << 2) | csd.v1.c_size_low;
-    uint8_t c_size_mult = (csd.v1.c_size_mult_high << 1)
-                          | csd.v1.c_size_mult_low;
-    return (uint32_t)(c_size + 1) << (c_size_mult + read_bl_len - 7);
-  } else if (csd.v2.csd_ver == 1) {
-    uint32_t c_size = ((uint32_t)csd.v2.c_size_high << 16)
-                      | (csd.v2.c_size_mid << 8) | csd.v2.c_size_low;
-    return (c_size + 1) << 10;
-  } else {
-    error(SD_CARD_ERROR_BAD_CSD);
-    return 0;
-  }
-}
-//------------------------------------------------------------------------------
-void Sd2Card::chipSelectHigh() {
-  digitalWrite(chipSelectPin_, HIGH);
-}
-//------------------------------------------------------------------------------
-void Sd2Card::chipSelectLow() {
-#ifndef SOFTWARE_SPI
-  spiInit(spiRate_);
-#endif  // SOFTWARE_SPI
-  digitalWrite(chipSelectPin_, LOW);
-}
-//------------------------------------------------------------------------------
-/** Erase a range of blocks.
- *
- * \param[in] firstBlock The address of the first block in the range.
- * \param[in] lastBlock The address of the last block in the range.
- *
- * \note This function requests the SD card to do a flash erase for a
- * range of blocks.  The data on the card after an erase operation is
- * either 0 or 1, depends on the card vendor.  The card must support
- * single block erase.
- *
- * \return The value one, true, is returned for success and
- * the value zero, false, is returned for failure.
- */
-bool Sd2Card::erase(uint32_t firstBlock, uint32_t lastBlock) {
-  csd_t csd;
-  if (!readCSD(&csd)) goto fail;
-  // check for single block erase
-  if (!csd.v1.erase_blk_en) {
-    // erase size mask
-    uint8_t m = (csd.v1.sector_size_high << 1) | csd.v1.sector_size_low;
-    if ((firstBlock & m) != 0 || ((lastBlock + 1) & m) != 0) {
-      // error card can't erase specified area
-      error(SD_CARD_ERROR_ERASE_SINGLE_BLOCK);
-      goto fail;
-    }
-  }
-  if (type_ != SD_CARD_TYPE_SDHC) {
-    firstBlock <<= 9;
-    lastBlock <<= 9;
-  }
-  if (cardCommand(CMD32, firstBlock)
-    || cardCommand(CMD33, lastBlock)
-    || cardCommand(CMD38, 0)) {
-      error(SD_CARD_ERROR_ERASE);
-      goto fail;
-  }
-  if (!waitNotBusy(SD_ERASE_TIMEOUT)) {
-    error(SD_CARD_ERROR_ERASE_TIMEOUT);
-    goto fail;
-  }
-  chipSelectHigh();
-  return true;
-
- fail:
-  chipSelectHigh();
-  return false;
-}
-//------------------------------------------------------------------------------
-/** Determine if card supports single block erase.
- *
- * \return The value one, true, is returned if single block erase is supported.
- * The value zero, false, is returned if single block erase is not supported.
- */
-bool Sd2Card::eraseSingleBlockEnable() {
-  csd_t csd;
-  return readCSD(&csd) ? csd.v1.erase_blk_en : false;
-}
-//------------------------------------------------------------------------------
-/**
- * Initialize an SD flash memory card.
- *
- * \param[in] sckRateID SPI clock rate selector. See setSckRate().
- * \param[in] chipSelectPin SD chip select pin number.
- *
- * \return The value one, true, is returned for success and
- * the value zero, false, is returned for failure.  The reason for failure
- * can be determined by calling errorCode() and errorData().
- */
-bool Sd2Card::init(uint8_t sckRateID, uint8_t chipSelectPin) {
-  errorCode_ = type_ = 0;
-  chipSelectPin_ = chipSelectPin;
-  // 16-bit init start time allows over a minute
-  uint16_t t0 = (uint16_t)millis();
-  uint32_t arg;
-
-  // set pin modes
-  pinMode(chipSelectPin_, OUTPUT);
-  chipSelectHigh();
-  pinMode(SPI_MISO_PIN, INPUT);
-  pinMode(SPI_MOSI_PIN, OUTPUT);
-  pinMode(SPI_SCK_PIN, OUTPUT);
-
-#ifndef SOFTWARE_SPI
-  // SS must be in output mode even it is not chip select
-  pinMode(SS_PIN, OUTPUT);
-  // set SS high - may be chip select for another SPI device
-#if SET_SPI_SS_HIGH
-  digitalWrite(SS_PIN, HIGH);
-#endif  // SET_SPI_SS_HIGH
-  // set SCK rate for initialization commands
-  spiRate_ = SPI_SD_INIT_RATE;
-  spiInit(spiRate_);
-#endif  // SOFTWARE_SPI
-
-  // must supply min of 74 clock cycles with CS high.
-  for (uint8_t i = 0; i < 10; i++) spiSend(0XFF);
-
-  // command to go idle in SPI mode
-  while ((status_ = cardCommand(CMD0, 0)) != R1_IDLE_STATE) {
-    if (((uint16_t)millis() - t0) > SD_INIT_TIMEOUT) {
-      error(SD_CARD_ERROR_CMD0);
-      goto fail;
-    }
-  }
-  // check SD version
-  if ((cardCommand(CMD8, 0x1AA) & R1_ILLEGAL_COMMAND)) {
-    type(SD_CARD_TYPE_SD1);
-  } else {
-    // only need last byte of r7 response
-    for (uint8_t i = 0; i < 4; i++) status_ = spiRec();
-    if (status_ != 0XAA) {
-      error(SD_CARD_ERROR_CMD8);
-      goto fail;
-    }
-    type(SD_CARD_TYPE_SD2);
-  }
-  // initialize card and send host supports SDHC if SD2
-  arg = type() == SD_CARD_TYPE_SD2 ? 0X40000000 : 0;
-
-  while ((status_ = cardAcmd(ACMD41, arg)) != R1_READY_STATE) {
-    // check for timeout
-    if (((uint16_t)millis() - t0) > SD_INIT_TIMEOUT) {
-      error(SD_CARD_ERROR_ACMD41);
-      goto fail;
-    }
-  }
-  // if SD2 read OCR register to check for SDHC card
-  if (type() == SD_CARD_TYPE_SD2) {
-    if (cardCommand(CMD58, 0)) {
-      error(SD_CARD_ERROR_CMD58);
-      goto fail;
-    }
-    if ((spiRec() & 0XC0) == 0XC0) type(SD_CARD_TYPE_SDHC);
-    // discard rest of ocr - contains allowed voltage range
-    for (uint8_t i = 0; i < 3; i++) spiRec();
-  }
-  chipSelectHigh();
-
-#ifndef SOFTWARE_SPI
-  return setSckRate(sckRateID);
-#else  // SOFTWARE_SPI
-  return true;
-#endif  // SOFTWARE_SPI
-
- fail:
-  chipSelectHigh();
-  return false;
-}
-//------------------------------------------------------------------------------
-/**
- * Read a 512 byte block from an SD card.
- *
- * \param[in] blockNumber Logical block to be read.
- * \param[out] dst Pointer to the location that will receive the data.
- * \return The value one, true, is returned for success and
- * the value zero, false, is returned for failure.
- */
-bool Sd2Card::readBlock(uint32_t blockNumber, uint8_t* dst) {
-#ifdef SD_CHECK_AND_RETRY
-  uint8_t retryCnt = 3;
-  // use address if not SDHC card
-  if (type()!= SD_CARD_TYPE_SDHC) blockNumber <<= 9;
- retry2:
-  retryCnt --;
-  if (cardCommand(CMD17, blockNumber)) {
-    error(SD_CARD_ERROR_CMD17);
-    if (retryCnt > 0) goto retry;
-    goto fail;
-  }
-  if (!readData(dst, 512))
-  {
-    if (retryCnt > 0) goto retry;
-    goto fail;
-  }
-  return true;
- retry:
-   chipSelectHigh();
-   cardCommand(CMD12, 0);//Try sending a stop command, but ignore the result.
-   errorCode_ = 0;
-   goto retry2;
-#else
-  // use address if not SDHC card
-  if (type()!= SD_CARD_TYPE_SDHC) blockNumber <<= 9;
-  if (cardCommand(CMD17, blockNumber)) {
-    error(SD_CARD_ERROR_CMD17);
-    goto fail;
-  }
-  return readData(dst, 512);
-#endif
-
- fail:
-  chipSelectHigh();
-  return false;
-}
-//------------------------------------------------------------------------------
-/** Read one data block in a multiple block read sequence
- *
- * \param[in] dst Pointer to the location for the data to be read.
- *
- * \return The value one, true, is returned for success and
- * the value zero, false, is returned for failure.
- */
-bool Sd2Card::readData(uint8_t *dst) {
-  chipSelectLow();
-  return readData(dst, 512);
-}
-
-#ifdef SD_CHECK_AND_RETRY
-static const uint16_t crctab[] PROGMEM = {
-  0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
-  0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
-  0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
-  0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
-  0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
-  0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
-  0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
-  0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
-  0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
-  0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
-  0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
-  0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
-  0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
-  0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
-  0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
-  0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
-  0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
-  0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
-  0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
-  0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
-  0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
-  0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
-  0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
-  0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
-  0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
-  0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
-  0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
-  0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
-  0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
-  0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
-  0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
-  0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
-};
-static uint16_t CRC_CCITT(const uint8_t* data, size_t n) {
-  uint16_t crc = 0;
-  for (size_t i = 0; i < n; i++) {
-    crc = pgm_read_word(&crctab[(crc >> 8 ^ data[i]) & 0XFF]) ^ (crc << 8);
-  }
-  return crc;
-}
-#endif
-
-//------------------------------------------------------------------------------
-bool Sd2Card::readData(uint8_t* dst, uint16_t count) {
-  // wait for start block token
-  uint16_t t0 = millis();
-  while ((status_ = spiRec()) == 0XFF) {
-    if (((uint16_t)millis() - t0) > SD_READ_TIMEOUT) {
-      error(SD_CARD_ERROR_READ_TIMEOUT);
-      goto fail;
-    }
-  }
-  if (status_ != DATA_START_BLOCK) {
-    error(SD_CARD_ERROR_READ);
-    goto fail;
-  }
-  // transfer data
-  spiRead(dst, count);
-
-#ifdef SD_CHECK_AND_RETRY
-  {
-    uint16_t calcCrc = CRC_CCITT(dst, count);
-    uint16_t recvCrc = spiRec() << 8;
-    recvCrc |= spiRec();
-    if (calcCrc != recvCrc)
-    {
-        error(SD_CARD_ERROR_CRC);
-        goto fail;
-    }
-  }
-#else
-  // discard CRC
-  spiRec();
-  spiRec();
-#endif
-  chipSelectHigh();
-  // Toshiba FlashAir Patch. Purge pending status byte.
-  if (flash_air_compatible_)
-    spiSend(0XFF);
-  return true;
-
- fail:
-  chipSelectHigh();
-  // Toshiba FlashAir Patch. Purge pending status byte.
-  if (flash_air_compatible_)
-    spiSend(0XFF);
-  return false;
-}
-//------------------------------------------------------------------------------
-/** read CID or CSR register */
-bool Sd2Card::readRegister(uint8_t cmd, void* buf) {
-  uint8_t* dst = reinterpret_cast<uint8_t*>(buf);
-  if (cardCommand(cmd, 0)) {
-    error(SD_CARD_ERROR_READ_REG);
-    goto fail;
-  }
-  return readData(dst, 16);
-
- fail:
-  chipSelectHigh();
-  return false;
-}
-//------------------------------------------------------------------------------
-/** Start a read multiple blocks sequence.
- *
- * \param[in] blockNumber Address of first block in sequence.
- *
- * \note This function is used with readData() and readStop() for optimized
- * multiple block reads.  SPI chipSelect must be low for the entire sequence.
- *
- * \return The value one, true, is returned for success and
- * the value zero, false, is returned for failure.
- */
-bool Sd2Card::readStart(uint32_t blockNumber) {
-  if (type()!= SD_CARD_TYPE_SDHC) blockNumber <<= 9;
-  if (cardCommand(CMD18, blockNumber)) {
-    error(SD_CARD_ERROR_CMD18);
-    goto fail;
-  }
-  chipSelectHigh();
-  return true;
-
- fail:
-  chipSelectHigh();
-  return false;
-}
-//------------------------------------------------------------------------------
-/** End a read multiple blocks sequence.
- *
-* \return The value one, true, is returned for success and
- * the value zero, false, is returned for failure.
- */
-bool Sd2Card::readStop() {
-  chipSelectLow();
-  if (cardCommand(CMD12, 0)) {
-    error(SD_CARD_ERROR_CMD12);
-    goto fail;
-  }
-  chipSelectHigh();
-  return true;
-
- fail:
-  chipSelectHigh();
-  return false;
-}
-//------------------------------------------------------------------------------
-/**
- * Set the SPI clock rate.
- *
- * \param[in] sckRateID A value in the range [0, 6].
- *
- * The SPI clock will be set to F_CPU/pow(2, 1 + sckRateID). The maximum
- * SPI rate is F_CPU/2 for \a sckRateID = 0 and the minimum rate is F_CPU/128
- * for \a scsRateID = 6.
- *
- * \return The value one, true, is returned for success and the value zero,
- * false, is returned for an invalid value of \a sckRateID.
- */
-bool Sd2Card::setSckRate(uint8_t sckRateID) {
-  if (sckRateID > 6) {
-    error(SD_CARD_ERROR_SCK_RATE);
-    return false;
-  }
-  spiRate_ = sckRateID;
-  return true;
-}
-//------------------------------------------------------------------------------
-// wait for card to go not busy
-bool Sd2Card::waitNotBusy(uint16_t timeoutMillis) {
-  uint16_t t0 = millis();
-  while (spiRec() != 0XFF) {
-    if (((uint16_t)millis() - t0) >= timeoutMillis) goto fail;
-  }
-  return true;
-
- fail:
-  return false;
-}
-//------------------------------------------------------------------------------
-/**
- * Writes a 512 byte block to an SD card.
- *
- * \param[in] blockNumber Logical block to be written.
- * \param[in] src Pointer to the location of the data to be written.
- * \return The value one, true, is returned for success and
- * the value zero, false, is returned for failure.
- */
-bool Sd2Card::writeBlock(uint32_t blockNumber, const uint8_t* src) {
-  // use address if not SDHC card
-  if (type() != SD_CARD_TYPE_SDHC) blockNumber <<= 9;
-  if (cardCommand(CMD24, blockNumber)) {
-    error(SD_CARD_ERROR_CMD24);
-    goto fail;
-  }
-  if (!writeData(DATA_START_BLOCK, src)) goto fail;
-
-  // wait for flash programming to complete
-  if (!waitNotBusy(SD_WRITE_TIMEOUT)) {
-    error(SD_CARD_ERROR_WRITE_TIMEOUT);
-    goto fail;
-  }
-  // response is r2 so get and check two bytes for nonzero
-  if (cardCommand(CMD13, 0) || spiRec()) {
-    error(SD_CARD_ERROR_WRITE_PROGRAMMING);
-    goto fail;
-  }
-  chipSelectHigh();
-  return true;
-
- fail:
-  chipSelectHigh();
-  return false;
-}
-//------------------------------------------------------------------------------
-/** Write one data block in a multiple block write sequence
- * \param[in] src Pointer to the location of the data to be written.
- * \return The value one, true, is returned for success and
- * the value zero, false, is returned for failure.
- */
-bool Sd2Card::writeData(const uint8_t* src) {
-  chipSelectLow();
-  // wait for previous write to finish
-  if (!waitNotBusy(SD_WRITE_TIMEOUT)) goto fail;
-  if (!writeData(WRITE_MULTIPLE_TOKEN, src)) goto fail;
-  chipSelectHigh();
-  return true;
-
- fail:
-  error(SD_CARD_ERROR_WRITE_MULTIPLE);
-  chipSelectHigh();
-  return false;
-}
-//------------------------------------------------------------------------------
-// send one block of data for write block or write multiple blocks
-bool Sd2Card::writeData(uint8_t token, const uint8_t* src) {
-  spiSendBlock(token, src);
-
-  spiSend(0xff);  // dummy crc
-  spiSend(0xff);  // dummy crc
-
-  status_ = spiRec();
-  if ((status_ & DATA_RES_MASK) != DATA_RES_ACCEPTED) {
-    error(SD_CARD_ERROR_WRITE);
-    goto fail;
-  }
-  return true;
-
- fail:
-  chipSelectHigh();
-  return false;
-}
-//------------------------------------------------------------------------------
-/** Start a write multiple blocks sequence.
- *
- * \param[in] blockNumber Address of first block in sequence.
- * \param[in] eraseCount The number of blocks to be pre-erased.
- *
- * \note This function is used with writeData() and writeStop()
- * for optimized multiple block writes.
- *
- * \return The value one, true, is returned for success and
- * the value zero, false, is returned for failure.
- */
-bool Sd2Card::writeStart(uint32_t blockNumber, uint32_t eraseCount) {
-  // send pre-erase count
-  if (cardAcmd(ACMD23, eraseCount)) {
-    error(SD_CARD_ERROR_ACMD23);
-    goto fail;
-  }
-  // use address if not SDHC card
-  if (type() != SD_CARD_TYPE_SDHC) blockNumber <<= 9;
-  if (cardCommand(CMD25, blockNumber)) {
-    error(SD_CARD_ERROR_CMD25);
-    goto fail;
-  }
-  chipSelectHigh();
-  return true;
-
- fail:
-  chipSelectHigh();
-  return false;
-}
-//------------------------------------------------------------------------------
-/** End a write multiple blocks sequence.
- *
-* \return The value one, true, is returned for success and
- * the value zero, false, is returned for failure.
- */
-bool Sd2Card::writeStop() {
-  chipSelectLow();
-  if (!waitNotBusy(SD_WRITE_TIMEOUT)) goto fail;
-  spiSend(STOP_TRAN_TOKEN);
-  if (!waitNotBusy(SD_WRITE_TIMEOUT)) goto fail;
-  chipSelectHigh();
-  return true;
-
- fail:
-  error(SD_CARD_ERROR_STOP_TRAN);
-  chipSelectHigh();
-  return false;
-}
-
-//------------------------------------------------------------------------------
-/** Wait for start block token */
-//FIXME Vojtech: Copied from a current version of Sd2Card Arduino code.
-// We shall likely upgrade the rest of the Sd2Card.
-uint8_t Sd2Card::waitStartBlock(void) {
-  uint16_t t0 = millis();
-  while ((status_ = spiRec()) == 0XFF) {
-    if (((uint16_t)millis() - t0) > SD_READ_TIMEOUT) {
-      error(SD_CARD_ERROR_READ_TIMEOUT);
-      goto fail;
-    }
-  }
-  if (status_ != DATA_START_BLOCK) {
-    error(SD_CARD_ERROR_READ);
-    goto fail;
-  }
-  return true;
-
- fail:
-  chipSelectHigh();
-  return false;
-}
-
-// Toshiba FlashAir support, copied from 
-// https://flashair-developers.com/en/documents/tutorials/arduino/
-
-//------------------------------------------------------------------------------
-/** Perform Extention Read. */
-uint8_t Sd2Card::readExt(uint32_t arg, uint8_t* dst, uint16_t count) {
-  uint16_t i;
-
-  // send command and argument.
-  if (cardCommand(CMD48, arg)) {
-    error(SD_CARD_ERROR_CMD48);
-    goto fail;
-  }
-  
-  // wait for start block token.
-  if (!waitStartBlock()) {
-    goto fail;
-  }
-
-  // receive data
-  for (i = 0; i < count; ++i) {
-    dst[i] = spiRec();
-  }
-  
-  // skip dummy bytes and 16-bit crc.
-  for (; i < 514; ++i) {
-    spiRec();
-  }
-
-  chipSelectHigh();
-  spiSend(0xFF); // dummy clock to force FlashAir finish the command.
-  return true;
-
- fail:
-  chipSelectHigh();
-  return false;
-}
-
-//------------------------------------------------------------------------------
-/**
- * Read an extension register space.
- *
- * \return The value one, true, is returned for success and
- * the value zero, false, is returned for failure.
- */
-uint8_t Sd2Card::readExtMemory(uint8_t mio, uint8_t func, 
-    uint32_t addr, uint16_t count, uint8_t* dst) {
-  uint32_t offset = addr & 0x1FF;
-  if (offset + count > 512) count = 512 - offset;
-  
-  if (count == 0) return true;
-  
-  uint32_t arg = 
-      (((uint32_t)mio & 0x1) << 31) | 
-    (mio ? (((uint32_t)func & 0x7) << 28) : (((uint32_t)func & 0xF) << 27)) |
-    ((addr & 0x1FFFF) << 9) |
-    ((count - 1) & 0x1FF);
-  
-  return readExt(arg, dst, count);
-}
-
-#endif
+/* Arduino Sd2Card Library
+ * Copyright (C) 2009 by William Greiman
+ *
+ * This file is part of the Arduino Sd2Card Library
+ *
+ * This Library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This Library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino Sd2Card Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+#include "Marlin.h"
+
+#ifdef SDSUPPORT
+#include "Sd2Card.h"
+//------------------------------------------------------------------------------
+#ifndef SOFTWARE_SPI
+// functions for hardware SPI
+//------------------------------------------------------------------------------
+// make sure SPCR rate is in expected bits
+#if (SPR0 != 0 || SPR1 != 1)
+#error unexpected SPCR bits
+#endif
+/**
+ * Initialize hardware SPI
+ * Set SCK rate to F_CPU/pow(2, 1 + spiRate) for spiRate [0,6]
+ */
+static void spiInit(uint8_t spiRate) {
+  // See avr processor documentation
+  SPCR = (1 << SPE) | (1 << MSTR) | (spiRate >> 1);
+  SPSR = spiRate & 1 || spiRate == 6 ? 0 : 1 << SPI2X;
+}
+//------------------------------------------------------------------------------
+/** SPI receive a byte */
+static uint8_t spiRec() {
+  SPDR = 0XFF;
+  while (!(SPSR & (1 << SPIF))) { /* Intentionally left empty */ }
+  return SPDR;
+}
+//------------------------------------------------------------------------------
+/** SPI read data - only one call so force inline */
+static inline __attribute__((always_inline))
+void spiRead(uint8_t* buf, uint16_t nbyte) {
+  if (nbyte-- == 0) return;
+  SPDR = 0XFF;
+  for (uint16_t i = 0; i < nbyte; i++) {
+    while (!(SPSR & (1 << SPIF))) { /* Intentionally left empty */ }
+    buf[i] = SPDR;
+    SPDR = 0XFF;
+  }
+  while (!(SPSR & (1 << SPIF))) { /* Intentionally left empty */ }
+  buf[nbyte] = SPDR;
+}
+//------------------------------------------------------------------------------
+/** SPI send a byte */
+static void spiSend(uint8_t b) {
+  SPDR = b;
+  while (!(SPSR & (1 << SPIF))) { /* Intentionally left empty */ }
+}
+//------------------------------------------------------------------------------
+/** SPI send block - only one call so force inline */
+static inline __attribute__((always_inline))
+  void spiSendBlock(uint8_t token, const uint8_t* buf) {
+  SPDR = token;
+  for (uint16_t i = 0; i < 512; i += 2) {
+    while (!(SPSR & (1 << SPIF))) { /* Intentionally left empty */ }
+    SPDR = buf[i];
+    while (!(SPSR & (1 << SPIF))) { /* Intentionally left empty */ }
+    SPDR = buf[i + 1];
+  }
+  while (!(SPSR & (1 << SPIF))) { /* Intentionally left empty */ }
+}
+//------------------------------------------------------------------------------
+#else  // SOFTWARE_SPI
+//------------------------------------------------------------------------------
+/** nop to tune soft SPI timing */
+#define nop asm volatile ("nop\n\t")
+//------------------------------------------------------------------------------
+/** Soft SPI receive byte */
+static uint8_t spiRec() {
+  uint8_t data = 0;
+  // no interrupts during byte receive - about 8 us
+  cli();
+  // output pin high - like sending 0XFF
+  fastDigitalWrite(SPI_MOSI_PIN, HIGH);
+
+  for (uint8_t i = 0; i < 8; i++) {
+    fastDigitalWrite(SPI_SCK_PIN, HIGH);
+
+    // adjust so SCK is nice
+    nop;
+    nop;
+
+    data <<= 1;
+
+    if (fastDigitalRead(SPI_MISO_PIN)) data |= 1;
+
+    fastDigitalWrite(SPI_SCK_PIN, LOW);
+  }
+  // enable interrupts
+  sei();
+  return data;
+}
+//------------------------------------------------------------------------------
+/** Soft SPI read data */
+static void spiRead(uint8_t* buf, uint16_t nbyte) {
+  for (uint16_t i = 0; i < nbyte; i++) {
+    buf[i] = spiRec();
+  }
+}
+//------------------------------------------------------------------------------
+/** Soft SPI send byte */
+static void spiSend(uint8_t data) {
+  // no interrupts during byte send - about 8 us
+  cli();
+  for (uint8_t i = 0; i < 8; i++) {
+    fastDigitalWrite(SPI_SCK_PIN, LOW);
+
+    fastDigitalWrite(SPI_MOSI_PIN, data & 0X80);
+
+    data <<= 1;
+
+    fastDigitalWrite(SPI_SCK_PIN, HIGH);
+  }
+  // hold SCK high for a few ns
+  nop;
+  nop;
+  nop;
+  nop;
+
+  fastDigitalWrite(SPI_SCK_PIN, LOW);
+  // enable interrupts
+  sei();
+}
+//------------------------------------------------------------------------------
+/** Soft SPI send block */
+  void spiSendBlock(uint8_t token, const uint8_t* buf) {
+  spiSend(token);
+  for (uint16_t i = 0; i < 512; i++) {
+    spiSend(buf[i]);
+  }
+}
+#endif  // SOFTWARE_SPI
+//------------------------------------------------------------------------------
+// send command and return error code.  Return zero for OK
+uint8_t Sd2Card::cardCommand(uint8_t cmd, uint32_t arg) {
+  // select card
+  chipSelectLow();
+
+  // wait up to 300 ms if busy
+  waitNotBusy(300);
+
+  // send command
+  spiSend(cmd | 0x40);
+
+  // send argument
+  for (int8_t s = 24; s >= 0; s -= 8) spiSend(arg >> s);
+
+  // send CRC
+  uint8_t crc = 0XFF;
+  if (cmd == CMD0) crc = 0X95;  // correct crc for CMD0 with arg 0
+  if (cmd == CMD8) crc = 0X87;  // correct crc for CMD8 with arg 0X1AA
+  spiSend(crc);
+
+  // skip stuff byte for stop read
+  if (cmd == CMD12) spiRec();
+
+  // wait for response
+  for (uint8_t i = 0; ((status_ = spiRec()) & 0X80) && i != 0XFF; i++) { /* Intentionally left empty */ }
+  return status_;
+}
+//------------------------------------------------------------------------------
+/**
+ * Determine the size of an SD flash memory card.
+ *
+ * \return The number of 512 byte data blocks in the card
+ *         or zero if an error occurs.
+ */
+uint32_t Sd2Card::cardSize() {
+  csd_t csd;
+  if (!readCSD(&csd)) return 0;
+  if (csd.v1.csd_ver == 0) {
+    uint8_t read_bl_len = csd.v1.read_bl_len;
+    uint16_t c_size = (csd.v1.c_size_high << 10)
+                      | (csd.v1.c_size_mid << 2) | csd.v1.c_size_low;
+    uint8_t c_size_mult = (csd.v1.c_size_mult_high << 1)
+                          | csd.v1.c_size_mult_low;
+    return (uint32_t)(c_size + 1) << (c_size_mult + read_bl_len - 7);
+  } else if (csd.v2.csd_ver == 1) {
+    uint32_t c_size = ((uint32_t)csd.v2.c_size_high << 16)
+                      | (csd.v2.c_size_mid << 8) | csd.v2.c_size_low;
+    return (c_size + 1) << 10;
+  } else {
+    error(SD_CARD_ERROR_BAD_CSD);
+    return 0;
+  }
+}
+//------------------------------------------------------------------------------
+void Sd2Card::chipSelectHigh() {
+  digitalWrite(chipSelectPin_, HIGH);
+}
+//------------------------------------------------------------------------------
+void Sd2Card::chipSelectLow() {
+#ifndef SOFTWARE_SPI
+  spiInit(spiRate_);
+#endif  // SOFTWARE_SPI
+  digitalWrite(chipSelectPin_, LOW);
+}
+//------------------------------------------------------------------------------
+/** Erase a range of blocks.
+ *
+ * \param[in] firstBlock The address of the first block in the range.
+ * \param[in] lastBlock The address of the last block in the range.
+ *
+ * \note This function requests the SD card to do a flash erase for a
+ * range of blocks.  The data on the card after an erase operation is
+ * either 0 or 1, depends on the card vendor.  The card must support
+ * single block erase.
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ */
+bool Sd2Card::erase(uint32_t firstBlock, uint32_t lastBlock) {
+  csd_t csd;
+  if (!readCSD(&csd)) goto fail;
+  // check for single block erase
+  if (!csd.v1.erase_blk_en) {
+    // erase size mask
+    uint8_t m = (csd.v1.sector_size_high << 1) | csd.v1.sector_size_low;
+    if ((firstBlock & m) != 0 || ((lastBlock + 1) & m) != 0) {
+      // error card can't erase specified area
+      error(SD_CARD_ERROR_ERASE_SINGLE_BLOCK);
+      goto fail;
+    }
+  }
+  if (type_ != SD_CARD_TYPE_SDHC) {
+    firstBlock <<= 9;
+    lastBlock <<= 9;
+  }
+  if (cardCommand(CMD32, firstBlock)
+    || cardCommand(CMD33, lastBlock)
+    || cardCommand(CMD38, 0)) {
+      error(SD_CARD_ERROR_ERASE);
+      goto fail;
+  }
+  if (!waitNotBusy(SD_ERASE_TIMEOUT)) {
+    error(SD_CARD_ERROR_ERASE_TIMEOUT);
+    goto fail;
+  }
+  chipSelectHigh();
+  return true;
+
+ fail:
+  chipSelectHigh();
+  return false;
+}
+//------------------------------------------------------------------------------
+/** Determine if card supports single block erase.
+ *
+ * \return The value one, true, is returned if single block erase is supported.
+ * The value zero, false, is returned if single block erase is not supported.
+ */
+bool Sd2Card::eraseSingleBlockEnable() {
+  csd_t csd;
+  return readCSD(&csd) ? csd.v1.erase_blk_en : false;
+}
+//------------------------------------------------------------------------------
+/**
+ * Initialize an SD flash memory card.
+ *
+ * \param[in] sckRateID SPI clock rate selector. See setSckRate().
+ * \param[in] chipSelectPin SD chip select pin number.
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.  The reason for failure
+ * can be determined by calling errorCode() and errorData().
+ */
+bool Sd2Card::init(uint8_t sckRateID, uint8_t chipSelectPin) {
+  errorCode_ = type_ = 0;
+  chipSelectPin_ = chipSelectPin;
+  // 16-bit init start time allows over a minute
+  uint16_t t0 = (uint16_t)millis();
+  uint32_t arg;
+
+  // set pin modes
+  pinMode(chipSelectPin_, OUTPUT);
+  chipSelectHigh();
+  pinMode(SPI_MISO_PIN, INPUT);
+  pinMode(SPI_MOSI_PIN, OUTPUT);
+  pinMode(SPI_SCK_PIN, OUTPUT);
+
+#ifndef SOFTWARE_SPI
+  // SS must be in output mode even it is not chip select
+  pinMode(SS_PIN, OUTPUT);
+  // set SS high - may be chip select for another SPI device
+#if SET_SPI_SS_HIGH
+  digitalWrite(SS_PIN, HIGH);
+#endif  // SET_SPI_SS_HIGH
+  // set SCK rate for initialization commands
+  spiRate_ = SPI_SD_INIT_RATE;
+  spiInit(spiRate_);
+#endif  // SOFTWARE_SPI
+
+  // must supply min of 74 clock cycles with CS high.
+  for (uint8_t i = 0; i < 10; i++) spiSend(0XFF);
+
+  // command to go idle in SPI mode
+  while ((status_ = cardCommand(CMD0, 0)) != R1_IDLE_STATE) {
+    if (((uint16_t)millis() - t0) > SD_INIT_TIMEOUT) {
+      error(SD_CARD_ERROR_CMD0);
+      goto fail;
+    }
+  }
+  // check SD version
+  if ((cardCommand(CMD8, 0x1AA) & R1_ILLEGAL_COMMAND)) {
+    type(SD_CARD_TYPE_SD1);
+  } else {
+    // only need last byte of r7 response
+    for (uint8_t i = 0; i < 4; i++) status_ = spiRec();
+    if (status_ != 0XAA) {
+      error(SD_CARD_ERROR_CMD8);
+      goto fail;
+    }
+    type(SD_CARD_TYPE_SD2);
+  }
+  // initialize card and send host supports SDHC if SD2
+  arg = type() == SD_CARD_TYPE_SD2 ? 0X40000000 : 0;
+
+  while ((status_ = cardAcmd(ACMD41, arg)) != R1_READY_STATE) {
+    // check for timeout
+    if (((uint16_t)millis() - t0) > SD_INIT_TIMEOUT) {
+      error(SD_CARD_ERROR_ACMD41);
+      goto fail;
+    }
+  }
+  // if SD2 read OCR register to check for SDHC card
+  if (type() == SD_CARD_TYPE_SD2) {
+    if (cardCommand(CMD58, 0)) {
+      error(SD_CARD_ERROR_CMD58);
+      goto fail;
+    }
+    if ((spiRec() & 0XC0) == 0XC0) type(SD_CARD_TYPE_SDHC);
+    // discard rest of ocr - contains allowed voltage range
+    for (uint8_t i = 0; i < 3; i++) spiRec();
+  }
+  chipSelectHigh();
+
+#ifndef SOFTWARE_SPI
+  return setSckRate(sckRateID);
+#else  // SOFTWARE_SPI
+  return true;
+#endif  // SOFTWARE_SPI
+
+ fail:
+  chipSelectHigh();
+  return false;
+}
+//------------------------------------------------------------------------------
+/**
+ * Read a 512 byte block from an SD card.
+ *
+ * \param[in] blockNumber Logical block to be read.
+ * \param[out] dst Pointer to the location that will receive the data.
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ */
+bool Sd2Card::readBlock(uint32_t blockNumber, uint8_t* dst) {
+#ifdef SD_CHECK_AND_RETRY
+  uint8_t retryCnt = 3;
+  // use address if not SDHC card
+  if (type()!= SD_CARD_TYPE_SDHC) blockNumber <<= 9;
+ retry2:
+  retryCnt --;
+  if (cardCommand(CMD17, blockNumber)) {
+    error(SD_CARD_ERROR_CMD17);
+    if (retryCnt > 0) goto retry;
+    goto fail;
+  }
+  if (!readData(dst, 512))
+  {
+    if (retryCnt > 0) goto retry;
+    goto fail;
+  }
+  return true;
+ retry:
+   chipSelectHigh();
+   cardCommand(CMD12, 0);//Try sending a stop command, but ignore the result.
+   errorCode_ = 0;
+   goto retry2;
+#else
+  // use address if not SDHC card
+  if (type()!= SD_CARD_TYPE_SDHC) blockNumber <<= 9;
+  if (cardCommand(CMD17, blockNumber)) {
+    error(SD_CARD_ERROR_CMD17);
+    goto fail;
+  }
+  return readData(dst, 512);
+#endif
+
+ fail:
+  chipSelectHigh();
+  return false;
+}
+//------------------------------------------------------------------------------
+/** Read one data block in a multiple block read sequence
+ *
+ * \param[in] dst Pointer to the location for the data to be read.
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ */
+bool Sd2Card::readData(uint8_t *dst) {
+  chipSelectLow();
+  return readData(dst, 512);
+}
+
+#ifdef SD_CHECK_AND_RETRY
+static const uint16_t crctab[] PROGMEM = {
+  0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
+  0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
+  0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
+  0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
+  0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
+  0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
+  0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
+  0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
+  0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
+  0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
+  0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
+  0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
+  0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
+  0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
+  0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
+  0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
+  0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
+  0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
+  0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
+  0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
+  0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
+  0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+  0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
+  0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
+  0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
+  0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
+  0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
+  0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
+  0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
+  0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
+  0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
+  0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
+};
+static uint16_t CRC_CCITT(const uint8_t* data, size_t n) {
+  uint16_t crc = 0;
+  for (size_t i = 0; i < n; i++) {
+    crc = pgm_read_word(&crctab[(crc >> 8 ^ data[i]) & 0XFF]) ^ (crc << 8);
+  }
+  return crc;
+}
+#endif
+
+//------------------------------------------------------------------------------
+bool Sd2Card::readData(uint8_t* dst, uint16_t count) {
+  // wait for start block token
+  uint16_t t0 = millis();
+  while ((status_ = spiRec()) == 0XFF) {
+    if (((uint16_t)millis() - t0) > SD_READ_TIMEOUT) {
+      error(SD_CARD_ERROR_READ_TIMEOUT);
+      goto fail;
+    }
+  }
+  if (status_ != DATA_START_BLOCK) {
+    error(SD_CARD_ERROR_READ);
+    goto fail;
+  }
+  // transfer data
+  spiRead(dst, count);
+
+#ifdef SD_CHECK_AND_RETRY
+  {
+    uint16_t calcCrc = CRC_CCITT(dst, count);
+    uint16_t recvCrc = spiRec() << 8;
+    recvCrc |= spiRec();
+    if (calcCrc != recvCrc)
+    {
+        error(SD_CARD_ERROR_CRC);
+        goto fail;
+    }
+  }
+#else
+  // discard CRC
+  spiRec();
+  spiRec();
+#endif
+  chipSelectHigh();
+  // Toshiba FlashAir Patch. Purge pending status byte.
+  if (flash_air_compatible_)
+    spiSend(0XFF);
+  return true;
+
+ fail:
+  chipSelectHigh();
+  // Toshiba FlashAir Patch. Purge pending status byte.
+  if (flash_air_compatible_)
+    spiSend(0XFF);
+  return false;
+}
+//------------------------------------------------------------------------------
+/** read CID or CSR register */
+bool Sd2Card::readRegister(uint8_t cmd, void* buf) {
+  uint8_t* dst = reinterpret_cast<uint8_t*>(buf);
+  if (cardCommand(cmd, 0)) {
+    error(SD_CARD_ERROR_READ_REG);
+    goto fail;
+  }
+  return readData(dst, 16);
+
+ fail:
+  chipSelectHigh();
+  return false;
+}
+//------------------------------------------------------------------------------
+/** Start a read multiple blocks sequence.
+ *
+ * \param[in] blockNumber Address of first block in sequence.
+ *
+ * \note This function is used with readData() and readStop() for optimized
+ * multiple block reads.  SPI chipSelect must be low for the entire sequence.
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ */
+bool Sd2Card::readStart(uint32_t blockNumber) {
+  if (type()!= SD_CARD_TYPE_SDHC) blockNumber <<= 9;
+  if (cardCommand(CMD18, blockNumber)) {
+    error(SD_CARD_ERROR_CMD18);
+    goto fail;
+  }
+  chipSelectHigh();
+  return true;
+
+ fail:
+  chipSelectHigh();
+  return false;
+}
+//------------------------------------------------------------------------------
+/** End a read multiple blocks sequence.
+ *
+* \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ */
+bool Sd2Card::readStop() {
+  chipSelectLow();
+  if (cardCommand(CMD12, 0)) {
+    error(SD_CARD_ERROR_CMD12);
+    goto fail;
+  }
+  chipSelectHigh();
+  return true;
+
+ fail:
+  chipSelectHigh();
+  return false;
+}
+//------------------------------------------------------------------------------
+/**
+ * Set the SPI clock rate.
+ *
+ * \param[in] sckRateID A value in the range [0, 6].
+ *
+ * The SPI clock will be set to F_CPU/pow(2, 1 + sckRateID). The maximum
+ * SPI rate is F_CPU/2 for \a sckRateID = 0 and the minimum rate is F_CPU/128
+ * for \a scsRateID = 6.
+ *
+ * \return The value one, true, is returned for success and the value zero,
+ * false, is returned for an invalid value of \a sckRateID.
+ */
+bool Sd2Card::setSckRate(uint8_t sckRateID) {
+  if (sckRateID > 6) {
+    error(SD_CARD_ERROR_SCK_RATE);
+    return false;
+  }
+  spiRate_ = sckRateID;
+  return true;
+}
+//------------------------------------------------------------------------------
+// wait for card to go not busy
+bool Sd2Card::waitNotBusy(uint16_t timeoutMillis) {
+  uint16_t t0 = millis();
+  while (spiRec() != 0XFF) {
+    if (((uint16_t)millis() - t0) >= timeoutMillis) goto fail;
+  }
+  return true;
+
+ fail:
+  return false;
+}
+//------------------------------------------------------------------------------
+/**
+ * Writes a 512 byte block to an SD card.
+ *
+ * \param[in] blockNumber Logical block to be written.
+ * \param[in] src Pointer to the location of the data to be written.
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ */
+bool Sd2Card::writeBlock(uint32_t blockNumber, const uint8_t* src) {
+  // use address if not SDHC card
+  if (type() != SD_CARD_TYPE_SDHC) blockNumber <<= 9;
+  if (cardCommand(CMD24, blockNumber)) {
+    error(SD_CARD_ERROR_CMD24);
+    goto fail;
+  }
+  if (!writeData(DATA_START_BLOCK, src)) goto fail;
+
+  // wait for flash programming to complete
+  if (!waitNotBusy(SD_WRITE_TIMEOUT)) {
+    error(SD_CARD_ERROR_WRITE_TIMEOUT);
+    goto fail;
+  }
+  // response is r2 so get and check two bytes for nonzero
+  if (cardCommand(CMD13, 0) || spiRec()) {
+    error(SD_CARD_ERROR_WRITE_PROGRAMMING);
+    goto fail;
+  }
+  chipSelectHigh();
+  return true;
+
+ fail:
+  chipSelectHigh();
+  return false;
+}
+//------------------------------------------------------------------------------
+/** Write one data block in a multiple block write sequence
+ * \param[in] src Pointer to the location of the data to be written.
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ */
+bool Sd2Card::writeData(const uint8_t* src) {
+  chipSelectLow();
+  // wait for previous write to finish
+  if (!waitNotBusy(SD_WRITE_TIMEOUT)) goto fail;
+  if (!writeData(WRITE_MULTIPLE_TOKEN, src)) goto fail;
+  chipSelectHigh();
+  return true;
+
+ fail:
+  error(SD_CARD_ERROR_WRITE_MULTIPLE);
+  chipSelectHigh();
+  return false;
+}
+//------------------------------------------------------------------------------
+// send one block of data for write block or write multiple blocks
+bool Sd2Card::writeData(uint8_t token, const uint8_t* src) {
+  spiSendBlock(token, src);
+
+  spiSend(0xff);  // dummy crc
+  spiSend(0xff);  // dummy crc
+
+  status_ = spiRec();
+  if ((status_ & DATA_RES_MASK) != DATA_RES_ACCEPTED) {
+    error(SD_CARD_ERROR_WRITE);
+    goto fail;
+  }
+  return true;
+
+ fail:
+  chipSelectHigh();
+  return false;
+}
+//------------------------------------------------------------------------------
+/** Start a write multiple blocks sequence.
+ *
+ * \param[in] blockNumber Address of first block in sequence.
+ * \param[in] eraseCount The number of blocks to be pre-erased.
+ *
+ * \note This function is used with writeData() and writeStop()
+ * for optimized multiple block writes.
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ */
+bool Sd2Card::writeStart(uint32_t blockNumber, uint32_t eraseCount) {
+  // send pre-erase count
+  if (cardAcmd(ACMD23, eraseCount)) {
+    error(SD_CARD_ERROR_ACMD23);
+    goto fail;
+  }
+  // use address if not SDHC card
+  if (type() != SD_CARD_TYPE_SDHC) blockNumber <<= 9;
+  if (cardCommand(CMD25, blockNumber)) {
+    error(SD_CARD_ERROR_CMD25);
+    goto fail;
+  }
+  chipSelectHigh();
+  return true;
+
+ fail:
+  chipSelectHigh();
+  return false;
+}
+//------------------------------------------------------------------------------
+/** End a write multiple blocks sequence.
+ *
+* \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ */
+bool Sd2Card::writeStop() {
+  chipSelectLow();
+  if (!waitNotBusy(SD_WRITE_TIMEOUT)) goto fail;
+  spiSend(STOP_TRAN_TOKEN);
+  if (!waitNotBusy(SD_WRITE_TIMEOUT)) goto fail;
+  chipSelectHigh();
+  return true;
+
+ fail:
+  error(SD_CARD_ERROR_STOP_TRAN);
+  chipSelectHigh();
+  return false;
+}
+
+//------------------------------------------------------------------------------
+/** Wait for start block token */
+//FIXME Vojtech: Copied from a current version of Sd2Card Arduino code.
+// We shall likely upgrade the rest of the Sd2Card.
+uint8_t Sd2Card::waitStartBlock(void) {
+  uint16_t t0 = millis();
+  while ((status_ = spiRec()) == 0XFF) {
+    if (((uint16_t)millis() - t0) > SD_READ_TIMEOUT) {
+      error(SD_CARD_ERROR_READ_TIMEOUT);
+      goto fail;
+    }
+  }
+  if (status_ != DATA_START_BLOCK) {
+    error(SD_CARD_ERROR_READ);
+    goto fail;
+  }
+  return true;
+
+ fail:
+  chipSelectHigh();
+  return false;
+}
+
+// Toshiba FlashAir support, copied from 
+// https://flashair-developers.com/en/documents/tutorials/arduino/
+
+//------------------------------------------------------------------------------
+/** Perform Extention Read. */
+uint8_t Sd2Card::readExt(uint32_t arg, uint8_t* dst, uint16_t count) {
+  uint16_t i;
+
+  // send command and argument.
+  if (cardCommand(CMD48, arg)) {
+    error(SD_CARD_ERROR_CMD48);
+    goto fail;
+  }
+  
+  // wait for start block token.
+  if (!waitStartBlock()) {
+    goto fail;
+  }
+
+  // receive data
+  for (i = 0; i < count; ++i) {
+    dst[i] = spiRec();
+  }
+  
+  // skip dummy bytes and 16-bit crc.
+  for (; i < 514; ++i) {
+    spiRec();
+  }
+
+  chipSelectHigh();
+  spiSend(0xFF); // dummy clock to force FlashAir finish the command.
+  return true;
+
+ fail:
+  chipSelectHigh();
+  return false;
+}
+
+//------------------------------------------------------------------------------
+/**
+ * Read an extension register space.
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ */
+uint8_t Sd2Card::readExtMemory(uint8_t mio, uint8_t func, 
+    uint32_t addr, uint16_t count, uint8_t* dst) {
+  uint32_t offset = addr & 0x1FF;
+  if (offset + count > 512) count = 512 - offset;
+  
+  if (count == 0) return true;
+  
+  uint32_t arg = 
+      (((uint32_t)mio & 0x1) << 31) | 
+    (mio ? (((uint32_t)func & 0x7) << 28) : (((uint32_t)func & 0xF) << 27)) |
+    ((addr & 0x1FFFF) << 9) |
+    ((count - 1) & 0x1FF);
+  
+  return readExt(arg, dst, count);
+}
+
+#endif

+ 261 - 261
Firmware/Sd2Card.h

@@ -1,262 +1,262 @@
-/* Arduino Sd2Card Library
- * Copyright (C) 2009 by William Greiman
- *
- * This file is part of the Arduino Sd2Card Library
- *
- * This Library is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This Library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with the Arduino Sd2Card Library.  If not, see
- * <http://www.gnu.org/licenses/>.
- */
-
-#include "Marlin.h"
-#ifdef SDSUPPORT
-
-#ifndef Sd2Card_h
-#define Sd2Card_h
-/**
- * \file
- * \brief Sd2Card class for V2 SD/SDHC cards
- */
-#include "SdFatConfig.h"
-#include "Sd2PinMap.h"
-#include "SdInfo.h"
-//------------------------------------------------------------------------------
-// SPI speed is F_CPU/2^(1 + index), 0 <= index <= 6
-/** Set SCK to max rate of F_CPU/2. See Sd2Card::setSckRate(). */
-uint8_t const SPI_FULL_SPEED = 0;
-/** Set SCK rate to F_CPU/4. See Sd2Card::setSckRate(). */
-uint8_t const SPI_HALF_SPEED = 1;
-/** Set SCK rate to F_CPU/8. See Sd2Card::setSckRate(). */
-uint8_t const SPI_QUARTER_SPEED = 2;
-/** Set SCK rate to F_CPU/16. See Sd2Card::setSckRate(). */
-uint8_t const SPI_EIGHTH_SPEED = 3;
-/** Set SCK rate to F_CPU/32. See Sd2Card::setSckRate(). */
-uint8_t const SPI_SIXTEENTH_SPEED = 4;
-//------------------------------------------------------------------------------
-/** init timeout ms */
-uint16_t const SD_INIT_TIMEOUT = 2000;
-/** erase timeout ms */
-uint16_t const SD_ERASE_TIMEOUT = 10000;
-/** read timeout ms */
-uint16_t const SD_READ_TIMEOUT = 300;
-/** write time out ms */
-uint16_t const SD_WRITE_TIMEOUT = 600;
-//------------------------------------------------------------------------------
-// SD card errors
-/** timeout error for command CMD0 (initialize card in SPI mode) */
-uint8_t const SD_CARD_ERROR_CMD0 = 0X1;
-/** CMD8 was not accepted - not a valid SD card*/
-uint8_t const SD_CARD_ERROR_CMD8 = 0X2;
-/** card returned an error response for CMD12 (write stop) */
-uint8_t const SD_CARD_ERROR_CMD12 = 0X3;
-/** card returned an error response for CMD17 (read block) */
-uint8_t const SD_CARD_ERROR_CMD17 = 0X4;
-/** card returned an error response for CMD18 (read multiple block) */
-uint8_t const SD_CARD_ERROR_CMD18 = 0X5;
-/** card returned an error response for CMD24 (write block) */
-uint8_t const SD_CARD_ERROR_CMD24 = 0X6;
-/**  WRITE_MULTIPLE_BLOCKS command failed */
-uint8_t const SD_CARD_ERROR_CMD25 = 0X7;
-/** card returned an error response for CMD58 (read OCR) */
-uint8_t const SD_CARD_ERROR_CMD58 = 0X8;
-/** SET_WR_BLK_ERASE_COUNT failed */
-uint8_t const SD_CARD_ERROR_ACMD23 = 0X9;
-/** ACMD41 initialization process timeout */
-uint8_t const SD_CARD_ERROR_ACMD41 = 0XA;
-/** card returned a bad CSR version field */
-uint8_t const SD_CARD_ERROR_BAD_CSD = 0XB;
-/** erase block group command failed */
-uint8_t const SD_CARD_ERROR_ERASE = 0XC;
-/** card not capable of single block erase */
-uint8_t const SD_CARD_ERROR_ERASE_SINGLE_BLOCK = 0XD;
-/** Erase sequence timed out */
-uint8_t const SD_CARD_ERROR_ERASE_TIMEOUT = 0XE;
-/** card returned an error token instead of read data */
-uint8_t const SD_CARD_ERROR_READ = 0XF;
-/** read CID or CSD failed */
-uint8_t const SD_CARD_ERROR_READ_REG = 0X10;
-/** timeout while waiting for start of read data */
-uint8_t const SD_CARD_ERROR_READ_TIMEOUT = 0X11;
-/** card did not accept STOP_TRAN_TOKEN */
-uint8_t const SD_CARD_ERROR_STOP_TRAN = 0X12;
-/** card returned an error token as a response to a write operation */
-uint8_t const SD_CARD_ERROR_WRITE = 0X13;
-/** attempt to write protected block zero */
-uint8_t const SD_CARD_ERROR_WRITE_BLOCK_ZERO = 0X14;  // REMOVE - not used
-/** card did not go ready for a multiple block write */
-uint8_t const SD_CARD_ERROR_WRITE_MULTIPLE = 0X15;
-/** card returned an error to a CMD13 status check after a write */
-uint8_t const SD_CARD_ERROR_WRITE_PROGRAMMING = 0X16;
-/** timeout occurred during write programming */
-uint8_t const SD_CARD_ERROR_WRITE_TIMEOUT = 0X17;
-/** incorrect rate selected */
-uint8_t const SD_CARD_ERROR_SCK_RATE = 0X18;
-/** init() not called */
-uint8_t const SD_CARD_ERROR_INIT_NOT_CALLED = 0X19;
-/** crc check error */
-uint8_t const SD_CARD_ERROR_CRC = 0X20;
-
-/** Toshiba FlashAir: iSDIO */
-uint8_t const SD_CARD_ERROR_CMD48 = 0x80;
-/** Toshiba FlashAir: iSDIO */
-uint8_t const SD_CARD_ERROR_CMD49 = 0x81;
-
-//------------------------------------------------------------------------------
-// card types
-/** Standard capacity V1 SD card */
-uint8_t const SD_CARD_TYPE_SD1  = 1;
-/** Standard capacity V2 SD card */
-uint8_t const SD_CARD_TYPE_SD2  = 2;
-/** High Capacity SD card */
-uint8_t const SD_CARD_TYPE_SDHC = 3;
-/**
- * define SOFTWARE_SPI to use bit-bang SPI
- */
-//------------------------------------------------------------------------------
-#if MEGA_SOFT_SPI && (defined(__AVR_ATmega1280__)||defined(__AVR_ATmega2560__))
-#define SOFTWARE_SPI
-#elif USE_SOFTWARE_SPI
-#define SOFTWARE_SPI
-#endif  // MEGA_SOFT_SPI
-//------------------------------------------------------------------------------
-// SPI pin definitions - do not edit here - change in SdFatConfig.h
-//
-#ifndef SOFTWARE_SPI
-// hardware pin defs
-/** The default chip select pin for the SD card is SS. */
-uint8_t const  SD_CHIP_SELECT_PIN = SS_PIN;
-// The following three pins must not be redefined for hardware SPI.
-/** SPI Master Out Slave In pin */
-uint8_t const  SPI_MOSI_PIN = MOSI_PIN;
-/** SPI Master In Slave Out pin */
-uint8_t const  SPI_MISO_PIN = MISO_PIN;
-/** SPI Clock pin */
-uint8_t const  SPI_SCK_PIN = SCK_PIN;
-
-#else  // SOFTWARE_SPI
-
-/** SPI chip select pin */
-uint8_t const SD_CHIP_SELECT_PIN = SOFT_SPI_CS_PIN;
-/** SPI Master Out Slave In pin */
-uint8_t const SPI_MOSI_PIN = SOFT_SPI_MOSI_PIN;
-/** SPI Master In Slave Out pin */
-uint8_t const SPI_MISO_PIN = SOFT_SPI_MISO_PIN;
-/** SPI Clock pin */
-uint8_t const SPI_SCK_PIN = SOFT_SPI_SCK_PIN;
-#endif  // SOFTWARE_SPI
-//------------------------------------------------------------------------------
-/**
- * \class Sd2Card
- * \brief Raw access to SD and SDHC flash memory cards.
- */
-class Sd2Card {
- public:
-  /** Construct an instance of Sd2Card. */
-  Sd2Card() : errorCode_(SD_CARD_ERROR_INIT_NOT_CALLED), type_(0), flash_air_compatible_(false) {}
-  uint32_t cardSize();
-  bool erase(uint32_t firstBlock, uint32_t lastBlock);
-  bool eraseSingleBlockEnable();
-  /**
-   *  Set SD error code.
-   *  \param[in] code value for error code.
-   */
-  void error(uint8_t code) {errorCode_ = code;}
-  /**
-   * \return error code for last error. See Sd2Card.h for a list of error codes.
-   */
-  int errorCode() const {return errorCode_;}
-  /** \return error data for last error. */
-  int errorData() const {return status_;}
-  /**
-   * Initialize an SD flash memory card with default clock rate and chip
-   * select pin.  See sd2Card::init(uint8_t sckRateID, uint8_t chipSelectPin).
-   *
-   * \return true for success or false for failure.
-   */
-  bool init(uint8_t sckRateID = SPI_FULL_SPEED,
-    uint8_t chipSelectPin = SD_CHIP_SELECT_PIN);
-  bool readBlock(uint32_t block, uint8_t* dst);
-  /**
-   * Read a card's CID register. The CID contains card identification
-   * information such as Manufacturer ID, Product name, Product serial
-   * number and Manufacturing date. 
-   *
-   * \param[out] cid pointer to area for returned data.
-   *
-   * \return true for success or false for failure.
-   */
-  bool readCID(cid_t* cid) {
-    return readRegister(CMD10, cid);
-  }
-  /**
-   * Read a card's CSD register. The CSD contains Card-Specific Data that
-   * provides information regarding access to the card's contents.
-   *
-   * \param[out] csd pointer to area for returned data.
-   *
-   * \return true for success or false for failure.
-   */
-  bool readCSD(csd_t* csd) {
-    return readRegister(CMD9, csd);
-  }
-  bool readData(uint8_t *dst);
-  bool readStart(uint32_t blockNumber);
-  bool readStop();
-  bool setSckRate(uint8_t sckRateID);
-  /** Return the card type: SD V1, SD V2 or SDHC
-   * \return 0 - SD V1, 1 - SD V2, or 3 - SDHC.
-   */
-  int type() const {return type_;}
-  bool writeBlock(uint32_t blockNumber, const uint8_t* src);
-  bool writeData(const uint8_t* src);
-  bool writeStart(uint32_t blockNumber, uint32_t eraseCount);
-  bool writeStop();
-
-  // Toshiba FlashAir support
-  uint8_t readExtMemory(uint8_t mio, uint8_t func, uint32_t addr, uint16_t count, uint8_t* dst);
-
-  void setFlashAirCompatible(bool flashAirCompatible) { flash_air_compatible_ = flashAirCompatible; }
-  bool getFlashAirCompatible() const { return flash_air_compatible_; }
-
- private:
-  //----------------------------------------------------------------------------
-  uint8_t chipSelectPin_;
-  uint8_t errorCode_;
-  uint8_t spiRate_;
-  uint8_t status_;
-  uint8_t type_;
-  bool    flash_air_compatible_;
-  // private functions
-  uint8_t cardAcmd(uint8_t cmd, uint32_t arg) {
-    cardCommand(CMD55, 0);
-    return cardCommand(cmd, arg);
-  }
-  uint8_t cardCommand(uint8_t cmd, uint32_t arg);
-
-  bool readData(uint8_t* dst, uint16_t count);
-  bool readRegister(uint8_t cmd, void* buf);
-  void chipSelectHigh();
-  void chipSelectLow();
-  void type(uint8_t value) {type_ = value;}
-  bool waitNotBusy(uint16_t timeoutMillis);
-  bool writeData(uint8_t token, const uint8_t* src);
-
-
-  // Toshiba FlashAir support
-  uint8_t waitStartBlock(void);
-  uint8_t readExt(uint32_t arg, uint8_t* dst, uint16_t count);
-};
-#endif  // Sd2Card_h
-
-
+/* Arduino Sd2Card Library
+ * Copyright (C) 2009 by William Greiman
+ *
+ * This file is part of the Arduino Sd2Card Library
+ *
+ * This Library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This Library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino Sd2Card Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+#include "Marlin.h"
+#ifdef SDSUPPORT
+
+#ifndef Sd2Card_h
+#define Sd2Card_h
+/**
+ * \file
+ * \brief Sd2Card class for V2 SD/SDHC cards
+ */
+#include "SdFatConfig.h"
+#include "Sd2PinMap.h"
+#include "SdInfo.h"
+//------------------------------------------------------------------------------
+// SPI speed is F_CPU/2^(1 + index), 0 <= index <= 6
+/** Set SCK to max rate of F_CPU/2. See Sd2Card::setSckRate(). */
+uint8_t const SPI_FULL_SPEED = 0;
+/** Set SCK rate to F_CPU/4. See Sd2Card::setSckRate(). */
+uint8_t const SPI_HALF_SPEED = 1;
+/** Set SCK rate to F_CPU/8. See Sd2Card::setSckRate(). */
+uint8_t const SPI_QUARTER_SPEED = 2;
+/** Set SCK rate to F_CPU/16. See Sd2Card::setSckRate(). */
+uint8_t const SPI_EIGHTH_SPEED = 3;
+/** Set SCK rate to F_CPU/32. See Sd2Card::setSckRate(). */
+uint8_t const SPI_SIXTEENTH_SPEED = 4;
+//------------------------------------------------------------------------------
+/** init timeout ms */
+uint16_t const SD_INIT_TIMEOUT = 2000;
+/** erase timeout ms */
+uint16_t const SD_ERASE_TIMEOUT = 10000;
+/** read timeout ms */
+uint16_t const SD_READ_TIMEOUT = 300;
+/** write time out ms */
+uint16_t const SD_WRITE_TIMEOUT = 600;
+//------------------------------------------------------------------------------
+// SD card errors
+/** timeout error for command CMD0 (initialize card in SPI mode) */
+uint8_t const SD_CARD_ERROR_CMD0 = 0X1;
+/** CMD8 was not accepted - not a valid SD card*/
+uint8_t const SD_CARD_ERROR_CMD8 = 0X2;
+/** card returned an error response for CMD12 (write stop) */
+uint8_t const SD_CARD_ERROR_CMD12 = 0X3;
+/** card returned an error response for CMD17 (read block) */
+uint8_t const SD_CARD_ERROR_CMD17 = 0X4;
+/** card returned an error response for CMD18 (read multiple block) */
+uint8_t const SD_CARD_ERROR_CMD18 = 0X5;
+/** card returned an error response for CMD24 (write block) */
+uint8_t const SD_CARD_ERROR_CMD24 = 0X6;
+/**  WRITE_MULTIPLE_BLOCKS command failed */
+uint8_t const SD_CARD_ERROR_CMD25 = 0X7;
+/** card returned an error response for CMD58 (read OCR) */
+uint8_t const SD_CARD_ERROR_CMD58 = 0X8;
+/** SET_WR_BLK_ERASE_COUNT failed */
+uint8_t const SD_CARD_ERROR_ACMD23 = 0X9;
+/** ACMD41 initialization process timeout */
+uint8_t const SD_CARD_ERROR_ACMD41 = 0XA;
+/** card returned a bad CSR version field */
+uint8_t const SD_CARD_ERROR_BAD_CSD = 0XB;
+/** erase block group command failed */
+uint8_t const SD_CARD_ERROR_ERASE = 0XC;
+/** card not capable of single block erase */
+uint8_t const SD_CARD_ERROR_ERASE_SINGLE_BLOCK = 0XD;
+/** Erase sequence timed out */
+uint8_t const SD_CARD_ERROR_ERASE_TIMEOUT = 0XE;
+/** card returned an error token instead of read data */
+uint8_t const SD_CARD_ERROR_READ = 0XF;
+/** read CID or CSD failed */
+uint8_t const SD_CARD_ERROR_READ_REG = 0X10;
+/** timeout while waiting for start of read data */
+uint8_t const SD_CARD_ERROR_READ_TIMEOUT = 0X11;
+/** card did not accept STOP_TRAN_TOKEN */
+uint8_t const SD_CARD_ERROR_STOP_TRAN = 0X12;
+/** card returned an error token as a response to a write operation */
+uint8_t const SD_CARD_ERROR_WRITE = 0X13;
+/** attempt to write protected block zero */
+uint8_t const SD_CARD_ERROR_WRITE_BLOCK_ZERO = 0X14;  // REMOVE - not used
+/** card did not go ready for a multiple block write */
+uint8_t const SD_CARD_ERROR_WRITE_MULTIPLE = 0X15;
+/** card returned an error to a CMD13 status check after a write */
+uint8_t const SD_CARD_ERROR_WRITE_PROGRAMMING = 0X16;
+/** timeout occurred during write programming */
+uint8_t const SD_CARD_ERROR_WRITE_TIMEOUT = 0X17;
+/** incorrect rate selected */
+uint8_t const SD_CARD_ERROR_SCK_RATE = 0X18;
+/** init() not called */
+uint8_t const SD_CARD_ERROR_INIT_NOT_CALLED = 0X19;
+/** crc check error */
+uint8_t const SD_CARD_ERROR_CRC = 0X20;
+
+/** Toshiba FlashAir: iSDIO */
+uint8_t const SD_CARD_ERROR_CMD48 = 0x80;
+/** Toshiba FlashAir: iSDIO */
+uint8_t const SD_CARD_ERROR_CMD49 = 0x81;
+
+//------------------------------------------------------------------------------
+// card types
+/** Standard capacity V1 SD card */
+uint8_t const SD_CARD_TYPE_SD1  = 1;
+/** Standard capacity V2 SD card */
+uint8_t const SD_CARD_TYPE_SD2  = 2;
+/** High Capacity SD card */
+uint8_t const SD_CARD_TYPE_SDHC = 3;
+/**
+ * define SOFTWARE_SPI to use bit-bang SPI
+ */
+//------------------------------------------------------------------------------
+#if MEGA_SOFT_SPI && (defined(__AVR_ATmega1280__)||defined(__AVR_ATmega2560__))
+#define SOFTWARE_SPI
+#elif USE_SOFTWARE_SPI
+#define SOFTWARE_SPI
+#endif  // MEGA_SOFT_SPI
+//------------------------------------------------------------------------------
+// SPI pin definitions - do not edit here - change in SdFatConfig.h
+//
+#ifndef SOFTWARE_SPI
+// hardware pin defs
+/** The default chip select pin for the SD card is SS. */
+uint8_t const  SD_CHIP_SELECT_PIN = SS_PIN;
+// The following three pins must not be redefined for hardware SPI.
+/** SPI Master Out Slave In pin */
+uint8_t const  SPI_MOSI_PIN = MOSI_PIN;
+/** SPI Master In Slave Out pin */
+uint8_t const  SPI_MISO_PIN = MISO_PIN;
+/** SPI Clock pin */
+uint8_t const  SPI_SCK_PIN = SCK_PIN;
+
+#else  // SOFTWARE_SPI
+
+/** SPI chip select pin */
+uint8_t const SD_CHIP_SELECT_PIN = SOFT_SPI_CS_PIN;
+/** SPI Master Out Slave In pin */
+uint8_t const SPI_MOSI_PIN = SOFT_SPI_MOSI_PIN;
+/** SPI Master In Slave Out pin */
+uint8_t const SPI_MISO_PIN = SOFT_SPI_MISO_PIN;
+/** SPI Clock pin */
+uint8_t const SPI_SCK_PIN = SOFT_SPI_SCK_PIN;
+#endif  // SOFTWARE_SPI
+//------------------------------------------------------------------------------
+/**
+ * \class Sd2Card
+ * \brief Raw access to SD and SDHC flash memory cards.
+ */
+class Sd2Card {
+ public:
+  /** Construct an instance of Sd2Card. */
+  Sd2Card() : errorCode_(SD_CARD_ERROR_INIT_NOT_CALLED), type_(0), flash_air_compatible_(false) {}
+  uint32_t cardSize();
+  bool erase(uint32_t firstBlock, uint32_t lastBlock);
+  bool eraseSingleBlockEnable();
+  /**
+   *  Set SD error code.
+   *  \param[in] code value for error code.
+   */
+  void error(uint8_t code) {errorCode_ = code;}
+  /**
+   * \return error code for last error. See Sd2Card.h for a list of error codes.
+   */
+  int errorCode() const {return errorCode_;}
+  /** \return error data for last error. */
+  int errorData() const {return status_;}
+  /**
+   * Initialize an SD flash memory card with default clock rate and chip
+   * select pin.  See sd2Card::init(uint8_t sckRateID, uint8_t chipSelectPin).
+   *
+   * \return true for success or false for failure.
+   */
+  bool init(uint8_t sckRateID = SPI_FULL_SPEED,
+    uint8_t chipSelectPin = SD_CHIP_SELECT_PIN);
+  bool readBlock(uint32_t block, uint8_t* dst);
+  /**
+   * Read a card's CID register. The CID contains card identification
+   * information such as Manufacturer ID, Product name, Product serial
+   * number and Manufacturing date. 
+   *
+   * \param[out] cid pointer to area for returned data.
+   *
+   * \return true for success or false for failure.
+   */
+  bool readCID(cid_t* cid) {
+    return readRegister(CMD10, cid);
+  }
+  /**
+   * Read a card's CSD register. The CSD contains Card-Specific Data that
+   * provides information regarding access to the card's contents.
+   *
+   * \param[out] csd pointer to area for returned data.
+   *
+   * \return true for success or false for failure.
+   */
+  bool readCSD(csd_t* csd) {
+    return readRegister(CMD9, csd);
+  }
+  bool readData(uint8_t *dst);
+  bool readStart(uint32_t blockNumber);
+  bool readStop();
+  bool setSckRate(uint8_t sckRateID);
+  /** Return the card type: SD V1, SD V2 or SDHC
+   * \return 0 - SD V1, 1 - SD V2, or 3 - SDHC.
+   */
+  int type() const {return type_;}
+  bool writeBlock(uint32_t blockNumber, const uint8_t* src);
+  bool writeData(const uint8_t* src);
+  bool writeStart(uint32_t blockNumber, uint32_t eraseCount);
+  bool writeStop();
+
+  // Toshiba FlashAir support
+  uint8_t readExtMemory(uint8_t mio, uint8_t func, uint32_t addr, uint16_t count, uint8_t* dst);
+
+  void setFlashAirCompatible(bool flashAirCompatible) { flash_air_compatible_ = flashAirCompatible; }
+  bool getFlashAirCompatible() const { return flash_air_compatible_; }
+
+ private:
+  //----------------------------------------------------------------------------
+  uint8_t chipSelectPin_;
+  uint8_t errorCode_;
+  uint8_t spiRate_;
+  uint8_t status_;
+  uint8_t type_;
+  bool    flash_air_compatible_;
+  // private functions
+  uint8_t cardAcmd(uint8_t cmd, uint32_t arg) {
+    cardCommand(CMD55, 0);
+    return cardCommand(cmd, arg);
+  }
+  uint8_t cardCommand(uint8_t cmd, uint32_t arg);
+
+  bool readData(uint8_t* dst, uint16_t count);
+  bool readRegister(uint8_t cmd, void* buf);
+  void chipSelectHigh();
+  void chipSelectLow();
+  void type(uint8_t value) {type_ = value;}
+  bool waitNotBusy(uint16_t timeoutMillis);
+  bool writeData(uint8_t token, const uint8_t* src);
+
+
+  // Toshiba FlashAir support
+  uint8_t waitStartBlock(void);
+  uint8_t readExt(uint32_t arg, uint8_t* dst, uint16_t count);
+};
+#endif  // Sd2Card_h
+
+
 #endif

+ 367 - 367
Firmware/Sd2PinMap.h

@@ -1,368 +1,368 @@
-/* Arduino SdFat Library
- * Copyright (C) 2010 by William Greiman
- *
- * This file is part of the Arduino SdFat Library
- *
- * This Library is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This Library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with the Arduino SdFat Library.  If not, see
- * <http://www.gnu.org/licenses/>.
- */
-// Warning this file was generated by a program.
-#include "Marlin.h"
-#ifdef SDSUPPORT
-
-#ifndef Sd2PinMap_h
-#define Sd2PinMap_h
-#include <avr/io.h>
-//------------------------------------------------------------------------------
-/** struct for mapping digital pins */
-struct pin_map_t {
-  volatile uint8_t* ddr;
-  volatile uint8_t* pin;
-  volatile uint8_t* port;
-  uint8_t bit;
-};
-//------------------------------------------------------------------------------
-#if defined(__AVR_ATmega1280__)\
-|| defined(__AVR_ATmega2560__)
-// Mega
-
-// Two Wire (aka I2C) ports
-uint8_t const SDA_PIN = 20;  // D1
-uint8_t const SCL_PIN = 21;  // D0
-
-#undef MOSI_PIN
-#undef MISO_PIN
-// SPI port
-uint8_t const SS_PIN = 53;    // B0
-uint8_t const MOSI_PIN = 51;  // B2
-uint8_t const MISO_PIN = 50;  // B3
-uint8_t const SCK_PIN = 52;   // B1
-
-static const pin_map_t digitalPinMap[] = {
-  {&DDRE, &PINE, &PORTE, 0},  // E0  0
-  {&DDRE, &PINE, &PORTE, 1},  // E1  1
-  {&DDRE, &PINE, &PORTE, 4},  // E4  2
-  {&DDRE, &PINE, &PORTE, 5},  // E5  3
-  {&DDRG, &PING, &PORTG, 5},  // G5  4
-  {&DDRE, &PINE, &PORTE, 3},  // E3  5
-  {&DDRH, &PINH, &PORTH, 3},  // H3  6
-  {&DDRH, &PINH, &PORTH, 4},  // H4  7
-  {&DDRH, &PINH, &PORTH, 5},  // H5  8
-  {&DDRH, &PINH, &PORTH, 6},  // H6  9
-  {&DDRB, &PINB, &PORTB, 4},  // B4 10
-  {&DDRB, &PINB, &PORTB, 5},  // B5 11
-  {&DDRB, &PINB, &PORTB, 6},  // B6 12
-  {&DDRB, &PINB, &PORTB, 7},  // B7 13
-  {&DDRJ, &PINJ, &PORTJ, 1},  // J1 14
-  {&DDRJ, &PINJ, &PORTJ, 0},  // J0 15
-  {&DDRH, &PINH, &PORTH, 1},  // H1 16
-  {&DDRH, &PINH, &PORTH, 0},  // H0 17
-  {&DDRD, &PIND, &PORTD, 3},  // D3 18
-  {&DDRD, &PIND, &PORTD, 2},  // D2 19
-  {&DDRD, &PIND, &PORTD, 1},  // D1 20
-  {&DDRD, &PIND, &PORTD, 0},  // D0 21
-  {&DDRA, &PINA, &PORTA, 0},  // A0 22
-  {&DDRA, &PINA, &PORTA, 1},  // A1 23
-  {&DDRA, &PINA, &PORTA, 2},  // A2 24
-  {&DDRA, &PINA, &PORTA, 3},  // A3 25
-  {&DDRA, &PINA, &PORTA, 4},  // A4 26
-  {&DDRA, &PINA, &PORTA, 5},  // A5 27
-  {&DDRA, &PINA, &PORTA, 6},  // A6 28
-  {&DDRA, &PINA, &PORTA, 7},  // A7 29
-  {&DDRC, &PINC, &PORTC, 7},  // C7 30
-  {&DDRC, &PINC, &PORTC, 6},  // C6 31
-  {&DDRC, &PINC, &PORTC, 5},  // C5 32
-  {&DDRC, &PINC, &PORTC, 4},  // C4 33
-  {&DDRC, &PINC, &PORTC, 3},  // C3 34
-  {&DDRC, &PINC, &PORTC, 2},  // C2 35
-  {&DDRC, &PINC, &PORTC, 1},  // C1 36
-  {&DDRC, &PINC, &PORTC, 0},  // C0 37
-  {&DDRD, &PIND, &PORTD, 7},  // D7 38
-  {&DDRG, &PING, &PORTG, 2},  // G2 39
-  {&DDRG, &PING, &PORTG, 1},  // G1 40
-  {&DDRG, &PING, &PORTG, 0},  // G0 41
-  {&DDRL, &PINL, &PORTL, 7},  // L7 42
-  {&DDRL, &PINL, &PORTL, 6},  // L6 43
-  {&DDRL, &PINL, &PORTL, 5},  // L5 44
-  {&DDRL, &PINL, &PORTL, 4},  // L4 45
-  {&DDRL, &PINL, &PORTL, 3},  // L3 46
-  {&DDRL, &PINL, &PORTL, 2},  // L2 47
-  {&DDRL, &PINL, &PORTL, 1},  // L1 48
-  {&DDRL, &PINL, &PORTL, 0},  // L0 49
-  {&DDRB, &PINB, &PORTB, 3},  // B3 50
-  {&DDRB, &PINB, &PORTB, 2},  // B2 51
-  {&DDRB, &PINB, &PORTB, 1},  // B1 52
-  {&DDRB, &PINB, &PORTB, 0},  // B0 53
-  {&DDRF, &PINF, &PORTF, 0},  // F0 54
-  {&DDRF, &PINF, &PORTF, 1},  // F1 55
-  {&DDRF, &PINF, &PORTF, 2},  // F2 56
-  {&DDRF, &PINF, &PORTF, 3},  // F3 57
-  {&DDRF, &PINF, &PORTF, 4},  // F4 58
-  {&DDRF, &PINF, &PORTF, 5},  // F5 59
-  {&DDRF, &PINF, &PORTF, 6},  // F6 60
-  {&DDRF, &PINF, &PORTF, 7},  // F7 61
-  {&DDRK, &PINK, &PORTK, 0},  // K0 62
-  {&DDRK, &PINK, &PORTK, 1},  // K1 63
-  {&DDRK, &PINK, &PORTK, 2},  // K2 64
-  {&DDRK, &PINK, &PORTK, 3},  // K3 65
-  {&DDRK, &PINK, &PORTK, 4},  // K4 66
-  {&DDRK, &PINK, &PORTK, 5},  // K5 67
-  {&DDRK, &PINK, &PORTK, 6},  // K6 68
-  {&DDRK, &PINK, &PORTK, 7}   // K7 69
-};
-//------------------------------------------------------------------------------
-#elif defined(__AVR_ATmega644P__)\
-|| defined(__AVR_ATmega644__)\
-|| defined(__AVR_ATmega1284P__)
-// Sanguino
-
-// Two Wire (aka I2C) ports
-uint8_t const SDA_PIN = 17;  // C1
-uint8_t const SCL_PIN = 18;  // C2
-
-// SPI port
-uint8_t const SS_PIN = 4;    // B4
-uint8_t const MOSI_PIN = 5;  // B5
-uint8_t const MISO_PIN = 6;  // B6
-uint8_t const SCK_PIN = 7;   // B7
-
-static const pin_map_t digitalPinMap[] = {
-  {&DDRB, &PINB, &PORTB, 0},  // B0  0
-  {&DDRB, &PINB, &PORTB, 1},  // B1  1
-  {&DDRB, &PINB, &PORTB, 2},  // B2  2
-  {&DDRB, &PINB, &PORTB, 3},  // B3  3
-  {&DDRB, &PINB, &PORTB, 4},  // B4  4
-  {&DDRB, &PINB, &PORTB, 5},  // B5  5
-  {&DDRB, &PINB, &PORTB, 6},  // B6  6
-  {&DDRB, &PINB, &PORTB, 7},  // B7  7
-  {&DDRD, &PIND, &PORTD, 0},  // D0  8
-  {&DDRD, &PIND, &PORTD, 1},  // D1  9
-  {&DDRD, &PIND, &PORTD, 2},  // D2 10
-  {&DDRD, &PIND, &PORTD, 3},  // D3 11
-  {&DDRD, &PIND, &PORTD, 4},  // D4 12
-  {&DDRD, &PIND, &PORTD, 5},  // D5 13
-  {&DDRD, &PIND, &PORTD, 6},  // D6 14
-  {&DDRD, &PIND, &PORTD, 7},  // D7 15
-  {&DDRC, &PINC, &PORTC, 0},  // C0 16
-  {&DDRC, &PINC, &PORTC, 1},  // C1 17
-  {&DDRC, &PINC, &PORTC, 2},  // C2 18
-  {&DDRC, &PINC, &PORTC, 3},  // C3 19
-  {&DDRC, &PINC, &PORTC, 4},  // C4 20
-  {&DDRC, &PINC, &PORTC, 5},  // C5 21
-  {&DDRC, &PINC, &PORTC, 6},  // C6 22
-  {&DDRC, &PINC, &PORTC, 7},  // C7 23
-  {&DDRA, &PINA, &PORTA, 7},  // A7 24
-  {&DDRA, &PINA, &PORTA, 6},  // A6 25
-  {&DDRA, &PINA, &PORTA, 5},  // A5 26
-  {&DDRA, &PINA, &PORTA, 4},  // A4 27
-  {&DDRA, &PINA, &PORTA, 3},  // A3 28
-  {&DDRA, &PINA, &PORTA, 2},  // A2 29
-  {&DDRA, &PINA, &PORTA, 1},  // A1 30
-  {&DDRA, &PINA, &PORTA, 0}   // A0 31
-};
-//------------------------------------------------------------------------------
-#elif defined(__AVR_ATmega32U4__)
-// Teensy 2.0
-
-// Two Wire (aka I2C) ports
-uint8_t const SDA_PIN = 6;  // D1
-uint8_t const SCL_PIN = 5;  // D0
-
-// SPI port
-uint8_t const SS_PIN = 0;    // B0
-uint8_t const MOSI_PIN = 2;  // B2
-uint8_t const MISO_PIN = 3;  // B3
-uint8_t const SCK_PIN = 1;   // B1
-
-static const pin_map_t digitalPinMap[] = {
-  {&DDRB, &PINB, &PORTB, 0},  // B0  0
-  {&DDRB, &PINB, &PORTB, 1},  // B1  1
-  {&DDRB, &PINB, &PORTB, 2},  // B2  2
-  {&DDRB, &PINB, &PORTB, 3},  // B3  3
-  {&DDRB, &PINB, &PORTB, 7},  // B7  4
-  {&DDRD, &PIND, &PORTD, 0},  // D0  5
-  {&DDRD, &PIND, &PORTD, 1},  // D1  6
-  {&DDRD, &PIND, &PORTD, 2},  // D2  7
-  {&DDRD, &PIND, &PORTD, 3},  // D3  8
-  {&DDRC, &PINC, &PORTC, 6},  // C6  9
-  {&DDRC, &PINC, &PORTC, 7},  // C7 10
-  {&DDRD, &PIND, &PORTD, 6},  // D6 11
-  {&DDRD, &PIND, &PORTD, 7},  // D7 12
-  {&DDRB, &PINB, &PORTB, 4},  // B4 13
-  {&DDRB, &PINB, &PORTB, 5},  // B5 14
-  {&DDRB, &PINB, &PORTB, 6},  // B6 15
-  {&DDRF, &PINF, &PORTF, 7},  // F7 16
-  {&DDRF, &PINF, &PORTF, 6},  // F6 17
-  {&DDRF, &PINF, &PORTF, 5},  // F5 18
-  {&DDRF, &PINF, &PORTF, 4},  // F4 19
-  {&DDRF, &PINF, &PORTF, 1},  // F1 20
-  {&DDRF, &PINF, &PORTF, 0},  // F0 21
-  {&DDRD, &PIND, &PORTD, 4},  // D4 22
-  {&DDRD, &PIND, &PORTD, 5},  // D5 23
-  {&DDRE, &PINE, &PORTE, 6}   // E6 24
-};
-//------------------------------------------------------------------------------
-#elif defined(__AVR_AT90USB646__)\
-|| defined(__AVR_AT90USB1286__)
-// Teensy++ 1.0 & 2.0
-
-// Two Wire (aka I2C) ports
-uint8_t const SDA_PIN = 1;  // D1
-uint8_t const SCL_PIN = 0;  // D0
-
-// SPI port
-uint8_t const SS_PIN    = 20;    // B0
-uint8_t const MOSI_PIN  = 22;    // B2
-uint8_t const MISO_PIN  = 23;    // B3
-uint8_t const SCK_PIN   = 21;    // B1
-
-static const pin_map_t digitalPinMap[] = {
-  {&DDRD, &PIND, &PORTD, 0},  // D0  0
-  {&DDRD, &PIND, &PORTD, 1},  // D1  1
-  {&DDRD, &PIND, &PORTD, 2},  // D2  2
-  {&DDRD, &PIND, &PORTD, 3},  // D3  3
-  {&DDRD, &PIND, &PORTD, 4},  // D4  4
-  {&DDRD, &PIND, &PORTD, 5},  // D5  5
-  {&DDRD, &PIND, &PORTD, 6},  // D6  6
-  {&DDRD, &PIND, &PORTD, 7},  // D7  7
-  {&DDRE, &PINE, &PORTE, 0},  // E0  8
-  {&DDRE, &PINE, &PORTE, 1},  // E1  9
-  {&DDRC, &PINC, &PORTC, 0},  // C0 10
-  {&DDRC, &PINC, &PORTC, 1},  // C1 11
-  {&DDRC, &PINC, &PORTC, 2},  // C2 12
-  {&DDRC, &PINC, &PORTC, 3},  // C3 13
-  {&DDRC, &PINC, &PORTC, 4},  // C4 14
-  {&DDRC, &PINC, &PORTC, 5},  // C5 15
-  {&DDRC, &PINC, &PORTC, 6},  // C6 16
-  {&DDRC, &PINC, &PORTC, 7},  // C7 17
-  {&DDRE, &PINE, &PORTE, 6},  // E6 18
-  {&DDRE, &PINE, &PORTE, 7},  // E7 19
-  {&DDRB, &PINB, &PORTB, 0},  // B0 20
-  {&DDRB, &PINB, &PORTB, 1},  // B1 21
-  {&DDRB, &PINB, &PORTB, 2},  // B2 22
-  {&DDRB, &PINB, &PORTB, 3},  // B3 23
-  {&DDRB, &PINB, &PORTB, 4},  // B4 24
-  {&DDRB, &PINB, &PORTB, 5},  // B5 25
-  {&DDRB, &PINB, &PORTB, 6},  // B6 26
-  {&DDRB, &PINB, &PORTB, 7},  // B7 27
-  {&DDRA, &PINA, &PORTA, 0},  // A0 28
-  {&DDRA, &PINA, &PORTA, 1},  // A1 29
-  {&DDRA, &PINA, &PORTA, 2},  // A2 30
-  {&DDRA, &PINA, &PORTA, 3},  // A3 31
-  {&DDRA, &PINA, &PORTA, 4},  // A4 32
-  {&DDRA, &PINA, &PORTA, 5},  // A5 33
-  {&DDRA, &PINA, &PORTA, 6},  // A6 34
-  {&DDRA, &PINA, &PORTA, 7},  // A7 35
-  {&DDRE, &PINE, &PORTE, 4},  // E4 36
-  {&DDRE, &PINE, &PORTE, 5},  // E5 37
-  {&DDRF, &PINF, &PORTF, 0},  // F0 38
-  {&DDRF, &PINF, &PORTF, 1},  // F1 39
-  {&DDRF, &PINF, &PORTF, 2},  // F2 40
-  {&DDRF, &PINF, &PORTF, 3},  // F3 41
-  {&DDRF, &PINF, &PORTF, 4},  // F4 42
-  {&DDRF, &PINF, &PORTF, 5},  // F5 43
-  {&DDRF, &PINF, &PORTF, 6},  // F6 44
-  {&DDRF, &PINF, &PORTF, 7}   // F7 45
-};
-//------------------------------------------------------------------------------
-#elif defined(__AVR_ATmega168__)\
-||defined(__AVR_ATmega168P__)\
-||defined(__AVR_ATmega328P__)
-// 168 and 328 Arduinos
-
-// Two Wire (aka I2C) ports
-uint8_t const SDA_PIN = 18;  // C4
-uint8_t const SCL_PIN = 19;  // C5
-
-// SPI port
-uint8_t const SS_PIN = 10;    // B2
-uint8_t const MOSI_PIN = 11;  // B3
-uint8_t const MISO_PIN = 12;  // B4
-uint8_t const SCK_PIN = 13;   // B5
-
-static const pin_map_t digitalPinMap[] = {
-  {&DDRD, &PIND, &PORTD, 0},  // D0  0
-  {&DDRD, &PIND, &PORTD, 1},  // D1  1
-  {&DDRD, &PIND, &PORTD, 2},  // D2  2
-  {&DDRD, &PIND, &PORTD, 3},  // D3  3
-  {&DDRD, &PIND, &PORTD, 4},  // D4  4
-  {&DDRD, &PIND, &PORTD, 5},  // D5  5
-  {&DDRD, &PIND, &PORTD, 6},  // D6  6
-  {&DDRD, &PIND, &PORTD, 7},  // D7  7
-  {&DDRB, &PINB, &PORTB, 0},  // B0  8
-  {&DDRB, &PINB, &PORTB, 1},  // B1  9
-  {&DDRB, &PINB, &PORTB, 2},  // B2 10
-  {&DDRB, &PINB, &PORTB, 3},  // B3 11
-  {&DDRB, &PINB, &PORTB, 4},  // B4 12
-  {&DDRB, &PINB, &PORTB, 5},  // B5 13
-  {&DDRC, &PINC, &PORTC, 0},  // C0 14
-  {&DDRC, &PINC, &PORTC, 1},  // C1 15
-  {&DDRC, &PINC, &PORTC, 2},  // C2 16
-  {&DDRC, &PINC, &PORTC, 3},  // C3 17
-  {&DDRC, &PINC, &PORTC, 4},  // C4 18
-  {&DDRC, &PINC, &PORTC, 5}   // C5 19
-};
-#else  // defined(__AVR_ATmega1280__)
-#error unknown chip
-#endif  // defined(__AVR_ATmega1280__)
-//------------------------------------------------------------------------------
-static const uint8_t digitalPinCount = sizeof(digitalPinMap)/sizeof(pin_map_t);
-
-uint8_t badPinNumber(void)
-  __attribute__((error("Pin number is too large or not a constant")));
-
-static inline __attribute__((always_inline))
-  bool getPinMode(uint8_t pin) {
-  if (__builtin_constant_p(pin) && pin < digitalPinCount) {
-    return (*digitalPinMap[pin].ddr >> digitalPinMap[pin].bit) & 1;
-  } else {
-    return badPinNumber();
-  }
-}
-static inline __attribute__((always_inline))
-  void setPinMode(uint8_t pin, uint8_t mode) {
-  if (__builtin_constant_p(pin) && pin < digitalPinCount) {
-    if (mode) {
-      *digitalPinMap[pin].ddr |= 1 << digitalPinMap[pin].bit;
-    } else {
-      *digitalPinMap[pin].ddr &= ~(1 << digitalPinMap[pin].bit);
-    }
-  } else {
-    badPinNumber();
-  }
-}
-static inline __attribute__((always_inline))
-  bool fastDigitalRead(uint8_t pin) {
-  if (__builtin_constant_p(pin) && pin < digitalPinCount) {
-    return (*digitalPinMap[pin].pin >> digitalPinMap[pin].bit) & 1;
-  } else {
-    return badPinNumber();
-  }
-}
-static inline __attribute__((always_inline))
-  void fastDigitalWrite(uint8_t pin, uint8_t value) {
-  if (__builtin_constant_p(pin) && pin < digitalPinCount) {
-    if (value) {
-      *digitalPinMap[pin].port |= 1 << digitalPinMap[pin].bit;
-    } else {
-      *digitalPinMap[pin].port &= ~(1 << digitalPinMap[pin].bit);
-    }
-  } else {
-    badPinNumber();
-  }
-}
-#endif  // Sd2PinMap_h
-
-
+/* Arduino SdFat Library
+ * Copyright (C) 2010 by William Greiman
+ *
+ * This file is part of the Arduino SdFat Library
+ *
+ * This Library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This Library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino SdFat Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+// Warning this file was generated by a program.
+#include "Marlin.h"
+#ifdef SDSUPPORT
+
+#ifndef Sd2PinMap_h
+#define Sd2PinMap_h
+#include <avr/io.h>
+//------------------------------------------------------------------------------
+/** struct for mapping digital pins */
+struct pin_map_t {
+  volatile uint8_t* ddr;
+  volatile uint8_t* pin;
+  volatile uint8_t* port;
+  uint8_t bit;
+};
+//------------------------------------------------------------------------------
+#if defined(__AVR_ATmega1280__)\
+|| defined(__AVR_ATmega2560__)
+// Mega
+
+// Two Wire (aka I2C) ports
+uint8_t const SDA_PIN = 20;  // D1
+uint8_t const SCL_PIN = 21;  // D0
+
+#undef MOSI_PIN
+#undef MISO_PIN
+// SPI port
+uint8_t const SS_PIN = 53;    // B0
+uint8_t const MOSI_PIN = 51;  // B2
+uint8_t const MISO_PIN = 50;  // B3
+uint8_t const SCK_PIN = 52;   // B1
+
+static const pin_map_t digitalPinMap[] = {
+  {&DDRE, &PINE, &PORTE, 0},  // E0  0
+  {&DDRE, &PINE, &PORTE, 1},  // E1  1
+  {&DDRE, &PINE, &PORTE, 4},  // E4  2
+  {&DDRE, &PINE, &PORTE, 5},  // E5  3
+  {&DDRG, &PING, &PORTG, 5},  // G5  4
+  {&DDRE, &PINE, &PORTE, 3},  // E3  5
+  {&DDRH, &PINH, &PORTH, 3},  // H3  6
+  {&DDRH, &PINH, &PORTH, 4},  // H4  7
+  {&DDRH, &PINH, &PORTH, 5},  // H5  8
+  {&DDRH, &PINH, &PORTH, 6},  // H6  9
+  {&DDRB, &PINB, &PORTB, 4},  // B4 10
+  {&DDRB, &PINB, &PORTB, 5},  // B5 11
+  {&DDRB, &PINB, &PORTB, 6},  // B6 12
+  {&DDRB, &PINB, &PORTB, 7},  // B7 13
+  {&DDRJ, &PINJ, &PORTJ, 1},  // J1 14
+  {&DDRJ, &PINJ, &PORTJ, 0},  // J0 15
+  {&DDRH, &PINH, &PORTH, 1},  // H1 16
+  {&DDRH, &PINH, &PORTH, 0},  // H0 17
+  {&DDRD, &PIND, &PORTD, 3},  // D3 18
+  {&DDRD, &PIND, &PORTD, 2},  // D2 19
+  {&DDRD, &PIND, &PORTD, 1},  // D1 20
+  {&DDRD, &PIND, &PORTD, 0},  // D0 21
+  {&DDRA, &PINA, &PORTA, 0},  // A0 22
+  {&DDRA, &PINA, &PORTA, 1},  // A1 23
+  {&DDRA, &PINA, &PORTA, 2},  // A2 24
+  {&DDRA, &PINA, &PORTA, 3},  // A3 25
+  {&DDRA, &PINA, &PORTA, 4},  // A4 26
+  {&DDRA, &PINA, &PORTA, 5},  // A5 27
+  {&DDRA, &PINA, &PORTA, 6},  // A6 28
+  {&DDRA, &PINA, &PORTA, 7},  // A7 29
+  {&DDRC, &PINC, &PORTC, 7},  // C7 30
+  {&DDRC, &PINC, &PORTC, 6},  // C6 31
+  {&DDRC, &PINC, &PORTC, 5},  // C5 32
+  {&DDRC, &PINC, &PORTC, 4},  // C4 33
+  {&DDRC, &PINC, &PORTC, 3},  // C3 34
+  {&DDRC, &PINC, &PORTC, 2},  // C2 35
+  {&DDRC, &PINC, &PORTC, 1},  // C1 36
+  {&DDRC, &PINC, &PORTC, 0},  // C0 37
+  {&DDRD, &PIND, &PORTD, 7},  // D7 38
+  {&DDRG, &PING, &PORTG, 2},  // G2 39
+  {&DDRG, &PING, &PORTG, 1},  // G1 40
+  {&DDRG, &PING, &PORTG, 0},  // G0 41
+  {&DDRL, &PINL, &PORTL, 7},  // L7 42
+  {&DDRL, &PINL, &PORTL, 6},  // L6 43
+  {&DDRL, &PINL, &PORTL, 5},  // L5 44
+  {&DDRL, &PINL, &PORTL, 4},  // L4 45
+  {&DDRL, &PINL, &PORTL, 3},  // L3 46
+  {&DDRL, &PINL, &PORTL, 2},  // L2 47
+  {&DDRL, &PINL, &PORTL, 1},  // L1 48
+  {&DDRL, &PINL, &PORTL, 0},  // L0 49
+  {&DDRB, &PINB, &PORTB, 3},  // B3 50
+  {&DDRB, &PINB, &PORTB, 2},  // B2 51
+  {&DDRB, &PINB, &PORTB, 1},  // B1 52
+  {&DDRB, &PINB, &PORTB, 0},  // B0 53
+  {&DDRF, &PINF, &PORTF, 0},  // F0 54
+  {&DDRF, &PINF, &PORTF, 1},  // F1 55
+  {&DDRF, &PINF, &PORTF, 2},  // F2 56
+  {&DDRF, &PINF, &PORTF, 3},  // F3 57
+  {&DDRF, &PINF, &PORTF, 4},  // F4 58
+  {&DDRF, &PINF, &PORTF, 5},  // F5 59
+  {&DDRF, &PINF, &PORTF, 6},  // F6 60
+  {&DDRF, &PINF, &PORTF, 7},  // F7 61
+  {&DDRK, &PINK, &PORTK, 0},  // K0 62
+  {&DDRK, &PINK, &PORTK, 1},  // K1 63
+  {&DDRK, &PINK, &PORTK, 2},  // K2 64
+  {&DDRK, &PINK, &PORTK, 3},  // K3 65
+  {&DDRK, &PINK, &PORTK, 4},  // K4 66
+  {&DDRK, &PINK, &PORTK, 5},  // K5 67
+  {&DDRK, &PINK, &PORTK, 6},  // K6 68
+  {&DDRK, &PINK, &PORTK, 7}   // K7 69
+};
+//------------------------------------------------------------------------------
+#elif defined(__AVR_ATmega644P__)\
+|| defined(__AVR_ATmega644__)\
+|| defined(__AVR_ATmega1284P__)
+// Sanguino
+
+// Two Wire (aka I2C) ports
+uint8_t const SDA_PIN = 17;  // C1
+uint8_t const SCL_PIN = 18;  // C2
+
+// SPI port
+uint8_t const SS_PIN = 4;    // B4
+uint8_t const MOSI_PIN = 5;  // B5
+uint8_t const MISO_PIN = 6;  // B6
+uint8_t const SCK_PIN = 7;   // B7
+
+static const pin_map_t digitalPinMap[] = {
+  {&DDRB, &PINB, &PORTB, 0},  // B0  0
+  {&DDRB, &PINB, &PORTB, 1},  // B1  1
+  {&DDRB, &PINB, &PORTB, 2},  // B2  2
+  {&DDRB, &PINB, &PORTB, 3},  // B3  3
+  {&DDRB, &PINB, &PORTB, 4},  // B4  4
+  {&DDRB, &PINB, &PORTB, 5},  // B5  5
+  {&DDRB, &PINB, &PORTB, 6},  // B6  6
+  {&DDRB, &PINB, &PORTB, 7},  // B7  7
+  {&DDRD, &PIND, &PORTD, 0},  // D0  8
+  {&DDRD, &PIND, &PORTD, 1},  // D1  9
+  {&DDRD, &PIND, &PORTD, 2},  // D2 10
+  {&DDRD, &PIND, &PORTD, 3},  // D3 11
+  {&DDRD, &PIND, &PORTD, 4},  // D4 12
+  {&DDRD, &PIND, &PORTD, 5},  // D5 13
+  {&DDRD, &PIND, &PORTD, 6},  // D6 14
+  {&DDRD, &PIND, &PORTD, 7},  // D7 15
+  {&DDRC, &PINC, &PORTC, 0},  // C0 16
+  {&DDRC, &PINC, &PORTC, 1},  // C1 17
+  {&DDRC, &PINC, &PORTC, 2},  // C2 18
+  {&DDRC, &PINC, &PORTC, 3},  // C3 19
+  {&DDRC, &PINC, &PORTC, 4},  // C4 20
+  {&DDRC, &PINC, &PORTC, 5},  // C5 21
+  {&DDRC, &PINC, &PORTC, 6},  // C6 22
+  {&DDRC, &PINC, &PORTC, 7},  // C7 23
+  {&DDRA, &PINA, &PORTA, 7},  // A7 24
+  {&DDRA, &PINA, &PORTA, 6},  // A6 25
+  {&DDRA, &PINA, &PORTA, 5},  // A5 26
+  {&DDRA, &PINA, &PORTA, 4},  // A4 27
+  {&DDRA, &PINA, &PORTA, 3},  // A3 28
+  {&DDRA, &PINA, &PORTA, 2},  // A2 29
+  {&DDRA, &PINA, &PORTA, 1},  // A1 30
+  {&DDRA, &PINA, &PORTA, 0}   // A0 31
+};
+//------------------------------------------------------------------------------
+#elif defined(__AVR_ATmega32U4__)
+// Teensy 2.0
+
+// Two Wire (aka I2C) ports
+uint8_t const SDA_PIN = 6;  // D1
+uint8_t const SCL_PIN = 5;  // D0
+
+// SPI port
+uint8_t const SS_PIN = 0;    // B0
+uint8_t const MOSI_PIN = 2;  // B2
+uint8_t const MISO_PIN = 3;  // B3
+uint8_t const SCK_PIN = 1;   // B1
+
+static const pin_map_t digitalPinMap[] = {
+  {&DDRB, &PINB, &PORTB, 0},  // B0  0
+  {&DDRB, &PINB, &PORTB, 1},  // B1  1
+  {&DDRB, &PINB, &PORTB, 2},  // B2  2
+  {&DDRB, &PINB, &PORTB, 3},  // B3  3
+  {&DDRB, &PINB, &PORTB, 7},  // B7  4
+  {&DDRD, &PIND, &PORTD, 0},  // D0  5
+  {&DDRD, &PIND, &PORTD, 1},  // D1  6
+  {&DDRD, &PIND, &PORTD, 2},  // D2  7
+  {&DDRD, &PIND, &PORTD, 3},  // D3  8
+  {&DDRC, &PINC, &PORTC, 6},  // C6  9
+  {&DDRC, &PINC, &PORTC, 7},  // C7 10
+  {&DDRD, &PIND, &PORTD, 6},  // D6 11
+  {&DDRD, &PIND, &PORTD, 7},  // D7 12
+  {&DDRB, &PINB, &PORTB, 4},  // B4 13
+  {&DDRB, &PINB, &PORTB, 5},  // B5 14
+  {&DDRB, &PINB, &PORTB, 6},  // B6 15
+  {&DDRF, &PINF, &PORTF, 7},  // F7 16
+  {&DDRF, &PINF, &PORTF, 6},  // F6 17
+  {&DDRF, &PINF, &PORTF, 5},  // F5 18
+  {&DDRF, &PINF, &PORTF, 4},  // F4 19
+  {&DDRF, &PINF, &PORTF, 1},  // F1 20
+  {&DDRF, &PINF, &PORTF, 0},  // F0 21
+  {&DDRD, &PIND, &PORTD, 4},  // D4 22
+  {&DDRD, &PIND, &PORTD, 5},  // D5 23
+  {&DDRE, &PINE, &PORTE, 6}   // E6 24
+};
+//------------------------------------------------------------------------------
+#elif defined(__AVR_AT90USB646__)\
+|| defined(__AVR_AT90USB1286__)
+// Teensy++ 1.0 & 2.0
+
+// Two Wire (aka I2C) ports
+uint8_t const SDA_PIN = 1;  // D1
+uint8_t const SCL_PIN = 0;  // D0
+
+// SPI port
+uint8_t const SS_PIN    = 20;    // B0
+uint8_t const MOSI_PIN  = 22;    // B2
+uint8_t const MISO_PIN  = 23;    // B3
+uint8_t const SCK_PIN   = 21;    // B1
+
+static const pin_map_t digitalPinMap[] = {
+  {&DDRD, &PIND, &PORTD, 0},  // D0  0
+  {&DDRD, &PIND, &PORTD, 1},  // D1  1
+  {&DDRD, &PIND, &PORTD, 2},  // D2  2
+  {&DDRD, &PIND, &PORTD, 3},  // D3  3
+  {&DDRD, &PIND, &PORTD, 4},  // D4  4
+  {&DDRD, &PIND, &PORTD, 5},  // D5  5
+  {&DDRD, &PIND, &PORTD, 6},  // D6  6
+  {&DDRD, &PIND, &PORTD, 7},  // D7  7
+  {&DDRE, &PINE, &PORTE, 0},  // E0  8
+  {&DDRE, &PINE, &PORTE, 1},  // E1  9
+  {&DDRC, &PINC, &PORTC, 0},  // C0 10
+  {&DDRC, &PINC, &PORTC, 1},  // C1 11
+  {&DDRC, &PINC, &PORTC, 2},  // C2 12
+  {&DDRC, &PINC, &PORTC, 3},  // C3 13
+  {&DDRC, &PINC, &PORTC, 4},  // C4 14
+  {&DDRC, &PINC, &PORTC, 5},  // C5 15
+  {&DDRC, &PINC, &PORTC, 6},  // C6 16
+  {&DDRC, &PINC, &PORTC, 7},  // C7 17
+  {&DDRE, &PINE, &PORTE, 6},  // E6 18
+  {&DDRE, &PINE, &PORTE, 7},  // E7 19
+  {&DDRB, &PINB, &PORTB, 0},  // B0 20
+  {&DDRB, &PINB, &PORTB, 1},  // B1 21
+  {&DDRB, &PINB, &PORTB, 2},  // B2 22
+  {&DDRB, &PINB, &PORTB, 3},  // B3 23
+  {&DDRB, &PINB, &PORTB, 4},  // B4 24
+  {&DDRB, &PINB, &PORTB, 5},  // B5 25
+  {&DDRB, &PINB, &PORTB, 6},  // B6 26
+  {&DDRB, &PINB, &PORTB, 7},  // B7 27
+  {&DDRA, &PINA, &PORTA, 0},  // A0 28
+  {&DDRA, &PINA, &PORTA, 1},  // A1 29
+  {&DDRA, &PINA, &PORTA, 2},  // A2 30
+  {&DDRA, &PINA, &PORTA, 3},  // A3 31
+  {&DDRA, &PINA, &PORTA, 4},  // A4 32
+  {&DDRA, &PINA, &PORTA, 5},  // A5 33
+  {&DDRA, &PINA, &PORTA, 6},  // A6 34
+  {&DDRA, &PINA, &PORTA, 7},  // A7 35
+  {&DDRE, &PINE, &PORTE, 4},  // E4 36
+  {&DDRE, &PINE, &PORTE, 5},  // E5 37
+  {&DDRF, &PINF, &PORTF, 0},  // F0 38
+  {&DDRF, &PINF, &PORTF, 1},  // F1 39
+  {&DDRF, &PINF, &PORTF, 2},  // F2 40
+  {&DDRF, &PINF, &PORTF, 3},  // F3 41
+  {&DDRF, &PINF, &PORTF, 4},  // F4 42
+  {&DDRF, &PINF, &PORTF, 5},  // F5 43
+  {&DDRF, &PINF, &PORTF, 6},  // F6 44
+  {&DDRF, &PINF, &PORTF, 7}   // F7 45
+};
+//------------------------------------------------------------------------------
+#elif defined(__AVR_ATmega168__)\
+||defined(__AVR_ATmega168P__)\
+||defined(__AVR_ATmega328P__)
+// 168 and 328 Arduinos
+
+// Two Wire (aka I2C) ports
+uint8_t const SDA_PIN = 18;  // C4
+uint8_t const SCL_PIN = 19;  // C5
+
+// SPI port
+uint8_t const SS_PIN = 10;    // B2
+uint8_t const MOSI_PIN = 11;  // B3
+uint8_t const MISO_PIN = 12;  // B4
+uint8_t const SCK_PIN = 13;   // B5
+
+static const pin_map_t digitalPinMap[] = {
+  {&DDRD, &PIND, &PORTD, 0},  // D0  0
+  {&DDRD, &PIND, &PORTD, 1},  // D1  1
+  {&DDRD, &PIND, &PORTD, 2},  // D2  2
+  {&DDRD, &PIND, &PORTD, 3},  // D3  3
+  {&DDRD, &PIND, &PORTD, 4},  // D4  4
+  {&DDRD, &PIND, &PORTD, 5},  // D5  5
+  {&DDRD, &PIND, &PORTD, 6},  // D6  6
+  {&DDRD, &PIND, &PORTD, 7},  // D7  7
+  {&DDRB, &PINB, &PORTB, 0},  // B0  8
+  {&DDRB, &PINB, &PORTB, 1},  // B1  9
+  {&DDRB, &PINB, &PORTB, 2},  // B2 10
+  {&DDRB, &PINB, &PORTB, 3},  // B3 11
+  {&DDRB, &PINB, &PORTB, 4},  // B4 12
+  {&DDRB, &PINB, &PORTB, 5},  // B5 13
+  {&DDRC, &PINC, &PORTC, 0},  // C0 14
+  {&DDRC, &PINC, &PORTC, 1},  // C1 15
+  {&DDRC, &PINC, &PORTC, 2},  // C2 16
+  {&DDRC, &PINC, &PORTC, 3},  // C3 17
+  {&DDRC, &PINC, &PORTC, 4},  // C4 18
+  {&DDRC, &PINC, &PORTC, 5}   // C5 19
+};
+#else  // defined(__AVR_ATmega1280__)
+#error unknown chip
+#endif  // defined(__AVR_ATmega1280__)
+//------------------------------------------------------------------------------
+static const uint8_t digitalPinCount = sizeof(digitalPinMap)/sizeof(pin_map_t);
+
+uint8_t badPinNumber(void)
+  __attribute__((error("Pin number is too large or not a constant")));
+
+static inline __attribute__((always_inline))
+  bool getPinMode(uint8_t pin) {
+  if (__builtin_constant_p(pin) && pin < digitalPinCount) {
+    return (*digitalPinMap[pin].ddr >> digitalPinMap[pin].bit) & 1;
+  } else {
+    return badPinNumber();
+  }
+}
+static inline __attribute__((always_inline))
+  void setPinMode(uint8_t pin, uint8_t mode) {
+  if (__builtin_constant_p(pin) && pin < digitalPinCount) {
+    if (mode) {
+      *digitalPinMap[pin].ddr |= 1 << digitalPinMap[pin].bit;
+    } else {
+      *digitalPinMap[pin].ddr &= ~(1 << digitalPinMap[pin].bit);
+    }
+  } else {
+    badPinNumber();
+  }
+}
+static inline __attribute__((always_inline))
+  bool fastDigitalRead(uint8_t pin) {
+  if (__builtin_constant_p(pin) && pin < digitalPinCount) {
+    return (*digitalPinMap[pin].pin >> digitalPinMap[pin].bit) & 1;
+  } else {
+    return badPinNumber();
+  }
+}
+static inline __attribute__((always_inline))
+  void fastDigitalWrite(uint8_t pin, uint8_t value) {
+  if (__builtin_constant_p(pin) && pin < digitalPinCount) {
+    if (value) {
+      *digitalPinMap[pin].port |= 1 << digitalPinMap[pin].bit;
+    } else {
+      *digitalPinMap[pin].port &= ~(1 << digitalPinMap[pin].bit);
+    }
+  } else {
+    badPinNumber();
+  }
+}
+#endif  // Sd2PinMap_h
+
+
 #endif

+ 50 - 50
Firmware/SdFatUtil.h

@@ -1,51 +1,51 @@
-/* Arduino SdFat Library
- * Copyright (C) 2008 by William Greiman
- *
- * This file is part of the Arduino SdFat Library
- *
- * This Library is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This Library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
-
- * You should have received a copy of the GNU General Public License
- * along with the Arduino SdFat Library.  If not, see
- * <http://www.gnu.org/licenses/>.
- */
-#include "Marlin.h"
-#ifdef SDSUPPORT
-
-#ifndef SdFatUtil_h
-#define SdFatUtil_h
-/**
- * \file
- * \brief Useful utility functions.
- */
-#include "Marlin.h"
-#include "MarlinSerial.h"
-/** Store and print a string in flash memory.*/
-#define PgmPrint(x) SerialPrint_P(PSTR(x))
-/** Store and print a string in flash memory followed by a CR/LF.*/
-#define PgmPrintln(x) SerialPrintln_P(PSTR(x))
-
-namespace SdFatUtil {
-  int FreeRam();
-  void print_P( PGM_P str);
-  void println_P( PGM_P str);
-  void SerialPrint_P(PGM_P str);
-  void SerialPrintln_P(PGM_P str);
-  void set_stack_guard();
-  bool test_stack_integrity();
-  uint32_t get_stack_guard_test_value();
-}
-
-using namespace SdFatUtil;  // NOLINT
-#endif  // #define SdFatUtil_h
-
-
+/* Arduino SdFat Library
+ * Copyright (C) 2008 by William Greiman
+ *
+ * This file is part of the Arduino SdFat Library
+ *
+ * This Library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This Library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino SdFat Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+#include "Marlin.h"
+#ifdef SDSUPPORT
+
+#ifndef SdFatUtil_h
+#define SdFatUtil_h
+/**
+ * \file
+ * \brief Useful utility functions.
+ */
+#include "Marlin.h"
+#include "MarlinSerial.h"
+/** Store and print a string in flash memory.*/
+#define PgmPrint(x) SerialPrint_P(PSTR(x))
+/** Store and print a string in flash memory followed by a CR/LF.*/
+#define PgmPrintln(x) SerialPrintln_P(PSTR(x))
+
+namespace SdFatUtil {
+  int FreeRam();
+  void print_P( PGM_P str);
+  void println_P( PGM_P str);
+  void SerialPrint_P(PGM_P str);
+  void SerialPrintln_P(PGM_P str);
+  void set_stack_guard();
+  bool test_stack_integrity();
+  uint32_t get_stack_guard_test_value();
+}
+
+using namespace SdFatUtil;  // NOLINT
+#endif  // #define SdFatUtil_h
+
+
 #endif

+ 95 - 95
Firmware/SdFile.cpp

@@ -1,95 +1,95 @@
-/* Arduino SdFat Library
- * Copyright (C) 2009 by William Greiman
- *
- * This file is part of the Arduino SdFat Library
- *
- * This Library is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This Library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with the Arduino SdFat Library.  If not, see
- * <http://www.gnu.org/licenses/>.
- */
-#include "Marlin.h"
-
-#ifdef SDSUPPORT
-#include "SdFile.h"
-/**  Create a file object and open it in the current working directory.
- *
- * \param[in] path A path with a valid 8.3 DOS name for a file to be opened.
- *
- * \param[in] oflag Values for \a oflag are constructed by a bitwise-inclusive
- * OR of open flags. see SdBaseFile::open(SdBaseFile*, const char*, uint8_t).
- */
-SdFile::SdFile(const char* path, uint8_t oflag) : SdBaseFile(path, oflag) {
-}
-//------------------------------------------------------------------------------
-/** Write data to an open file.
- *
- * \note Data is moved to the cache but may not be written to the
- * storage device until sync() is called.
- *
- * \param[in] buf Pointer to the location of the data to be written.
- *
- * \param[in] nbyte Number of bytes to write.
- *
- * \return For success write() returns the number of bytes written, always
- * \a nbyte.  If an error occurs, write() returns -1.  Possible errors
- * include write() is called before a file has been opened, write is called
- * for a read-only file, device is full, a corrupt file system or an I/O error.
- *
- */
-int16_t SdFile::write(const void* buf, uint16_t nbyte) {
-  return SdBaseFile::write(buf, nbyte);
-}
-//------------------------------------------------------------------------------
-/** Write a byte to a file. Required by the Arduino Print class.
- * \param[in] b the byte to be written.
- * Use writeError to check for errors.
- */
-#if ARDUINO >= 100
-size_t SdFile::write(uint8_t b)
-{
-    return SdBaseFile::write(&b, 1);
-}
-#else
-void SdFile::write(uint8_t b)
-{
-    SdBaseFile::write(&b, 1);
-}
-#endif
-//------------------------------------------------------------------------------
-/** Write a string to a file. Used by the Arduino Print class.
- * \param[in] str Pointer to the string.
- * Use writeError to check for errors.
- */
-void SdFile::write(const char* str) {
-  SdBaseFile::write(str, strlen(str));
-}
-//------------------------------------------------------------------------------
-/** Write a PROGMEM string to a file.
- * \param[in] str Pointer to the PROGMEM string.
- * Use writeError to check for errors.
- */
-void SdFile::write_P(PGM_P str) {
-  for (uint8_t c; (c = pgm_read_byte(str)); str++) write(c);
-}
-//------------------------------------------------------------------------------
-/** Write a PROGMEM string followed by CR/LF to a file.
- * \param[in] str Pointer to the PROGMEM string.
- * Use writeError to check for errors.
- */
-void SdFile::writeln_P(PGM_P str) {
-  write_P(str);
-  write_P(PSTR("\r\n"));
-}
-
-
-#endif
+/* Arduino SdFat Library
+ * Copyright (C) 2009 by William Greiman
+ *
+ * This file is part of the Arduino SdFat Library
+ *
+ * This Library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This Library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino SdFat Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+#include "Marlin.h"
+
+#ifdef SDSUPPORT
+#include "SdFile.h"
+/**  Create a file object and open it in the current working directory.
+ *
+ * \param[in] path A path with a valid 8.3 DOS name for a file to be opened.
+ *
+ * \param[in] oflag Values for \a oflag are constructed by a bitwise-inclusive
+ * OR of open flags. see SdBaseFile::open(SdBaseFile*, const char*, uint8_t).
+ */
+SdFile::SdFile(const char* path, uint8_t oflag) : SdBaseFile(path, oflag) {
+}
+//------------------------------------------------------------------------------
+/** Write data to an open file.
+ *
+ * \note Data is moved to the cache but may not be written to the
+ * storage device until sync() is called.
+ *
+ * \param[in] buf Pointer to the location of the data to be written.
+ *
+ * \param[in] nbyte Number of bytes to write.
+ *
+ * \return For success write() returns the number of bytes written, always
+ * \a nbyte.  If an error occurs, write() returns -1.  Possible errors
+ * include write() is called before a file has been opened, write is called
+ * for a read-only file, device is full, a corrupt file system or an I/O error.
+ *
+ */
+int16_t SdFile::write(const void* buf, uint16_t nbyte) {
+  return SdBaseFile::write(buf, nbyte);
+}
+//------------------------------------------------------------------------------
+/** Write a byte to a file. Required by the Arduino Print class.
+ * \param[in] b the byte to be written.
+ * Use writeError to check for errors.
+ */
+#if ARDUINO >= 100
+size_t SdFile::write(uint8_t b)
+{
+    return SdBaseFile::write(&b, 1);
+}
+#else
+void SdFile::write(uint8_t b)
+{
+    SdBaseFile::write(&b, 1);
+}
+#endif
+//------------------------------------------------------------------------------
+/** Write a string to a file. Used by the Arduino Print class.
+ * \param[in] str Pointer to the string.
+ * Use writeError to check for errors.
+ */
+void SdFile::write(const char* str) {
+  SdBaseFile::write(str, strlen(str));
+}
+//------------------------------------------------------------------------------
+/** Write a PROGMEM string to a file.
+ * \param[in] str Pointer to the PROGMEM string.
+ * Use writeError to check for errors.
+ */
+void SdFile::write_P(PGM_P str) {
+  for (uint8_t c; (c = pgm_read_byte(str)); str++) write(c);
+}
+//------------------------------------------------------------------------------
+/** Write a PROGMEM string followed by CR/LF to a file.
+ * \param[in] str Pointer to the PROGMEM string.
+ * Use writeError to check for errors.
+ */
+void SdFile::writeln_P(PGM_P str) {
+  write_P(str);
+  write_P(PSTR("\r\n"));
+}
+
+
+#endif

+ 285 - 285
Firmware/SdInfo.h

@@ -1,286 +1,286 @@
-/* Arduino Sd2Card Library
- * Copyright (C) 2009 by William Greiman
- *
- * This file is part of the Arduino Sd2Card Library
- *
- * This Library is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This Library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with the Arduino Sd2Card Library.  If not, see
- * <http://www.gnu.org/licenses/>.
- */
-#include "Marlin.h"
-#ifdef SDSUPPORT
-
-#ifndef SdInfo_h
-#define SdInfo_h
-#include <stdint.h>
-// Based on the document:
-//
-// SD Specifications
-// Part 1
-// Physical Layer
-// Simplified Specification
-// Version 3.01
-// May 18, 2010
-//
-// http://www.sdcard.org/developers/tech/sdcard/pls/simplified_specs
-//------------------------------------------------------------------------------
-// SD card commands
-/** GO_IDLE_STATE - init card in spi mode if CS low */
-uint8_t const CMD0 = 0X00;
-/** SEND_IF_COND - verify SD Memory Card interface operating condition.*/
-uint8_t const CMD8 = 0X08;
-/** SEND_CSD - read the Card Specific Data (CSD register) */
-uint8_t const CMD9 = 0X09;
-/** SEND_CID - read the card identification information (CID register) */
-uint8_t const CMD10 = 0X0A;
-/** STOP_TRANSMISSION - end multiple block read sequence */
-uint8_t const CMD12 = 0X0C;
-/** SEND_STATUS - read the card status register */
-uint8_t const CMD13 = 0X0D;
-/** READ_SINGLE_BLOCK - read a single data block from the card */
-uint8_t const CMD17 = 0X11;
-/** READ_MULTIPLE_BLOCK - read a multiple data blocks from the card */
-uint8_t const CMD18 = 0X12;
-/** WRITE_BLOCK - write a single data block to the card */
-uint8_t const CMD24 = 0X18;
-/** WRITE_MULTIPLE_BLOCK - write blocks of data until a STOP_TRANSMISSION */
-uint8_t const CMD25 = 0X19;
-/** ERASE_WR_BLK_START - sets the address of the first block to be erased */
-uint8_t const CMD32 = 0X20;
-/** ERASE_WR_BLK_END - sets the address of the last block of the continuous
-    range to be erased*/
-uint8_t const CMD33 = 0X21;
-/** ERASE - erase all previously selected blocks */
-uint8_t const CMD38 = 0X26;
-
-/** Toshiba FlashAir: iSDIO */
-uint8_t const CMD48 = 0x30;
-/** Toshiba FlashAir: iSDIO */
-uint8_t const CMD49 = 0x31;
-
-/** APP_CMD - escape for application specific command */
-uint8_t const CMD55 = 0X37;
-/** READ_OCR - read the OCR register of a card */
-uint8_t const CMD58 = 0X3A;
-/** SET_WR_BLK_ERASE_COUNT - Set the number of write blocks to be
-     pre-erased before writing */
-uint8_t const ACMD23 = 0X17;
-/** SD_SEND_OP_COMD - Sends host capacity support information and
-    activates the card's initialization process */
-uint8_t const ACMD41 = 0X29;
-//------------------------------------------------------------------------------
-/** status for card in the ready state */
-uint8_t const R1_READY_STATE = 0X00;
-/** status for card in the idle state */
-uint8_t const R1_IDLE_STATE = 0X01;
-/** status bit for illegal command */
-uint8_t const R1_ILLEGAL_COMMAND = 0X04;
-/** start data token for read or write single block*/
-uint8_t const DATA_START_BLOCK = 0XFE;
-/** stop token for write multiple blocks*/
-uint8_t const STOP_TRAN_TOKEN = 0XFD;
-/** start data token for write multiple blocks*/
-uint8_t const WRITE_MULTIPLE_TOKEN = 0XFC;
-/** mask for data response tokens after a write block operation */
-uint8_t const DATA_RES_MASK = 0X1F;
-/** write data accepted token */
-uint8_t const DATA_RES_ACCEPTED = 0X05;
-//------------------------------------------------------------------------------
-/** Card IDentification (CID) register */
-typedef struct CID {
-  // byte 0
-  /** Manufacturer ID */
-  unsigned char mid;
-  // byte 1-2
-  /** OEM/Application ID */
-  char oid[2];
-  // byte 3-7
-  /** Product name */
-  char pnm[5];
-  // byte 8
-  /** Product revision least significant digit */
-  unsigned char prv_m : 4;
-  /** Product revision most significant digit */
-  unsigned char prv_n : 4;
-  // byte 9-12
-  /** Product serial number */
-  uint32_t psn;
-  // byte 13
-  /** Manufacturing date year low digit */
-  unsigned char mdt_year_high : 4;
-  /** not used */
-  unsigned char reserved : 4;
-  // byte 14
-  /** Manufacturing date month */
-  unsigned char mdt_month : 4;
-  /** Manufacturing date year low digit */
-  unsigned char mdt_year_low :4;
-  // byte 15
-  /** not used always 1 */
-  unsigned char always1 : 1;
-  /** CRC7 checksum */
-  unsigned char crc : 7;
-}cid_t;
-//------------------------------------------------------------------------------
-/** CSD for version 1.00 cards */
-typedef struct CSDV1 {
-  // byte 0
-  unsigned char reserved1 : 6;
-  unsigned char csd_ver : 2;
-  // byte 1
-  unsigned char taac;
-  // byte 2
-  unsigned char nsac;
-  // byte 3
-  unsigned char tran_speed;
-  // byte 4
-  unsigned char ccc_high;
-  // byte 5
-  unsigned char read_bl_len : 4;
-  unsigned char ccc_low : 4;
-  // byte 6
-  unsigned char c_size_high : 2;
-  unsigned char reserved2 : 2;
-  unsigned char dsr_imp : 1;
-  unsigned char read_blk_misalign :1;
-  unsigned char write_blk_misalign : 1;
-  unsigned char read_bl_partial : 1;
-  // byte 7
-  unsigned char c_size_mid;
-  // byte 8
-  unsigned char vdd_r_curr_max : 3;
-  unsigned char vdd_r_curr_min : 3;
-  unsigned char c_size_low :2;
-  // byte 9
-  unsigned char c_size_mult_high : 2;
-  unsigned char vdd_w_cur_max : 3;
-  unsigned char vdd_w_curr_min : 3;
-  // byte 10
-  unsigned char sector_size_high : 6;
-  unsigned char erase_blk_en : 1;
-  unsigned char c_size_mult_low : 1;
-  // byte 11
-  unsigned char wp_grp_size : 7;
-  unsigned char sector_size_low : 1;
-  // byte 12
-  unsigned char write_bl_len_high : 2;
-  unsigned char r2w_factor : 3;
-  unsigned char reserved3 : 2;
-  unsigned char wp_grp_enable : 1;
-  // byte 13
-  unsigned char reserved4 : 5;
-  unsigned char write_partial : 1;
-  unsigned char write_bl_len_low : 2;
-  // byte 14
-  unsigned char reserved5: 2;
-  unsigned char file_format : 2;
-  unsigned char tmp_write_protect : 1;
-  unsigned char perm_write_protect : 1;
-  unsigned char copy : 1;
-  /** Indicates the file format on the card */
-  unsigned char file_format_grp : 1;
-  // byte 15
-  unsigned char always1 : 1;
-  unsigned char crc : 7;
-}csd1_t;
-//------------------------------------------------------------------------------
-/** CSD for version 2.00 cards */
-typedef struct CSDV2 {
-  // byte 0
-  unsigned char reserved1 : 6;
-  unsigned char csd_ver : 2;
-  // byte 1
-  /** fixed to 0X0E */
-  unsigned char taac;
-  // byte 2
-  /** fixed to 0 */
-  unsigned char nsac;
-  // byte 3
-  unsigned char tran_speed;
-  // byte 4
-  unsigned char ccc_high;
-  // byte 5
-  /** This field is fixed to 9h, which indicates READ_BL_LEN=512 Byte */
-  unsigned char read_bl_len : 4;
-  unsigned char ccc_low : 4;
-  // byte 6
-  /** not used */
-  unsigned char reserved2 : 4;
-  unsigned char dsr_imp : 1;
-  /** fixed to 0 */
-  unsigned char read_blk_misalign :1;
-  /** fixed to 0 */
-  unsigned char write_blk_misalign : 1;
-  /** fixed to 0 - no partial read */
-  unsigned char read_bl_partial : 1;
-  // byte 7
-  /** not used */
-  unsigned char reserved3 : 2;
-  /** high part of card size */
-  unsigned char c_size_high : 6;
-  // byte 8
-  /** middle part of card size */
-  unsigned char c_size_mid;
-  // byte 9
-  /** low part of card size */
-  unsigned char c_size_low;
-  // byte 10
-  /** sector size is fixed at 64 KB */
-  unsigned char sector_size_high : 6;
-  /** fixed to 1 - erase single is supported */
-  unsigned char erase_blk_en : 1;
-  /** not used */
-  unsigned char reserved4 : 1;
-  // byte 11
-  unsigned char wp_grp_size : 7;
-  /** sector size is fixed at 64 KB */
-  unsigned char sector_size_low : 1;
-  // byte 12
-  /** write_bl_len fixed for 512 byte blocks */
-  unsigned char write_bl_len_high : 2;
-  /** fixed value of 2 */
-  unsigned char r2w_factor : 3;
-  /** not used */
-  unsigned char reserved5 : 2;
-  /** fixed value of 0 - no write protect groups */
-  unsigned char wp_grp_enable : 1;
-  // byte 13
-  unsigned char reserved6 : 5;
-  /** always zero - no partial block read*/
-  unsigned char write_partial : 1;
-  /** write_bl_len fixed for 512 byte blocks */
-  unsigned char write_bl_len_low : 2;
-  // byte 14
-  unsigned char reserved7: 2;
-  /** Do not use always 0 */
-  unsigned char file_format : 2;
-  unsigned char tmp_write_protect : 1;
-  unsigned char perm_write_protect : 1;
-  unsigned char copy : 1;
-  /** Do not use always 0 */
-  unsigned char file_format_grp : 1;
-  // byte 15
-  /** not used always 1 */
-  unsigned char always1 : 1;
-  /** checksum */
-  unsigned char crc : 7;
-}csd2_t;
-//------------------------------------------------------------------------------
-/** union of old and new style CSD register */
-union csd_t {
-  csd1_t v1;
-  csd2_t v2;
-};
-#endif  // SdInfo_h
-
+/* Arduino Sd2Card Library
+ * Copyright (C) 2009 by William Greiman
+ *
+ * This file is part of the Arduino Sd2Card Library
+ *
+ * This Library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This Library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino Sd2Card Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+#include "Marlin.h"
+#ifdef SDSUPPORT
+
+#ifndef SdInfo_h
+#define SdInfo_h
+#include <stdint.h>
+// Based on the document:
+//
+// SD Specifications
+// Part 1
+// Physical Layer
+// Simplified Specification
+// Version 3.01
+// May 18, 2010
+//
+// http://www.sdcard.org/developers/tech/sdcard/pls/simplified_specs
+//------------------------------------------------------------------------------
+// SD card commands
+/** GO_IDLE_STATE - init card in spi mode if CS low */
+uint8_t const CMD0 = 0X00;
+/** SEND_IF_COND - verify SD Memory Card interface operating condition.*/
+uint8_t const CMD8 = 0X08;
+/** SEND_CSD - read the Card Specific Data (CSD register) */
+uint8_t const CMD9 = 0X09;
+/** SEND_CID - read the card identification information (CID register) */
+uint8_t const CMD10 = 0X0A;
+/** STOP_TRANSMISSION - end multiple block read sequence */
+uint8_t const CMD12 = 0X0C;
+/** SEND_STATUS - read the card status register */
+uint8_t const CMD13 = 0X0D;
+/** READ_SINGLE_BLOCK - read a single data block from the card */
+uint8_t const CMD17 = 0X11;
+/** READ_MULTIPLE_BLOCK - read a multiple data blocks from the card */
+uint8_t const CMD18 = 0X12;
+/** WRITE_BLOCK - write a single data block to the card */
+uint8_t const CMD24 = 0X18;
+/** WRITE_MULTIPLE_BLOCK - write blocks of data until a STOP_TRANSMISSION */
+uint8_t const CMD25 = 0X19;
+/** ERASE_WR_BLK_START - sets the address of the first block to be erased */
+uint8_t const CMD32 = 0X20;
+/** ERASE_WR_BLK_END - sets the address of the last block of the continuous
+    range to be erased*/
+uint8_t const CMD33 = 0X21;
+/** ERASE - erase all previously selected blocks */
+uint8_t const CMD38 = 0X26;
+
+/** Toshiba FlashAir: iSDIO */
+uint8_t const CMD48 = 0x30;
+/** Toshiba FlashAir: iSDIO */
+uint8_t const CMD49 = 0x31;
+
+/** APP_CMD - escape for application specific command */
+uint8_t const CMD55 = 0X37;
+/** READ_OCR - read the OCR register of a card */
+uint8_t const CMD58 = 0X3A;
+/** SET_WR_BLK_ERASE_COUNT - Set the number of write blocks to be
+     pre-erased before writing */
+uint8_t const ACMD23 = 0X17;
+/** SD_SEND_OP_COMD - Sends host capacity support information and
+    activates the card's initialization process */
+uint8_t const ACMD41 = 0X29;
+//------------------------------------------------------------------------------
+/** status for card in the ready state */
+uint8_t const R1_READY_STATE = 0X00;
+/** status for card in the idle state */
+uint8_t const R1_IDLE_STATE = 0X01;
+/** status bit for illegal command */
+uint8_t const R1_ILLEGAL_COMMAND = 0X04;
+/** start data token for read or write single block*/
+uint8_t const DATA_START_BLOCK = 0XFE;
+/** stop token for write multiple blocks*/
+uint8_t const STOP_TRAN_TOKEN = 0XFD;
+/** start data token for write multiple blocks*/
+uint8_t const WRITE_MULTIPLE_TOKEN = 0XFC;
+/** mask for data response tokens after a write block operation */
+uint8_t const DATA_RES_MASK = 0X1F;
+/** write data accepted token */
+uint8_t const DATA_RES_ACCEPTED = 0X05;
+//------------------------------------------------------------------------------
+/** Card IDentification (CID) register */
+typedef struct CID {
+  // byte 0
+  /** Manufacturer ID */
+  unsigned char mid;
+  // byte 1-2
+  /** OEM/Application ID */
+  char oid[2];
+  // byte 3-7
+  /** Product name */
+  char pnm[5];
+  // byte 8
+  /** Product revision least significant digit */
+  unsigned char prv_m : 4;
+  /** Product revision most significant digit */
+  unsigned char prv_n : 4;
+  // byte 9-12
+  /** Product serial number */
+  uint32_t psn;
+  // byte 13
+  /** Manufacturing date year low digit */
+  unsigned char mdt_year_high : 4;
+  /** not used */
+  unsigned char reserved : 4;
+  // byte 14
+  /** Manufacturing date month */
+  unsigned char mdt_month : 4;
+  /** Manufacturing date year low digit */
+  unsigned char mdt_year_low :4;
+  // byte 15
+  /** not used always 1 */
+  unsigned char always1 : 1;
+  /** CRC7 checksum */
+  unsigned char crc : 7;
+}cid_t;
+//------------------------------------------------------------------------------
+/** CSD for version 1.00 cards */
+typedef struct CSDV1 {
+  // byte 0
+  unsigned char reserved1 : 6;
+  unsigned char csd_ver : 2;
+  // byte 1
+  unsigned char taac;
+  // byte 2
+  unsigned char nsac;
+  // byte 3
+  unsigned char tran_speed;
+  // byte 4
+  unsigned char ccc_high;
+  // byte 5
+  unsigned char read_bl_len : 4;
+  unsigned char ccc_low : 4;
+  // byte 6
+  unsigned char c_size_high : 2;
+  unsigned char reserved2 : 2;
+  unsigned char dsr_imp : 1;
+  unsigned char read_blk_misalign :1;
+  unsigned char write_blk_misalign : 1;
+  unsigned char read_bl_partial : 1;
+  // byte 7
+  unsigned char c_size_mid;
+  // byte 8
+  unsigned char vdd_r_curr_max : 3;
+  unsigned char vdd_r_curr_min : 3;
+  unsigned char c_size_low :2;
+  // byte 9
+  unsigned char c_size_mult_high : 2;
+  unsigned char vdd_w_cur_max : 3;
+  unsigned char vdd_w_curr_min : 3;
+  // byte 10
+  unsigned char sector_size_high : 6;
+  unsigned char erase_blk_en : 1;
+  unsigned char c_size_mult_low : 1;
+  // byte 11
+  unsigned char wp_grp_size : 7;
+  unsigned char sector_size_low : 1;
+  // byte 12
+  unsigned char write_bl_len_high : 2;
+  unsigned char r2w_factor : 3;
+  unsigned char reserved3 : 2;
+  unsigned char wp_grp_enable : 1;
+  // byte 13
+  unsigned char reserved4 : 5;
+  unsigned char write_partial : 1;
+  unsigned char write_bl_len_low : 2;
+  // byte 14
+  unsigned char reserved5: 2;
+  unsigned char file_format : 2;
+  unsigned char tmp_write_protect : 1;
+  unsigned char perm_write_protect : 1;
+  unsigned char copy : 1;
+  /** Indicates the file format on the card */
+  unsigned char file_format_grp : 1;
+  // byte 15
+  unsigned char always1 : 1;
+  unsigned char crc : 7;
+}csd1_t;
+//------------------------------------------------------------------------------
+/** CSD for version 2.00 cards */
+typedef struct CSDV2 {
+  // byte 0
+  unsigned char reserved1 : 6;
+  unsigned char csd_ver : 2;
+  // byte 1
+  /** fixed to 0X0E */
+  unsigned char taac;
+  // byte 2
+  /** fixed to 0 */
+  unsigned char nsac;
+  // byte 3
+  unsigned char tran_speed;
+  // byte 4
+  unsigned char ccc_high;
+  // byte 5
+  /** This field is fixed to 9h, which indicates READ_BL_LEN=512 Byte */
+  unsigned char read_bl_len : 4;
+  unsigned char ccc_low : 4;
+  // byte 6
+  /** not used */
+  unsigned char reserved2 : 4;
+  unsigned char dsr_imp : 1;
+  /** fixed to 0 */
+  unsigned char read_blk_misalign :1;
+  /** fixed to 0 */
+  unsigned char write_blk_misalign : 1;
+  /** fixed to 0 - no partial read */
+  unsigned char read_bl_partial : 1;
+  // byte 7
+  /** not used */
+  unsigned char reserved3 : 2;
+  /** high part of card size */
+  unsigned char c_size_high : 6;
+  // byte 8
+  /** middle part of card size */
+  unsigned char c_size_mid;
+  // byte 9
+  /** low part of card size */
+  unsigned char c_size_low;
+  // byte 10
+  /** sector size is fixed at 64 KB */
+  unsigned char sector_size_high : 6;
+  /** fixed to 1 - erase single is supported */
+  unsigned char erase_blk_en : 1;
+  /** not used */
+  unsigned char reserved4 : 1;
+  // byte 11
+  unsigned char wp_grp_size : 7;
+  /** sector size is fixed at 64 KB */
+  unsigned char sector_size_low : 1;
+  // byte 12
+  /** write_bl_len fixed for 512 byte blocks */
+  unsigned char write_bl_len_high : 2;
+  /** fixed value of 2 */
+  unsigned char r2w_factor : 3;
+  /** not used */
+  unsigned char reserved5 : 2;
+  /** fixed value of 0 - no write protect groups */
+  unsigned char wp_grp_enable : 1;
+  // byte 13
+  unsigned char reserved6 : 5;
+  /** always zero - no partial block read*/
+  unsigned char write_partial : 1;
+  /** write_bl_len fixed for 512 byte blocks */
+  unsigned char write_bl_len_low : 2;
+  // byte 14
+  unsigned char reserved7: 2;
+  /** Do not use always 0 */
+  unsigned char file_format : 2;
+  unsigned char tmp_write_protect : 1;
+  unsigned char perm_write_protect : 1;
+  unsigned char copy : 1;
+  /** Do not use always 0 */
+  unsigned char file_format_grp : 1;
+  // byte 15
+  /** not used always 1 */
+  unsigned char always1 : 1;
+  /** checksum */
+  unsigned char crc : 7;
+}csd2_t;
+//------------------------------------------------------------------------------
+/** union of old and new style CSD register */
+union csd_t {
+  csd1_t v1;
+  csd2_t v2;
+};
+#endif  // SdInfo_h
+
 #endif

+ 404 - 404
Firmware/SdVolume.cpp

@@ -1,405 +1,405 @@
-/* Arduino SdFat Library
- * Copyright (C) 2009 by William Greiman
- *
- * This file is part of the Arduino SdFat Library
- *
- * This Library is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This Library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with the Arduino SdFat Library.  If not, see
- * <http://www.gnu.org/licenses/>.
- */
-#include "Marlin.h"
-#ifdef SDSUPPORT
-
-#include "SdVolume.h"
-//------------------------------------------------------------------------------
-#if !USE_MULTIPLE_CARDS
-// raw block cache
-uint32_t SdVolume::cacheBlockNumber_;  // current block number
-cache_t  SdVolume::cacheBuffer_;       // 512 byte cache for Sd2Card
-Sd2Card* SdVolume::sdCard_;            // pointer to SD card object
-bool     SdVolume::cacheDirty_;        // cacheFlush() will write block if true
-uint32_t SdVolume::cacheMirrorBlock_;  // mirror  block for second FAT
-#endif  // USE_MULTIPLE_CARDS
-//------------------------------------------------------------------------------
-// find a contiguous group of clusters
-bool SdVolume::allocContiguous(uint32_t count, uint32_t* curCluster) {
-  // start of group
-  uint32_t bgnCluster;
-  // end of group
-  uint32_t endCluster;
-  // last cluster of FAT
-  uint32_t fatEnd = clusterCount_ + 1;
-
-  // flag to save place to start next search
-  bool setStart;
-
-  // set search start cluster
-  if (*curCluster) {
-    // try to make file contiguous
-    bgnCluster = *curCluster + 1;
-
-    // don't save new start location
-    setStart = false;
-  } else {
-    // start at likely place for free cluster
-    bgnCluster = allocSearchStart_;
-
-    // save next search start if one cluster
-    setStart = count == 1;
-  }
-  // end of group
-  endCluster = bgnCluster;
-
-  // search the FAT for free clusters
-  for (uint32_t n = 0;; n++, endCluster++) {
-    // can't find space checked all clusters
-    if (n >= clusterCount_) goto fail;
-
-    // past end - start from beginning of FAT
-    if (endCluster > fatEnd) {
-      bgnCluster = endCluster = 2;
-    }
-    uint32_t f;
-    if (!fatGet(endCluster, &f)) goto fail;
-
-    if (f != 0) {
-      // cluster in use try next cluster as bgnCluster
-      bgnCluster = endCluster + 1;
-    } else if ((endCluster - bgnCluster + 1) == count) {
-      // done - found space
-      break;
-    }
-  }
-  // mark end of chain
-  if (!fatPutEOC(endCluster)) goto fail;
-
-  // link clusters
-  while (endCluster > bgnCluster) {
-    if (!fatPut(endCluster - 1, endCluster)) goto fail;
-    endCluster--;
-  }
-  if (*curCluster != 0) {
-    // connect chains
-    if (!fatPut(*curCluster, bgnCluster)) goto fail;
-  }
-  // return first cluster number to caller
-  *curCluster = bgnCluster;
-
-  // remember possible next free cluster
-  if (setStart) allocSearchStart_ = bgnCluster + 1;
-
-  return true;
-
- fail:
-  return false;
-}
-//------------------------------------------------------------------------------
-bool SdVolume::cacheFlush() {
-  if (cacheDirty_) {
-    if (!sdCard_->writeBlock(cacheBlockNumber_, cacheBuffer_.data)) {
-      goto fail;
-    }
-    // mirror FAT tables
-    if (cacheMirrorBlock_) {
-      if (!sdCard_->writeBlock(cacheMirrorBlock_, cacheBuffer_.data)) {
-        goto fail;
-      }
-      cacheMirrorBlock_ = 0;
-    }
-    cacheDirty_ = 0;
-  }
-  return true;
-
- fail:
-  return false;
-}
-//------------------------------------------------------------------------------
-bool SdVolume::cacheRawBlock(uint32_t blockNumber, bool dirty) {
-  if (cacheBlockNumber_ != blockNumber) {
-    if (!cacheFlush()) goto fail;
-    if (!sdCard_->readBlock(blockNumber, cacheBuffer_.data)) goto fail;
-    cacheBlockNumber_ = blockNumber;
-  }
-  if (dirty) cacheDirty_ = true;
-  return true;
-
- fail:
-  return false;
-}
-//------------------------------------------------------------------------------
-// return the size in bytes of a cluster chain
-bool SdVolume::chainSize(uint32_t cluster, uint32_t* size) {
-  uint32_t s = 0;
-  do {
-    if (!fatGet(cluster, &cluster)) goto fail;
-    s += 512UL << clusterSizeShift_;
-  } while (!isEOC(cluster));
-  *size = s;
-  return true;
-
- fail:
-  return false;
-}
-//------------------------------------------------------------------------------
-// Fetch a FAT entry
-bool SdVolume::fatGet(uint32_t cluster, uint32_t* value) {
-  uint32_t lba;
-  if (cluster > (clusterCount_ + 1)) goto fail;
-  if (FAT12_SUPPORT && fatType_ == 12) {
-    uint16_t index = cluster;
-    index += index >> 1;
-    lba = fatStartBlock_ + (index >> 9);
-    if (!cacheRawBlock(lba, CACHE_FOR_READ)) goto fail;
-    index &= 0X1FF;
-    uint16_t tmp = cacheBuffer_.data[index];
-    index++;
-    if (index == 512) {
-      if (!cacheRawBlock(lba + 1, CACHE_FOR_READ)) goto fail;
-      index = 0;
-    }
-    tmp |= cacheBuffer_.data[index] << 8;
-    *value = cluster & 1 ? tmp >> 4 : tmp & 0XFFF;
-    return true;
-  }
-  if (fatType_ == 16) {
-    lba = fatStartBlock_ + (cluster >> 8);
-  } else if (fatType_ == 32) {
-    lba = fatStartBlock_ + (cluster >> 7);
-  } else {
-    goto fail;
-  }
-  if (lba != cacheBlockNumber_) {
-    if (!cacheRawBlock(lba, CACHE_FOR_READ)) goto fail;
-  }
-  if (fatType_ == 16) {
-    *value = cacheBuffer_.fat16[cluster & 0XFF];
-  } else {
-    *value = cacheBuffer_.fat32[cluster & 0X7F] & FAT32MASK;
-  }
-  return true;
-
- fail:
-  return false;
-}
-//------------------------------------------------------------------------------
-// Store a FAT entry
-bool SdVolume::fatPut(uint32_t cluster, uint32_t value) {
-  uint32_t lba;
-  // error if reserved cluster
-  if (cluster < 2) goto fail;
-
-  // error if not in FAT
-  if (cluster > (clusterCount_ + 1)) goto fail;
-
-  if (FAT12_SUPPORT && fatType_ == 12) {
-    uint16_t index = cluster;
-    index += index >> 1;
-    lba = fatStartBlock_ + (index >> 9);
-    if (!cacheRawBlock(lba, CACHE_FOR_WRITE)) goto fail;
-    // mirror second FAT
-    if (fatCount_ > 1) cacheMirrorBlock_ = lba + blocksPerFat_;
-    index &= 0X1FF;
-    uint8_t tmp = value;
-    if (cluster & 1) {
-      tmp = (cacheBuffer_.data[index] & 0XF) | tmp << 4;
-    }
-    cacheBuffer_.data[index] = tmp;
-    index++;
-    if (index == 512) {
-      lba++;
-      index = 0;
-      if (!cacheRawBlock(lba, CACHE_FOR_WRITE)) goto fail;
-      // mirror second FAT
-      if (fatCount_ > 1) cacheMirrorBlock_ = lba + blocksPerFat_;
-    }
-    tmp = value >> 4;
-    if (!(cluster & 1)) {
-      tmp = ((cacheBuffer_.data[index] & 0XF0)) | tmp >> 4;
-    }
-    cacheBuffer_.data[index] = tmp;
-    return true;
-  }
-  if (fatType_ == 16) {
-    lba = fatStartBlock_ + (cluster >> 8);
-  } else if (fatType_ == 32) {
-    lba = fatStartBlock_ + (cluster >> 7);
-  } else {
-    goto fail;
-  }
-  if (!cacheRawBlock(lba, CACHE_FOR_WRITE)) goto fail;
-  // store entry
-  if (fatType_ == 16) {
-    cacheBuffer_.fat16[cluster & 0XFF] = value;
-  } else {
-    cacheBuffer_.fat32[cluster & 0X7F] = value;
-  }
-  // mirror second FAT
-  if (fatCount_ > 1) cacheMirrorBlock_ = lba + blocksPerFat_;
-  return true;
-
- fail:
-  return false;
-}
-//------------------------------------------------------------------------------
-// free a cluster chain
-bool SdVolume::freeChain(uint32_t cluster) {
-  uint32_t next;
-
-  // clear free cluster location
-  allocSearchStart_ = 2;
-
-  do {
-    if (!fatGet(cluster, &next)) goto fail;
-
-    // free cluster
-    if (!fatPut(cluster, 0)) goto fail;
-
-    cluster = next;
-  } while (!isEOC(cluster));
-
-  return true;
-
- fail:
-  return false;
-}
-//------------------------------------------------------------------------------
-/** Volume free space in clusters.
- *
- * \return Count of free clusters for success or -1 if an error occurs.
- */
-int32_t SdVolume::freeClusterCount() {
-  uint32_t free = 0;
-  uint16_t n;
-  uint32_t todo = clusterCount_ + 2;
-
-  if (fatType_ == 16) {
-    n = 256;
-  } else if (fatType_ == 32) {
-    n = 128;
-  } else {
-    // put FAT12 here
-    return -1;
-  }
-
-  for (uint32_t lba = fatStartBlock_; todo; todo -= n, lba++) {
-    if (!cacheRawBlock(lba, CACHE_FOR_READ)) return -1;
-    if (todo < n) n = todo;
-    if (fatType_ == 16) {
-      for (uint16_t i = 0; i < n; i++) {
-        if (cacheBuffer_.fat16[i] == 0) free++;
-      }
-    } else {
-      for (uint16_t i = 0; i < n; i++) {
-        if (cacheBuffer_.fat32[i] == 0) free++;
-      }
-    }
-  }
-  return free;
-}
-//------------------------------------------------------------------------------
-/** Initialize a FAT volume.
- *
- * \param[in] dev The SD card where the volume is located.
- *
- * \param[in] part The partition to be used.  Legal values for \a part are
- * 1-4 to use the corresponding partition on a device formatted with
- * a MBR, Master Boot Record, or zero if the device is formatted as
- * a super floppy with the FAT boot sector in block zero.
- *
- * \return The value one, true, is returned for success and
- * the value zero, false, is returned for failure.  Reasons for
- * failure include not finding a valid partition, not finding a valid
- * FAT file system in the specified partition or an I/O error.
- */
-bool SdVolume::init(Sd2Card* dev, uint8_t part) {
-  uint32_t totalBlocks;
-  uint32_t volumeStartBlock = 0;
-  fat32_boot_t* fbs;
-
-  sdCard_ = dev;
-  fatType_ = 0;
-  allocSearchStart_ = 2;
-  cacheDirty_ = 0;  // cacheFlush() will write block if true
-  cacheMirrorBlock_ = 0;
-  cacheBlockNumber_ = 0XFFFFFFFF;
-
-  // if part == 0 assume super floppy with FAT boot sector in block zero
-  // if part > 0 assume mbr volume with partition table
-  if (part) {
-    if (part > 4)goto fail;
-    if (!cacheRawBlock(volumeStartBlock, CACHE_FOR_READ)) goto fail;
-    part_t* p = &cacheBuffer_.mbr.part[part-1];
-    if ((p->boot & 0X7F) !=0  ||
-      p->totalSectors < 100 ||
-      p->firstSector == 0) {
-      // not a valid partition
-      goto fail;
-    }
-    volumeStartBlock = p->firstSector;
-  }
-  if (!cacheRawBlock(volumeStartBlock, CACHE_FOR_READ)) goto fail;
-  fbs = &cacheBuffer_.fbs32;
-  if (fbs->bytesPerSector != 512 ||
-    fbs->fatCount == 0 ||
-    fbs->reservedSectorCount == 0 ||
-    fbs->sectorsPerCluster == 0) {
-       // not valid FAT volume
-      goto fail;
-  }
-  fatCount_ = fbs->fatCount;
-  blocksPerCluster_ = fbs->sectorsPerCluster;
-  // determine shift that is same as multiply by blocksPerCluster_
-  clusterSizeShift_ = 0;
-  while (blocksPerCluster_ != (1 << clusterSizeShift_)) {
-    // error if not power of 2
-    if (clusterSizeShift_++ > 7) goto fail;
-  }
-  blocksPerFat_ = fbs->sectorsPerFat16 ?
-                    fbs->sectorsPerFat16 : fbs->sectorsPerFat32;
-
-  fatStartBlock_ = volumeStartBlock + fbs->reservedSectorCount;
-
-  // count for FAT16 zero for FAT32
-  rootDirEntryCount_ = fbs->rootDirEntryCount;
-
-  // directory start for FAT16 dataStart for FAT32
-  rootDirStart_ = fatStartBlock_ + fbs->fatCount * blocksPerFat_;
-
-  // data start for FAT16 and FAT32
-  dataStartBlock_ = rootDirStart_ + ((32 * fbs->rootDirEntryCount + 511)/512);
-
-  // total blocks for FAT16 or FAT32
-  totalBlocks = fbs->totalSectors16 ?
-                           fbs->totalSectors16 : fbs->totalSectors32;
-  // total data blocks
-  clusterCount_ = totalBlocks - (dataStartBlock_ - volumeStartBlock);
-
-  // divide by cluster size to get cluster count
-  clusterCount_ >>= clusterSizeShift_;
-
-  // FAT type is determined by cluster count
-  if (clusterCount_ < 4085) {
-    fatType_ = 12;
-    if (!FAT12_SUPPORT) goto fail;
-  } else if (clusterCount_ < 65525) {
-    fatType_ = 16;
-  } else {
-    rootDirStart_ = fbs->fat32RootCluster;
-    fatType_ = 32;
-  }
-  return true;
-
- fail:
-  return false;
-}
+/* Arduino SdFat Library
+ * Copyright (C) 2009 by William Greiman
+ *
+ * This file is part of the Arduino SdFat Library
+ *
+ * This Library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This Library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino SdFat Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+#include "Marlin.h"
+#ifdef SDSUPPORT
+
+#include "SdVolume.h"
+//------------------------------------------------------------------------------
+#if !USE_MULTIPLE_CARDS
+// raw block cache
+uint32_t SdVolume::cacheBlockNumber_;  // current block number
+cache_t  SdVolume::cacheBuffer_;       // 512 byte cache for Sd2Card
+Sd2Card* SdVolume::sdCard_;            // pointer to SD card object
+bool     SdVolume::cacheDirty_;        // cacheFlush() will write block if true
+uint32_t SdVolume::cacheMirrorBlock_;  // mirror  block for second FAT
+#endif  // USE_MULTIPLE_CARDS
+//------------------------------------------------------------------------------
+// find a contiguous group of clusters
+bool SdVolume::allocContiguous(uint32_t count, uint32_t* curCluster) {
+  // start of group
+  uint32_t bgnCluster;
+  // end of group
+  uint32_t endCluster;
+  // last cluster of FAT
+  uint32_t fatEnd = clusterCount_ + 1;
+
+  // flag to save place to start next search
+  bool setStart;
+
+  // set search start cluster
+  if (*curCluster) {
+    // try to make file contiguous
+    bgnCluster = *curCluster + 1;
+
+    // don't save new start location
+    setStart = false;
+  } else {
+    // start at likely place for free cluster
+    bgnCluster = allocSearchStart_;
+
+    // save next search start if one cluster
+    setStart = count == 1;
+  }
+  // end of group
+  endCluster = bgnCluster;
+
+  // search the FAT for free clusters
+  for (uint32_t n = 0;; n++, endCluster++) {
+    // can't find space checked all clusters
+    if (n >= clusterCount_) goto fail;
+
+    // past end - start from beginning of FAT
+    if (endCluster > fatEnd) {
+      bgnCluster = endCluster = 2;
+    }
+    uint32_t f;
+    if (!fatGet(endCluster, &f)) goto fail;
+
+    if (f != 0) {
+      // cluster in use try next cluster as bgnCluster
+      bgnCluster = endCluster + 1;
+    } else if ((endCluster - bgnCluster + 1) == count) {
+      // done - found space
+      break;
+    }
+  }
+  // mark end of chain
+  if (!fatPutEOC(endCluster)) goto fail;
+
+  // link clusters
+  while (endCluster > bgnCluster) {
+    if (!fatPut(endCluster - 1, endCluster)) goto fail;
+    endCluster--;
+  }
+  if (*curCluster != 0) {
+    // connect chains
+    if (!fatPut(*curCluster, bgnCluster)) goto fail;
+  }
+  // return first cluster number to caller
+  *curCluster = bgnCluster;
+
+  // remember possible next free cluster
+  if (setStart) allocSearchStart_ = bgnCluster + 1;
+
+  return true;
+
+ fail:
+  return false;
+}
+//------------------------------------------------------------------------------
+bool SdVolume::cacheFlush() {
+  if (cacheDirty_) {
+    if (!sdCard_->writeBlock(cacheBlockNumber_, cacheBuffer_.data)) {
+      goto fail;
+    }
+    // mirror FAT tables
+    if (cacheMirrorBlock_) {
+      if (!sdCard_->writeBlock(cacheMirrorBlock_, cacheBuffer_.data)) {
+        goto fail;
+      }
+      cacheMirrorBlock_ = 0;
+    }
+    cacheDirty_ = 0;
+  }
+  return true;
+
+ fail:
+  return false;
+}
+//------------------------------------------------------------------------------
+bool SdVolume::cacheRawBlock(uint32_t blockNumber, bool dirty) {
+  if (cacheBlockNumber_ != blockNumber) {
+    if (!cacheFlush()) goto fail;
+    if (!sdCard_->readBlock(blockNumber, cacheBuffer_.data)) goto fail;
+    cacheBlockNumber_ = blockNumber;
+  }
+  if (dirty) cacheDirty_ = true;
+  return true;
+
+ fail:
+  return false;
+}
+//------------------------------------------------------------------------------
+// return the size in bytes of a cluster chain
+bool SdVolume::chainSize(uint32_t cluster, uint32_t* size) {
+  uint32_t s = 0;
+  do {
+    if (!fatGet(cluster, &cluster)) goto fail;
+    s += 512UL << clusterSizeShift_;
+  } while (!isEOC(cluster));
+  *size = s;
+  return true;
+
+ fail:
+  return false;
+}
+//------------------------------------------------------------------------------
+// Fetch a FAT entry
+bool SdVolume::fatGet(uint32_t cluster, uint32_t* value) {
+  uint32_t lba;
+  if (cluster > (clusterCount_ + 1)) goto fail;
+  if (FAT12_SUPPORT && fatType_ == 12) {
+    uint16_t index = cluster;
+    index += index >> 1;
+    lba = fatStartBlock_ + (index >> 9);
+    if (!cacheRawBlock(lba, CACHE_FOR_READ)) goto fail;
+    index &= 0X1FF;
+    uint16_t tmp = cacheBuffer_.data[index];
+    index++;
+    if (index == 512) {
+      if (!cacheRawBlock(lba + 1, CACHE_FOR_READ)) goto fail;
+      index = 0;
+    }
+    tmp |= cacheBuffer_.data[index] << 8;
+    *value = cluster & 1 ? tmp >> 4 : tmp & 0XFFF;
+    return true;
+  }
+  if (fatType_ == 16) {
+    lba = fatStartBlock_ + (cluster >> 8);
+  } else if (fatType_ == 32) {
+    lba = fatStartBlock_ + (cluster >> 7);
+  } else {
+    goto fail;
+  }
+  if (lba != cacheBlockNumber_) {
+    if (!cacheRawBlock(lba, CACHE_FOR_READ)) goto fail;
+  }
+  if (fatType_ == 16) {
+    *value = cacheBuffer_.fat16[cluster & 0XFF];
+  } else {
+    *value = cacheBuffer_.fat32[cluster & 0X7F] & FAT32MASK;
+  }
+  return true;
+
+ fail:
+  return false;
+}
+//------------------------------------------------------------------------------
+// Store a FAT entry
+bool SdVolume::fatPut(uint32_t cluster, uint32_t value) {
+  uint32_t lba;
+  // error if reserved cluster
+  if (cluster < 2) goto fail;
+
+  // error if not in FAT
+  if (cluster > (clusterCount_ + 1)) goto fail;
+
+  if (FAT12_SUPPORT && fatType_ == 12) {
+    uint16_t index = cluster;
+    index += index >> 1;
+    lba = fatStartBlock_ + (index >> 9);
+    if (!cacheRawBlock(lba, CACHE_FOR_WRITE)) goto fail;
+    // mirror second FAT
+    if (fatCount_ > 1) cacheMirrorBlock_ = lba + blocksPerFat_;
+    index &= 0X1FF;
+    uint8_t tmp = value;
+    if (cluster & 1) {
+      tmp = (cacheBuffer_.data[index] & 0XF) | tmp << 4;
+    }
+    cacheBuffer_.data[index] = tmp;
+    index++;
+    if (index == 512) {
+      lba++;
+      index = 0;
+      if (!cacheRawBlock(lba, CACHE_FOR_WRITE)) goto fail;
+      // mirror second FAT
+      if (fatCount_ > 1) cacheMirrorBlock_ = lba + blocksPerFat_;
+    }
+    tmp = value >> 4;
+    if (!(cluster & 1)) {
+      tmp = ((cacheBuffer_.data[index] & 0XF0)) | tmp >> 4;
+    }
+    cacheBuffer_.data[index] = tmp;
+    return true;
+  }
+  if (fatType_ == 16) {
+    lba = fatStartBlock_ + (cluster >> 8);
+  } else if (fatType_ == 32) {
+    lba = fatStartBlock_ + (cluster >> 7);
+  } else {
+    goto fail;
+  }
+  if (!cacheRawBlock(lba, CACHE_FOR_WRITE)) goto fail;
+  // store entry
+  if (fatType_ == 16) {
+    cacheBuffer_.fat16[cluster & 0XFF] = value;
+  } else {
+    cacheBuffer_.fat32[cluster & 0X7F] = value;
+  }
+  // mirror second FAT
+  if (fatCount_ > 1) cacheMirrorBlock_ = lba + blocksPerFat_;
+  return true;
+
+ fail:
+  return false;
+}
+//------------------------------------------------------------------------------
+// free a cluster chain
+bool SdVolume::freeChain(uint32_t cluster) {
+  uint32_t next;
+
+  // clear free cluster location
+  allocSearchStart_ = 2;
+
+  do {
+    if (!fatGet(cluster, &next)) goto fail;
+
+    // free cluster
+    if (!fatPut(cluster, 0)) goto fail;
+
+    cluster = next;
+  } while (!isEOC(cluster));
+
+  return true;
+
+ fail:
+  return false;
+}
+//------------------------------------------------------------------------------
+/** Volume free space in clusters.
+ *
+ * \return Count of free clusters for success or -1 if an error occurs.
+ */
+int32_t SdVolume::freeClusterCount() {
+  uint32_t free = 0;
+  uint16_t n;
+  uint32_t todo = clusterCount_ + 2;
+
+  if (fatType_ == 16) {
+    n = 256;
+  } else if (fatType_ == 32) {
+    n = 128;
+  } else {
+    // put FAT12 here
+    return -1;
+  }
+
+  for (uint32_t lba = fatStartBlock_; todo; todo -= n, lba++) {
+    if (!cacheRawBlock(lba, CACHE_FOR_READ)) return -1;
+    if (todo < n) n = todo;
+    if (fatType_ == 16) {
+      for (uint16_t i = 0; i < n; i++) {
+        if (cacheBuffer_.fat16[i] == 0) free++;
+      }
+    } else {
+      for (uint16_t i = 0; i < n; i++) {
+        if (cacheBuffer_.fat32[i] == 0) free++;
+      }
+    }
+  }
+  return free;
+}
+//------------------------------------------------------------------------------
+/** Initialize a FAT volume.
+ *
+ * \param[in] dev The SD card where the volume is located.
+ *
+ * \param[in] part The partition to be used.  Legal values for \a part are
+ * 1-4 to use the corresponding partition on a device formatted with
+ * a MBR, Master Boot Record, or zero if the device is formatted as
+ * a super floppy with the FAT boot sector in block zero.
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.  Reasons for
+ * failure include not finding a valid partition, not finding a valid
+ * FAT file system in the specified partition or an I/O error.
+ */
+bool SdVolume::init(Sd2Card* dev, uint8_t part) {
+  uint32_t totalBlocks;
+  uint32_t volumeStartBlock = 0;
+  fat32_boot_t* fbs;
+
+  sdCard_ = dev;
+  fatType_ = 0;
+  allocSearchStart_ = 2;
+  cacheDirty_ = 0;  // cacheFlush() will write block if true
+  cacheMirrorBlock_ = 0;
+  cacheBlockNumber_ = 0XFFFFFFFF;
+
+  // if part == 0 assume super floppy with FAT boot sector in block zero
+  // if part > 0 assume mbr volume with partition table
+  if (part) {
+    if (part > 4)goto fail;
+    if (!cacheRawBlock(volumeStartBlock, CACHE_FOR_READ)) goto fail;
+    part_t* p = &cacheBuffer_.mbr.part[part-1];
+    if ((p->boot & 0X7F) !=0  ||
+      p->totalSectors < 100 ||
+      p->firstSector == 0) {
+      // not a valid partition
+      goto fail;
+    }
+    volumeStartBlock = p->firstSector;
+  }
+  if (!cacheRawBlock(volumeStartBlock, CACHE_FOR_READ)) goto fail;
+  fbs = &cacheBuffer_.fbs32;
+  if (fbs->bytesPerSector != 512 ||
+    fbs->fatCount == 0 ||
+    fbs->reservedSectorCount == 0 ||
+    fbs->sectorsPerCluster == 0) {
+       // not valid FAT volume
+      goto fail;
+  }
+  fatCount_ = fbs->fatCount;
+  blocksPerCluster_ = fbs->sectorsPerCluster;
+  // determine shift that is same as multiply by blocksPerCluster_
+  clusterSizeShift_ = 0;
+  while (blocksPerCluster_ != (1 << clusterSizeShift_)) {
+    // error if not power of 2
+    if (clusterSizeShift_++ > 7) goto fail;
+  }
+  blocksPerFat_ = fbs->sectorsPerFat16 ?
+                    fbs->sectorsPerFat16 : fbs->sectorsPerFat32;
+
+  fatStartBlock_ = volumeStartBlock + fbs->reservedSectorCount;
+
+  // count for FAT16 zero for FAT32
+  rootDirEntryCount_ = fbs->rootDirEntryCount;
+
+  // directory start for FAT16 dataStart for FAT32
+  rootDirStart_ = fatStartBlock_ + fbs->fatCount * blocksPerFat_;
+
+  // data start for FAT16 and FAT32
+  dataStartBlock_ = rootDirStart_ + ((32 * fbs->rootDirEntryCount + 511)/512);
+
+  // total blocks for FAT16 or FAT32
+  totalBlocks = fbs->totalSectors16 ?
+                           fbs->totalSectors16 : fbs->totalSectors32;
+  // total data blocks
+  clusterCount_ = totalBlocks - (dataStartBlock_ - volumeStartBlock);
+
+  // divide by cluster size to get cluster count
+  clusterCount_ >>= clusterSizeShift_;
+
+  // FAT type is determined by cluster count
+  if (clusterCount_ < 4085) {
+    fatType_ = 12;
+    if (!FAT12_SUPPORT) goto fail;
+  } else if (clusterCount_ < 65525) {
+    fatType_ = 16;
+  } else {
+    rootDirStart_ = fbs->fat32RootCluster;
+    fatType_ = 32;
+  }
+  return true;
+
+ fail:
+  return false;
+}
 #endif

+ 283 - 283
Firmware/language_it.h

@@ -1,283 +1,283 @@
-#define WELCOME_MSG                         CUSTOM_MENDEL_NAME " pronta."
-#define MSG_SD_INSERTED                     "SD inserita"
-#define MSG_SD_REMOVED                      "SD rimossa"
-#define MSG_MAIN                            "Menu principale"
-#define MSG_DISABLE_STEPPERS                "Disabilit motori"
-#define MSG_AUTO_HOME                       "Trova origine"
-#define MSG_SET_HOME_OFFSETS                "Set home offsets"
-#define MSG_SET_ORIGIN                      "Set origin"
-#define MSG_COOLDOWN                        "Raffredda"
-#define MSG_SWITCH_PS_ON                    "Switch power on"
-#define MSG_SWITCH_PS_OFF                   "Switch power off"
-#define MSG_MOVE_AXIS                       "Muovi asse"
-#define MSG_MOVE_X                          "Muovi X"
-#define MSG_MOVE_Y                          "Muovi Y"
-#define MSG_MOVE_Z                          "Muovi Z"
-#define MSG_MOVE_E                          "Muovi Estrusore"
-#define MSG_MOVE_01MM                       "Move 0.1mm"
-#define MSG_MOVE_1MM                        "Move 1mm"
-#define MSG_MOVE_10MM                       "Move 10mm"
-#define MSG_SPEED                           "Velocita"
-#define MSG_NOZZLE                          "Ugello"
-#define MSG_NOZZLE1                         "Nozzle2"
-#define MSG_NOZZLE2                         "Nozzle3"
-#define MSG_BED                             "Letto"
-#define MSG_FAN_SPEED                       "Velocita vent."
-#define MSG_FLOW                            "Flusso"
-#define MSG_TEMPERATURE                     "Temperatura"
-#define MSG_MOTION                          "Motion"
-#define MSG_VOLUMETRIC                      "Filament"
-#define MSG_VOLUMETRIC_ENABLED		    	"E in mm3"
-#define MSG_STORE_EPROM                     "Store memory"
-#define MSG_LOAD_EPROM                      "Load memory"
-#define MSG_RESTORE_FAILSAFE                "Restore failsafe"
-#define MSG_REFRESH                         "\xF8" "Refresh"
-#define MSG_WATCH                           "Schermata info"
-#define MSG_TUNE                            "Regola"
-#define MSG_PAUSE_PRINT                     "Metti in pausa"
-#define MSG_RESUME_PRINT                    "Riprendi stampa"
-#define MSG_STOP_PRINT                      "Arresta stampa"
-#define MSG_CARD_MENU                       "Stampa da SD"
-#define MSG_NO_CARD                         "Nessuna SD"
-#define MSG_DWELL                           "Sospensione..."
-#define MSG_USERWAIT                        "Attendendo utente"
-#define MSG_RESUMING                        "Riprendi stampa"
-#define MSG_PRINT_ABORTED                   "Stampa abortita"
-#define MSG_NO_MOVE                         "Nessun movimento."
-#define MSG_KILLED                          "IN TILT."
-#define MSG_STOPPED                         "ARRESTATO."
-#define MSG_FILAMENTCHANGE                  "Camb. filamento"
-#define MSG_INIT_SDCARD                     "Init. SD card"
-#define MSG_CNG_SDCARD                      "Change SD card"
-#define MSG_ZPROBE_OUT                      "Z probe out. bed"
-#define MSG_POSITION_UNKNOWN                "Home X/Y before Z"
-#define MSG_ZPROBE_ZOFFSET                  "Z Offset"
-#define MSG_BABYSTEP_X                      "Babystep X"
-#define MSG_BABYSTEP_Y                      "Babystep Y"
-#define MSG_BABYSTEP_Z                      "Compensazione Z"
-#define MSG_ADJUSTZ			    			"Autoregolare Z?"
-#define MSG_PICK_Z			    			"Pick print"
-
-#define MSG_SETTINGS                        "Impostazioni"
-#define MSG_PREHEAT                         "Preriscalda"
-#define MSG_HEATING                         "Riscaldamento..."
-#define MSG_SUPPORT 						"Support"
-#define MSG_YES								"Si"
-#define MSG_NO								"No"
-#define MSG_NOT_LOADED 						"Fil. non caricato"
-#define MSG_NOT_COLOR 						"Colore non puro"
-#define MSG_LOADING_COLOR					"Caricando colore"
-#define MSG_CHANGE_SUCCESS					"Cambio riuscito!"
-#define MSG_PRESS							"e cliccare manopola"
-#define MSG_INSERT_FILAMENT					"Inserire filamento"
-#define MSG_CHANGING_FILAMENT				"Cambiando filam."
-
-#define MSG_PLEASE_WAIT						"Aspetta"
-#define MSG_PREHEAT_NOZZLE                  "Preris. ugello!"
-#define MSG_HEATING_COMPLETE                "Riscald. completo"
-#define MSG_BED_HEATING                     "Riscald. letto"
-#define MSG_BED_DONE                        "Piatto fatto."
-#define MSG_ERROR                        	"ERRORE:"
-#define MSG_CORRECTLY						"Cambiato corr.?"
-#define MSG_LOADING_FILAMENT				"Caricando filam."
-#define MSG_UNLOAD_FILAMENT                 "Scarica filamento"
-#define MSG_LOAD_FILAMENT                   "Carica filamento"
-
-#define MSG_SILENT_MODE_ON		    "Modo [silenzioso]"
-#define MSG_SILENT_MODE_OFF		    "Mode      [forte]" 
-#define MSG_REBOOT			    "Riavvia stampante"
-#define MSG_TAKE_EFFECT			    " per attualizzare"
-
-#define MSG_Enqueing                        "enqueing \""
-#define MSG_POWERUP                         "PowerUp"
-#define MSG_CONFIGURATION_VER               " Last Updated: "
-#define MSG_FREE_MEMORY                     " Free Memory: "
-#define MSG_PLANNER_BUFFER_BYTES            "  PlannerBufferBytes: "
-#define MSG_OK                              "ok"
-#define MSG_ERR_CHECKSUM_MISMATCH           "checksum mismatch, Last Line: "
-#define MSG_ERR_NO_CHECKSUM                 "No Checksum with line number, Last Line: "
-#define MSG_BEGIN_FILE_LIST                 "Begin file list"
-#define MSG_END_FILE_LIST                   "End file list"
-#define MSG_M104_INVALID_EXTRUDER           "M104 Invalid extruder "
-#define MSG_M105_INVALID_EXTRUDER           "M105 Invalid extruder "
-#define MSG_M200_INVALID_EXTRUDER           "M200 Invalid extruder "
-#define MSG_M218_INVALID_EXTRUDER           "M218 Invalid extruder "
-#define MSG_M221_INVALID_EXTRUDER           "M221 Invalid extruder "
-#define MSG_ERR_NO_THERMISTORS              "No thermistors - no temperature"
-#define MSG_M109_INVALID_EXTRUDER           "M109 Invalid extruder "
-#define MSG_M115_REPORT                     "FIRMWARE_NAME:Marlin V1.0.2; Sprinter/grbl mashup for gen6 FIRMWARE_URL:" FIRMWARE_URL " PROTOCOL_VERSION:" PROTOCOL_VERSION " MACHINE_TYPE:" CUSTOM_MENDEL_NAME " EXTRUDER_COUNT:" STRINGIFY(EXTRUDERS) " UUID:" MACHINE_UUID "\n"
-#define MSG_ERR_KILLED                      "Printer halted. kill() called!"
-#define MSG_ERR_STOPPED                     "Printer stopped due to errors. Fix the error and use M999 to restart. (Temperature is reset. Set it after restarting)"
-#define MSG_RESEND                          "Resend: "
-#define MSG_M119_REPORT                     "Reporting endstop status"
-#define MSG_ENDSTOP_HIT                     "TRIGGERED"
-#define MSG_ENDSTOP_OPEN                    "open"
-#define MSG_SD_CANT_OPEN_SUBDIR             "Cannot open subdir"
-#define MSG_SD_INIT_FAIL                    "SD init fail"
-#define MSG_SD_VOL_INIT_FAIL                "volume.init failed"
-#define MSG_SD_OPENROOT_FAIL                "openRoot failed"
-#define MSG_SD_CARD_OK                      "SD card ok"
-#define MSG_SD_WORKDIR_FAIL                 "workDir open failed"
-#define MSG_SD_OPEN_FILE_FAIL               "open failed, File: "
-#define MSG_SD_FILE_OPENED                  "File opened: "
-#define MSG_SD_FILE_SELECTED                "File selected"
-#define MSG_SD_WRITE_TO_FILE                "Writing to file: "
-#define MSG_SD_PRINTING_BYTE                "SD printing byte "
-#define MSG_SD_NOT_PRINTING                 "Not SD printing"
-#define MSG_SD_ERR_WRITE_TO_FILE            "error writing to file"
-#define MSG_SD_CANT_ENTER_SUBDIR            "Cannot enter subdir: "
-#define MSG_STEPPER_TOO_HIGH                "Steprate too high: "
-#define MSG_ENDSTOPS_HIT                    "endstops hit: "
-#define MSG_ERR_COLD_EXTRUDE_STOP           " cold extrusion prevented"
-#define MSG_BABYSTEPPING_X                  "Babystepping X"
-#define MSG_BABYSTEPPING_Y                  "Babystepping Y"
-#define MSG_BABYSTEPPING_Z                  "Adjusting Z"
-#define MSG_SERIAL_ERROR_MENU_STRUCTURE     "Error in menu structure"
-
-#define MSG_LANGUAGE_NAME		    "Italiano"
-#define MSG_LANGUAGE_SELECT		    "Seleziona lingua"
-#define MSG_PRUSA3D			    "prusa3d.com"
-#define MSG_PRUSA3D_FORUM		    "forum.prusa3d.com"
-#define MSG_PRUSA3D_HOWTO		    "howto.prusa3d.com"
-
-#define MSG_SELFTEST_ERROR		    "Autotest negativo"
-#define MSG_SELFTEST_PLEASECHECK	    "Verificare:"	
-#define MSG_SELFTEST_NOTCONNECTED	    "Non connesso"
-#define MSG_SELFTEST_HEATERTHERMISTOR	    "Riscald./Termist."
-#define MSG_SELFTEST_BEDHEATER		    "Letto/Riscald."
-#define MSG_SELFTEST_WIRINGERROR	    "Errore cablaggio"
-#define MSG_SELFTEST_ENDSTOPS		    "Finecorsa (2)"
-#define MSG_SELFTEST_MOTOR		    "Motore"
-#define MSG_SELFTEST_ENDSTOP		    "Finecorsa"
-#define MSG_SELFTEST_ENDSTOP_NOTHIT	    "Finec. fuori por."
-#define MSG_SELFTEST_OK			    "Autotest OK"
-
-#define(length=20) MSG_SELFTEST_FAN					"Prova del ventilator";
-#define(length=20) MSG_SELFTEST_COOLING_FAN			"Vent di stampa ant.?";
-#define(length=20) MSG_SELFTEST_EXTRUDER_FAN        "Vent SX sull'ugello?";
-#define MSG_SELFTEST_FAN_YES				"Gira";
-#define MSG_SELFTEST_FAN_NO					"Non si gira";
-
-#define MSG_STATS_TOTALFILAMENT		    "Filamento tot:"
-#define MSG_STATS_TOTALPRINTTIME	    "Tempo stampa tot:"
-#define MSG_STATS_FILAMENTUSED		    "Filamento usato:"
-#define MSG_STATS_PRINTTIME		    "Tempo di stampa:"
-#define MSG_SELFTEST_START		    "Avvia autotest"
-#define MSG_SELFTEST_CHECK_ENDSTOPS	    "Verifica finecorsa"
-#define MSG_SELFTEST_CHECK_HOTEND	    "Verifica ugello"  
-#define MSG_SELFTEST_CHECK_X		    "Verifica asse X"
-#define MSG_SELFTEST_CHECK_Y		    "Verifica asse Y"
-#define MSG_SELFTEST_CHECK_Z		    "Verifica asse Z"
-#define MSG_SELFTEST_CHECK_BED		    "Verifica letto"
-#define MSG_SELFTEST_CHECK_ALLCORRECT	    "Nessun errore"
-#define MSG_SELFTEST			    "Autotest"
-#define MSG_SELFTEST_FAILED		    "Autotest fallito"
-#define MSG_STATISTICS			    "Statistiche"
-#define MSG_USB_PRINTING		    "Stampa da USB"
-#define MSG_HOMEYZ                          "Calibra Z"
-#define MSG_HOMEYZ_PROGRESS                 "Calibrando Z"
-#define MSG_HOMEYZ_DONE		            "Calibrazione OK"
-
-
-
-#define MSG_SHOW_END_STOPS					"Stato finecorsa"
-#define MSG_CALIBRATE_BED					"Calibra XYZ"
-#define MSG_CALIBRATE_BED_RESET					"Reset XYZ calibr."
-
-#define MSG_MOVE_CARRIAGE_TO_THE_TOP 				"Calibrazione XYZ. Ruotare la manopola per alzare il carrello Z fino all'altezza massima. Click per terminare."
-#define MSG_MOVE_CARRIAGE_TO_THE_TOP_Z 				"Calibrazione Z. Ruotare la manopola per alzare il carrello Z fino all'altezza massima. Click per terminare."
-
-#define MSG_CONFIRM_NOZZLE_CLEAN					"Pulire l'ugello per la calibrazione, poi fare click."
-#define MSG_CONFIRM_CARRIAGE_AT_THE_TOP				"I carrelli Z sin/des sono altezza max?"
-
-#define MSG_FIND_BED_OFFSET_AND_SKEW_LINE1			"Ricerca del letto punto di calibraz."
-#define MSG_FIND_BED_OFFSET_AND_SKEW_LINE2			" su 4"
-#define MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE1		"Perfezion. il letto punto di calibraz."
-#define MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE2		" su 9"
-#define MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE1		"Misurare l'altezza di riferimento del punto di calibrazione"
-#define MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE2		" su 9"
-#define MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION	"Reiterazione "
-
-#define MSG_BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND		"Calibrazione XYZ fallita. Il punto di calibrazione sul letto non e' stato trovato."
-#define MSG_BED_SKEW_OFFSET_DETECTION_FITTING_FAILED		"Calibrazione XYZ fallita. Si prega di consultare il manuale."
-#define MSG_BED_SKEW_OFFSET_DETECTION_PERFECT			"Calibrazione XYZ OK. Gli assi X/Y sono perpendicolari. Complimenti!"
-#define MSG_BED_SKEW_OFFSET_DETECTION_SKEW_MILD			"Calibrazion XYZ corretta. Assi X/Y leggermente storti. Ben fatto!"
-#define MSG_BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME		"Calibrazion XYZ corretta. La distorsione verra' automaticamente compensata."
-#define MSG_BED_SKEW_OFFSET_DETECTION_FAILED_FRONT_LEFT_FAR	"Calibrazione XYZ fallita. Punto anteriore sinistro non raggiungibile."
-#define MSG_BED_SKEW_OFFSET_DETECTION_FAILED_FRONT_RIGHT_FAR	"Calibrazione XYZ fallita. Punto anteriore destro non raggiungibile."
-#define MSG_BED_SKEW_OFFSET_DETECTION_FAILED_FRONT_BOTH_FAR	"Calibrazione XYZ fallita. Punti anteriori non raggiungibili."
-#define MSG_BED_SKEW_OFFSET_DETECTION_WARNING_FRONT_LEFT_FAR	"Calibrazione XYZ compromessa. Punto anteriore sinistro non raggiungibile."
-#define MSG_BED_SKEW_OFFSET_DETECTION_WARNING_FRONT_RIGHT_FAR	"Calibrazione XYZ compromessa. Punto anteriore destro non raggiungibile."
-#define MSG_BED_SKEW_OFFSET_DETECTION_WARNING_FRONT_BOTH_FAR	"Calibrazione XYZ compromessa. Punti anteriori non raggiungibili."
-
-#define MSG_BED_LEVELING_FAILED_POINT_LOW				"Livellamento letto fallito.NoRispSensor Residui su ugello? In attesa di reset."
-#define MSG_BED_LEVELING_FAILED_POINT_HIGH				"Livellamento letto fallito.Risp sensore troppo prestoIn attesa di reset."
-#define MSG_BED_LEVELING_FAILED_PROBE_DISCONNECTED		"Livellamento letto fallito. Sensore discon. o Cavo Dann. In attesa di reset."
-
-#define MSG_NEW_FIRMWARE_AVAILABLE						"Nuova versione del firmware disponibile"
-#define MSG_NEW_FIRMWARE_PLEASE_UPGRADE					"Prega aggiorna."
-
-#define MSG_FOLLOW_CALIBRATION_FLOW						"Stampante ancora non calibrata. Si prega di seguire il manuale, capitolo PRIMI PASSI, sezione della calibrazione."
-#define MSG_BABYSTEP_Z_NOT_SET							"Distanza tra la punta dell'ugello e la superficie del letto non ancora imposta. Si prega di seguire il manuale, capitolo First steps, sezione First layer calibration."
-
-#define MSG_BED_CORRECTION_MENU							"Correz. liv.letto"
-#define MSG_BED_CORRECTION_LEFT							"Sinistra  [um]"
-#define MSG_BED_CORRECTION_RIGHT						"Destra    [um]"
-#define MSG_BED_CORRECTION_FRONT						"Fronte    [um]"
-#define MSG_BED_CORRECTION_REAR							"Retro     [um]"
-#define MSG_BED_CORRECTION_RESET						"Reset"			
-
-#define MSG_MESH_BED_LEVELING							"Mesh livel. letto"
-#define MSG_MENU_CALIBRATION							"Calibrazione"
-#define MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_OFF					"SD card [normal]"
-#define MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_ON					"SD card [FlshAir]"
-
-#define MSG_LOOSE_PULLEY								"Puleggia lenta"
-#define MSG_FILAMENT_LOADING_T0							"Inserire filamento nell'estrusore 1. Click per continuare."
-#define MSG_FILAMENT_LOADING_T1							"Inserire filamento nell'estrusore 2. Click per continuare."
-#define MSG_FILAMENT_LOADING_T2							"Inserire filamento nell'estrusore 3. Click per continuare."
-#define MSG_FILAMENT_LOADING_T3							"Inserire filamento nell'estrusore 4. Click per continuare."
-#define MSG_CHANGE_EXTR									"Cambio estrusore."
-
-#define MSG_FIL_ADJUSTING								"Filamento in fase di regolazione. Attendere prego."
-#define MSG_CONFIRM_NOZZLE_CLEAN_FIL_ADJ				"I filamenti sono regolati. Si prega di pulire l'ugello per la calibrazione. Click per continuare."
-#define MSG_CALIBRATE_E									"Calibra E"
-#define MSG_E_CAL_KNOB									"Girare la manopola affinche' il segno raggiunga il corpo dell'estrusore. Click per continuare."
-#define MSG_MARK_FIL									"Segnare il filamento a 100 mm di distanza dal corpo dell'estrusore. Click per continuare."
-#define MSG_CLEAN_NOZZLE_E								"Calibrazione E terminata. Si prega di pulire l'ugello. Click per continuare."
-#define MSG_WAITING_TEMP								"In attesa del raffreddamento della testina e del piatto"
-#define MSG_FILAMENT_CLEAN								"Il colore e' nitido?"
-#define MSG_UNLOADING_FILAMENT							"Rilasc. filamento"
-#define MSG_PAPER										"Porre un foglio sotto l'ugello durante la calibrazione dei primi 4 punti. In caso l'ugello muova il foglio spegnere prontamente la stampante."
-
-#define MSG_FINISHING_MOVEMENTS							"Arresto in corso"
-#define MSG_PRINT_PAUSED								"Stampa in pausa"
-#define MSG_RESUMING_PRINT								"Stampa in ripresa"
-#define MSG_PID_EXTRUDER								"Calibrazione PID"
-#define MSG_SET_TEMPERATURE								"Imposta temperatura"
-#define MSG_PID_FINISHED								"Cal. PID completa"
-#define MSG_PID_RUNNING									"Cal. PID"
-
-#define MSG_CALIBRATE_PINDA								"Calibrare"
-#define MSG_CALIBRATION_PINDA_MENU						"Taratura temp."
-#define MSG_PINDA_NOT_CALIBRATED						"Taratura della temperatura non ancora eseguita"
-#define MSG_PINDA_PREHEAT								"Riscald. PINDA"
-#define MSG_TEMP_CALIBRATION							"Cal. temp.          "
-#define MSG_TEMP_CALIBRATION_DONE						"Taratura temperatura terminata. Fare click per continuare."
-#define MSG_TEMP_CALIBRATION_ON							"Cal. temp. [ON]"
-#define MSG_TEMP_CALIBRATION_OFF						"Cal. temp. [OFF]"
-
-#define MSG_LOAD_ALL									"Caricare tutti"
-#define MSG_LOAD_FILAMENT_1								"Caricare fil. 1"
-#define MSG_LOAD_FILAMENT_2								"Caricare fil. 2"
-#define MSG_LOAD_FILAMENT_3								"Caricare fil. 3"
-#define MSG_LOAD_FILAMENT_4								"Caricare fil. 4"
-#define MSG_UNLOAD_FILAMENT_1							"Rilasciare fil. 1"
-#define MSG_UNLOAD_FILAMENT_2							"Rilasciare fil. 1"
-#define MSG_UNLOAD_FILAMENT_3							"Rilasciare fil. 1"
-#define MSG_UNLOAD_FILAMENT_4							"Rilasciare fil. 1"
-#define MSG_UNLOAD_ALL									"Rilasciare tutti"
-#define MSG_PREPARE_FILAMENT							"Preparare filamento"
-#define MSG_ALL											"Tutti"
-#define MSG_USED										"Usati nella stampa"
-#define MSG_CURRENT										"Attuale"
+#define WELCOME_MSG                         CUSTOM_MENDEL_NAME " pronta."
+#define MSG_SD_INSERTED                     "SD inserita"
+#define MSG_SD_REMOVED                      "SD rimossa"
+#define MSG_MAIN                            "Menu principale"
+#define MSG_DISABLE_STEPPERS                "Disabilit motori"
+#define MSG_AUTO_HOME                       "Trova origine"
+#define MSG_SET_HOME_OFFSETS                "Set home offsets"
+#define MSG_SET_ORIGIN                      "Set origin"
+#define MSG_COOLDOWN                        "Raffredda"
+#define MSG_SWITCH_PS_ON                    "Switch power on"
+#define MSG_SWITCH_PS_OFF                   "Switch power off"
+#define MSG_MOVE_AXIS                       "Muovi asse"
+#define MSG_MOVE_X                          "Muovi X"
+#define MSG_MOVE_Y                          "Muovi Y"
+#define MSG_MOVE_Z                          "Muovi Z"
+#define MSG_MOVE_E                          "Muovi Estrusore"
+#define MSG_MOVE_01MM                       "Move 0.1mm"
+#define MSG_MOVE_1MM                        "Move 1mm"
+#define MSG_MOVE_10MM                       "Move 10mm"
+#define MSG_SPEED                           "Velocita"
+#define MSG_NOZZLE                          "Ugello"
+#define MSG_NOZZLE1                         "Nozzle2"
+#define MSG_NOZZLE2                         "Nozzle3"
+#define MSG_BED                             "Letto"
+#define MSG_FAN_SPEED                       "Velocita vent."
+#define MSG_FLOW                            "Flusso"
+#define MSG_TEMPERATURE                     "Temperatura"
+#define MSG_MOTION                          "Motion"
+#define MSG_VOLUMETRIC                      "Filament"
+#define MSG_VOLUMETRIC_ENABLED		    	"E in mm3"
+#define MSG_STORE_EPROM                     "Store memory"
+#define MSG_LOAD_EPROM                      "Load memory"
+#define MSG_RESTORE_FAILSAFE                "Restore failsafe"
+#define MSG_REFRESH                         "\xF8" "Refresh"
+#define MSG_WATCH                           "Schermata info"
+#define MSG_TUNE                            "Regola"
+#define MSG_PAUSE_PRINT                     "Metti in pausa"
+#define MSG_RESUME_PRINT                    "Riprendi stampa"
+#define MSG_STOP_PRINT                      "Arresta stampa"
+#define MSG_CARD_MENU                       "Stampa da SD"
+#define MSG_NO_CARD                         "Nessuna SD"
+#define MSG_DWELL                           "Sospensione..."
+#define MSG_USERWAIT                        "Attendendo utente"
+#define MSG_RESUMING                        "Riprendi stampa"
+#define MSG_PRINT_ABORTED                   "Stampa abortita"
+#define MSG_NO_MOVE                         "Nessun movimento."
+#define MSG_KILLED                          "IN TILT."
+#define MSG_STOPPED                         "ARRESTATO."
+#define MSG_FILAMENTCHANGE                  "Camb. filamento"
+#define MSG_INIT_SDCARD                     "Init. SD card"
+#define MSG_CNG_SDCARD                      "Change SD card"
+#define MSG_ZPROBE_OUT                      "Z probe out. bed"
+#define MSG_POSITION_UNKNOWN                "Home X/Y before Z"
+#define MSG_ZPROBE_ZOFFSET                  "Z Offset"
+#define MSG_BABYSTEP_X                      "Babystep X"
+#define MSG_BABYSTEP_Y                      "Babystep Y"
+#define MSG_BABYSTEP_Z                      "Compensazione Z"
+#define MSG_ADJUSTZ			    			"Autoregolare Z?"
+#define MSG_PICK_Z			    			"Pick print"
+
+#define MSG_SETTINGS                        "Impostazioni"
+#define MSG_PREHEAT                         "Preriscalda"
+#define MSG_HEATING                         "Riscaldamento..."
+#define MSG_SUPPORT 						"Support"
+#define MSG_YES								"Si"
+#define MSG_NO								"No"
+#define MSG_NOT_LOADED 						"Fil. non caricato"
+#define MSG_NOT_COLOR 						"Colore non puro"
+#define MSG_LOADING_COLOR					"Caricando colore"
+#define MSG_CHANGE_SUCCESS					"Cambio riuscito!"
+#define MSG_PRESS							"e cliccare manopola"
+#define MSG_INSERT_FILAMENT					"Inserire filamento"
+#define MSG_CHANGING_FILAMENT				"Cambiando filam."
+
+#define MSG_PLEASE_WAIT						"Aspetta"
+#define MSG_PREHEAT_NOZZLE                  "Preris. ugello!"
+#define MSG_HEATING_COMPLETE                "Riscald. completo"
+#define MSG_BED_HEATING                     "Riscald. letto"
+#define MSG_BED_DONE                        "Piatto fatto."
+#define MSG_ERROR                        	"ERRORE:"
+#define MSG_CORRECTLY						"Cambiato corr.?"
+#define MSG_LOADING_FILAMENT				"Caricando filam."
+#define MSG_UNLOAD_FILAMENT                 "Scarica filamento"
+#define MSG_LOAD_FILAMENT                   "Carica filamento"
+
+#define MSG_SILENT_MODE_ON		    "Modo [silenzioso]"
+#define MSG_SILENT_MODE_OFF		    "Mode      [forte]" 
+#define MSG_REBOOT			    "Riavvia stampante"
+#define MSG_TAKE_EFFECT			    " per attualizzare"
+
+#define MSG_Enqueing                        "enqueing \""
+#define MSG_POWERUP                         "PowerUp"
+#define MSG_CONFIGURATION_VER               " Last Updated: "
+#define MSG_FREE_MEMORY                     " Free Memory: "
+#define MSG_PLANNER_BUFFER_BYTES            "  PlannerBufferBytes: "
+#define MSG_OK                              "ok"
+#define MSG_ERR_CHECKSUM_MISMATCH           "checksum mismatch, Last Line: "
+#define MSG_ERR_NO_CHECKSUM                 "No Checksum with line number, Last Line: "
+#define MSG_BEGIN_FILE_LIST                 "Begin file list"
+#define MSG_END_FILE_LIST                   "End file list"
+#define MSG_M104_INVALID_EXTRUDER           "M104 Invalid extruder "
+#define MSG_M105_INVALID_EXTRUDER           "M105 Invalid extruder "
+#define MSG_M200_INVALID_EXTRUDER           "M200 Invalid extruder "
+#define MSG_M218_INVALID_EXTRUDER           "M218 Invalid extruder "
+#define MSG_M221_INVALID_EXTRUDER           "M221 Invalid extruder "
+#define MSG_ERR_NO_THERMISTORS              "No thermistors - no temperature"
+#define MSG_M109_INVALID_EXTRUDER           "M109 Invalid extruder "
+#define MSG_M115_REPORT                     "FIRMWARE_NAME:Marlin V1.0.2; Sprinter/grbl mashup for gen6 FIRMWARE_URL:" FIRMWARE_URL " PROTOCOL_VERSION:" PROTOCOL_VERSION " MACHINE_TYPE:" CUSTOM_MENDEL_NAME " EXTRUDER_COUNT:" STRINGIFY(EXTRUDERS) " UUID:" MACHINE_UUID "\n"
+#define MSG_ERR_KILLED                      "Printer halted. kill() called!"
+#define MSG_ERR_STOPPED                     "Printer stopped due to errors. Fix the error and use M999 to restart. (Temperature is reset. Set it after restarting)"
+#define MSG_RESEND                          "Resend: "
+#define MSG_M119_REPORT                     "Reporting endstop status"
+#define MSG_ENDSTOP_HIT                     "TRIGGERED"
+#define MSG_ENDSTOP_OPEN                    "open"
+#define MSG_SD_CANT_OPEN_SUBDIR             "Cannot open subdir"
+#define MSG_SD_INIT_FAIL                    "SD init fail"
+#define MSG_SD_VOL_INIT_FAIL                "volume.init failed"
+#define MSG_SD_OPENROOT_FAIL                "openRoot failed"
+#define MSG_SD_CARD_OK                      "SD card ok"
+#define MSG_SD_WORKDIR_FAIL                 "workDir open failed"
+#define MSG_SD_OPEN_FILE_FAIL               "open failed, File: "
+#define MSG_SD_FILE_OPENED                  "File opened: "
+#define MSG_SD_FILE_SELECTED                "File selected"
+#define MSG_SD_WRITE_TO_FILE                "Writing to file: "
+#define MSG_SD_PRINTING_BYTE                "SD printing byte "
+#define MSG_SD_NOT_PRINTING                 "Not SD printing"
+#define MSG_SD_ERR_WRITE_TO_FILE            "error writing to file"
+#define MSG_SD_CANT_ENTER_SUBDIR            "Cannot enter subdir: "
+#define MSG_STEPPER_TOO_HIGH                "Steprate too high: "
+#define MSG_ENDSTOPS_HIT                    "endstops hit: "
+#define MSG_ERR_COLD_EXTRUDE_STOP           " cold extrusion prevented"
+#define MSG_BABYSTEPPING_X                  "Babystepping X"
+#define MSG_BABYSTEPPING_Y                  "Babystepping Y"
+#define MSG_BABYSTEPPING_Z                  "Adjusting Z"
+#define MSG_SERIAL_ERROR_MENU_STRUCTURE     "Error in menu structure"
+
+#define MSG_LANGUAGE_NAME		    "Italiano"
+#define MSG_LANGUAGE_SELECT		    "Seleziona lingua"
+#define MSG_PRUSA3D			    "prusa3d.com"
+#define MSG_PRUSA3D_FORUM		    "forum.prusa3d.com"
+#define MSG_PRUSA3D_HOWTO		    "howto.prusa3d.com"
+
+#define MSG_SELFTEST_ERROR		    "Autotest negativo"
+#define MSG_SELFTEST_PLEASECHECK	    "Verificare:"	
+#define MSG_SELFTEST_NOTCONNECTED	    "Non connesso"
+#define MSG_SELFTEST_HEATERTHERMISTOR	    "Riscald./Termist."
+#define MSG_SELFTEST_BEDHEATER		    "Letto/Riscald."
+#define MSG_SELFTEST_WIRINGERROR	    "Errore cablaggio"
+#define MSG_SELFTEST_ENDSTOPS		    "Finecorsa (2)"
+#define MSG_SELFTEST_MOTOR		    "Motore"
+#define MSG_SELFTEST_ENDSTOP		    "Finecorsa"
+#define MSG_SELFTEST_ENDSTOP_NOTHIT	    "Finec. fuori por."
+#define MSG_SELFTEST_OK			    "Autotest OK"
+
+#define(length=20) MSG_SELFTEST_FAN					"Prova del ventilator";
+#define(length=20) MSG_SELFTEST_COOLING_FAN			"Vent di stampa ant.?";
+#define(length=20) MSG_SELFTEST_EXTRUDER_FAN        "Vent SX sull'ugello?";
+#define MSG_SELFTEST_FAN_YES				"Gira";
+#define MSG_SELFTEST_FAN_NO					"Non si gira";
+
+#define MSG_STATS_TOTALFILAMENT		    "Filamento tot:"
+#define MSG_STATS_TOTALPRINTTIME	    "Tempo stampa tot:"
+#define MSG_STATS_FILAMENTUSED		    "Filamento usato:"
+#define MSG_STATS_PRINTTIME		    "Tempo di stampa:"
+#define MSG_SELFTEST_START		    "Avvia autotest"
+#define MSG_SELFTEST_CHECK_ENDSTOPS	    "Verifica finecorsa"
+#define MSG_SELFTEST_CHECK_HOTEND	    "Verifica ugello"  
+#define MSG_SELFTEST_CHECK_X		    "Verifica asse X"
+#define MSG_SELFTEST_CHECK_Y		    "Verifica asse Y"
+#define MSG_SELFTEST_CHECK_Z		    "Verifica asse Z"
+#define MSG_SELFTEST_CHECK_BED		    "Verifica letto"
+#define MSG_SELFTEST_CHECK_ALLCORRECT	    "Nessun errore"
+#define MSG_SELFTEST			    "Autotest"
+#define MSG_SELFTEST_FAILED		    "Autotest fallito"
+#define MSG_STATISTICS			    "Statistiche"
+#define MSG_USB_PRINTING		    "Stampa da USB"
+#define MSG_HOMEYZ                          "Calibra Z"
+#define MSG_HOMEYZ_PROGRESS                 "Calibrando Z"
+#define MSG_HOMEYZ_DONE		            "Calibrazione OK"
+
+
+
+#define MSG_SHOW_END_STOPS					"Stato finecorsa"
+#define MSG_CALIBRATE_BED					"Calibra XYZ"
+#define MSG_CALIBRATE_BED_RESET					"Reset XYZ calibr."
+
+#define MSG_MOVE_CARRIAGE_TO_THE_TOP 				"Calibrazione XYZ. Ruotare la manopola per alzare il carrello Z fino all'altezza massima. Click per terminare."
+#define MSG_MOVE_CARRIAGE_TO_THE_TOP_Z 				"Calibrazione Z. Ruotare la manopola per alzare il carrello Z fino all'altezza massima. Click per terminare."
+
+#define MSG_CONFIRM_NOZZLE_CLEAN					"Pulire l'ugello per la calibrazione, poi fare click."
+#define MSG_CONFIRM_CARRIAGE_AT_THE_TOP				"I carrelli Z sin/des sono altezza max?"
+
+#define MSG_FIND_BED_OFFSET_AND_SKEW_LINE1			"Ricerca del letto punto di calibraz."
+#define MSG_FIND_BED_OFFSET_AND_SKEW_LINE2			" su 4"
+#define MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE1		"Perfezion. il letto punto di calibraz."
+#define MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE2		" su 9"
+#define MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE1		"Misurare l'altezza di riferimento del punto di calibrazione"
+#define MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE2		" su 9"
+#define MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION	"Reiterazione "
+
+#define MSG_BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND		"Calibrazione XYZ fallita. Il punto di calibrazione sul letto non e' stato trovato."
+#define MSG_BED_SKEW_OFFSET_DETECTION_FITTING_FAILED		"Calibrazione XYZ fallita. Si prega di consultare il manuale."
+#define MSG_BED_SKEW_OFFSET_DETECTION_PERFECT			"Calibrazione XYZ OK. Gli assi X/Y sono perpendicolari. Complimenti!"
+#define MSG_BED_SKEW_OFFSET_DETECTION_SKEW_MILD			"Calibrazion XYZ corretta. Assi X/Y leggermente storti. Ben fatto!"
+#define MSG_BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME		"Calibrazion XYZ corretta. La distorsione verra' automaticamente compensata."
+#define MSG_BED_SKEW_OFFSET_DETECTION_FAILED_FRONT_LEFT_FAR	"Calibrazione XYZ fallita. Punto anteriore sinistro non raggiungibile."
+#define MSG_BED_SKEW_OFFSET_DETECTION_FAILED_FRONT_RIGHT_FAR	"Calibrazione XYZ fallita. Punto anteriore destro non raggiungibile."
+#define MSG_BED_SKEW_OFFSET_DETECTION_FAILED_FRONT_BOTH_FAR	"Calibrazione XYZ fallita. Punti anteriori non raggiungibili."
+#define MSG_BED_SKEW_OFFSET_DETECTION_WARNING_FRONT_LEFT_FAR	"Calibrazione XYZ compromessa. Punto anteriore sinistro non raggiungibile."
+#define MSG_BED_SKEW_OFFSET_DETECTION_WARNING_FRONT_RIGHT_FAR	"Calibrazione XYZ compromessa. Punto anteriore destro non raggiungibile."
+#define MSG_BED_SKEW_OFFSET_DETECTION_WARNING_FRONT_BOTH_FAR	"Calibrazione XYZ compromessa. Punti anteriori non raggiungibili."
+
+#define MSG_BED_LEVELING_FAILED_POINT_LOW				"Livellamento letto fallito.NoRispSensor Residui su ugello? In attesa di reset."
+#define MSG_BED_LEVELING_FAILED_POINT_HIGH				"Livellamento letto fallito.Risp sensore troppo prestoIn attesa di reset."
+#define MSG_BED_LEVELING_FAILED_PROBE_DISCONNECTED		"Livellamento letto fallito. Sensore discon. o Cavo Dann. In attesa di reset."
+
+#define MSG_NEW_FIRMWARE_AVAILABLE						"Nuova versione del firmware disponibile"
+#define MSG_NEW_FIRMWARE_PLEASE_UPGRADE					"Prega aggiorna."
+
+#define MSG_FOLLOW_CALIBRATION_FLOW						"Stampante ancora non calibrata. Si prega di seguire il manuale, capitolo PRIMI PASSI, sezione della calibrazione."
+#define MSG_BABYSTEP_Z_NOT_SET							"Distanza tra la punta dell'ugello e la superficie del letto non ancora imposta. Si prega di seguire il manuale, capitolo First steps, sezione First layer calibration."
+
+#define MSG_BED_CORRECTION_MENU							"Correz. liv.letto"
+#define MSG_BED_CORRECTION_LEFT							"Sinistra  [um]"
+#define MSG_BED_CORRECTION_RIGHT						"Destra    [um]"
+#define MSG_BED_CORRECTION_FRONT						"Fronte    [um]"
+#define MSG_BED_CORRECTION_REAR							"Retro     [um]"
+#define MSG_BED_CORRECTION_RESET						"Reset"			
+
+#define MSG_MESH_BED_LEVELING							"Mesh livel. letto"
+#define MSG_MENU_CALIBRATION							"Calibrazione"
+#define MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_OFF					"SD card [normal]"
+#define MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_ON					"SD card [FlshAir]"
+
+#define MSG_LOOSE_PULLEY								"Puleggia lenta"
+#define MSG_FILAMENT_LOADING_T0							"Inserire filamento nell'estrusore 1. Click per continuare."
+#define MSG_FILAMENT_LOADING_T1							"Inserire filamento nell'estrusore 2. Click per continuare."
+#define MSG_FILAMENT_LOADING_T2							"Inserire filamento nell'estrusore 3. Click per continuare."
+#define MSG_FILAMENT_LOADING_T3							"Inserire filamento nell'estrusore 4. Click per continuare."
+#define MSG_CHANGE_EXTR									"Cambio estrusore."
+
+#define MSG_FIL_ADJUSTING								"Filamento in fase di regolazione. Attendere prego."
+#define MSG_CONFIRM_NOZZLE_CLEAN_FIL_ADJ				"I filamenti sono regolati. Si prega di pulire l'ugello per la calibrazione. Click per continuare."
+#define MSG_CALIBRATE_E									"Calibra E"
+#define MSG_E_CAL_KNOB									"Girare la manopola affinche' il segno raggiunga il corpo dell'estrusore. Click per continuare."
+#define MSG_MARK_FIL									"Segnare il filamento a 100 mm di distanza dal corpo dell'estrusore. Click per continuare."
+#define MSG_CLEAN_NOZZLE_E								"Calibrazione E terminata. Si prega di pulire l'ugello. Click per continuare."
+#define MSG_WAITING_TEMP								"In attesa del raffreddamento della testina e del piatto"
+#define MSG_FILAMENT_CLEAN								"Il colore e' nitido?"
+#define MSG_UNLOADING_FILAMENT							"Rilasc. filamento"
+#define MSG_PAPER										"Porre un foglio sotto l'ugello durante la calibrazione dei primi 4 punti. In caso l'ugello muova il foglio spegnere prontamente la stampante."
+
+#define MSG_FINISHING_MOVEMENTS							"Arresto in corso"
+#define MSG_PRINT_PAUSED								"Stampa in pausa"
+#define MSG_RESUMING_PRINT								"Stampa in ripresa"
+#define MSG_PID_EXTRUDER								"Calibrazione PID"
+#define MSG_SET_TEMPERATURE								"Imposta temperatura"
+#define MSG_PID_FINISHED								"Cal. PID completa"
+#define MSG_PID_RUNNING									"Cal. PID"
+
+#define MSG_CALIBRATE_PINDA								"Calibrare"
+#define MSG_CALIBRATION_PINDA_MENU						"Taratura temp."
+#define MSG_PINDA_NOT_CALIBRATED						"Taratura della temperatura non ancora eseguita"
+#define MSG_PINDA_PREHEAT								"Riscald. PINDA"
+#define MSG_TEMP_CALIBRATION							"Cal. temp.          "
+#define MSG_TEMP_CALIBRATION_DONE						"Taratura temperatura terminata. Fare click per continuare."
+#define MSG_TEMP_CALIBRATION_ON							"Cal. temp. [ON]"
+#define MSG_TEMP_CALIBRATION_OFF						"Cal. temp. [OFF]"
+
+#define MSG_LOAD_ALL									"Caricare tutti"
+#define MSG_LOAD_FILAMENT_1								"Caricare fil. 1"
+#define MSG_LOAD_FILAMENT_2								"Caricare fil. 2"
+#define MSG_LOAD_FILAMENT_3								"Caricare fil. 3"
+#define MSG_LOAD_FILAMENT_4								"Caricare fil. 4"
+#define MSG_UNLOAD_FILAMENT_1							"Rilasciare fil. 1"
+#define MSG_UNLOAD_FILAMENT_2							"Rilasciare fil. 1"
+#define MSG_UNLOAD_FILAMENT_3							"Rilasciare fil. 1"
+#define MSG_UNLOAD_FILAMENT_4							"Rilasciare fil. 1"
+#define MSG_UNLOAD_ALL									"Rilasciare tutti"
+#define MSG_PREPARE_FILAMENT							"Preparare filamento"
+#define MSG_ALL											"Tutti"
+#define MSG_USED										"Usati nella stampa"
+#define MSG_CURRENT										"Attuale"

+ 2406 - 2406
Firmware/mesh_bed_calibration.cpp

@@ -1,2407 +1,2407 @@
-#include "Marlin.h"
-#include "Configuration.h"
-#include "ConfigurationStore.h"
-#include "language_all.h"
-#include "mesh_bed_calibration.h"
-#include "mesh_bed_leveling.h"
-#include "stepper.h"
-#include "ultralcd.h"
-
-uint8_t world2machine_correction_mode;
-float   world2machine_rotation_and_skew[2][2];
-float   world2machine_rotation_and_skew_inv[2][2];
-float   world2machine_shift[2];
-
-// Weight of the Y coordinate for the least squares fitting of the bed induction sensor targets.
-// Only used for the first row of the points, which may not befully in reach of the sensor.
-#define WEIGHT_FIRST_ROW_X_HIGH (1.f)
-#define WEIGHT_FIRST_ROW_X_LOW  (0.35f)
-#define WEIGHT_FIRST_ROW_Y_HIGH (0.3f)
-#define WEIGHT_FIRST_ROW_Y_LOW  (0.0f)
-
-#define BED_ZERO_REF_X (- 22.f + X_PROBE_OFFSET_FROM_EXTRUDER)
-#define BED_ZERO_REF_Y (- 0.6f + Y_PROBE_OFFSET_FROM_EXTRUDER)
-
-// Scaling of the real machine axes against the programmed dimensions in the firmware.
-// The correction is tiny, here around 0.5mm on 250mm length.
-//#define MACHINE_AXIS_SCALE_X ((250.f - 0.5f) / 250.f)
-//#define MACHINE_AXIS_SCALE_Y ((250.f - 0.5f) / 250.f)
-#define MACHINE_AXIS_SCALE_X 1.f
-#define MACHINE_AXIS_SCALE_Y 1.f
-
-// 0.12 degrees equals to an offset of 0.5mm on 250mm length. 
-#define BED_SKEW_ANGLE_MILD         (0.12f * M_PI / 180.f)
-// 0.25 degrees equals to an offset of 1.1mm on 250mm length.
-#define BED_SKEW_ANGLE_EXTREME      (0.25f * M_PI / 180.f)
-
-#define BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN  (0.8f)
-#define BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X  (0.8f)
-#define BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y  (1.5f)
-
-#define MIN_BED_SENSOR_POINT_RESPONSE_DMR           (2.0f)
-
-//#define Y_MIN_POS_FOR_BED_CALIBRATION (MANUAL_Y_HOME_POS-0.2f)
-#define Y_MIN_POS_FOR_BED_CALIBRATION (Y_MIN_POS)
-// Distances toward the print bed edge may not be accurate.
-#define Y_MIN_POS_CALIBRATION_POINT_ACCURATE (Y_MIN_POS + 3.f)
-// When the measured point center is out of reach of the sensor, Y coordinate will be ignored
-// by the Least Squares fitting and the X coordinate will be weighted low.
-#define Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH (Y_MIN_POS - 0.5f)
-
-// Positions of the bed reference points in the machine coordinates, referenced to the P.I.N.D.A sensor.
-// The points are ordered in a zig-zag fashion to speed up the calibration.
-const float bed_ref_points[] PROGMEM = {
-    13.f  - BED_ZERO_REF_X,   6.4f - BED_ZERO_REF_Y,
-    115.f - BED_ZERO_REF_X,   6.4f - BED_ZERO_REF_Y,
-    216.f - BED_ZERO_REF_X,   6.4f - BED_ZERO_REF_Y,
-
-    216.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
-    115.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
-    13.f  - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
-
-    13.f  - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
-    115.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
-    216.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y
-};
-
-// Positions of the bed reference points in the machine coordinates, referenced to the P.I.N.D.A sensor.
-// The points are the following: center front, center right, center rear, center left.
-const float bed_ref_points_4[] PROGMEM = {
-    115.f - BED_ZERO_REF_X,   6.4f - BED_ZERO_REF_Y,
-    216.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
-    115.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
-    13.f  - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y
-};
-
-static inline float sqr(float x) { return x * x; }
-
-static inline bool point_on_1st_row(const uint8_t i, const uint8_t npts)
-{
-	if (npts == 4) return (i == 0);
-	else return (i < 3);
-}
-
-// Weight of a point coordinate in a least squares optimization.
-// The first row of points may not be fully reachable
-// and the y values may be shortened a bit by the bed carriage
-// pulling the belt up.
-static inline float point_weight_x(const uint8_t i, const uint8_t npts, const float &y)
-{
-    float w = 1.f;
-    if (point_on_1st_row(i, npts)) {
-		if (y >= Y_MIN_POS_CALIBRATION_POINT_ACCURATE) {
-            w = WEIGHT_FIRST_ROW_X_HIGH;
-        } else if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
-            // If the point is fully outside, give it some weight.
-            w = WEIGHT_FIRST_ROW_X_LOW;
-        } else {
-            // Linearly interpolate the weight from 1 to WEIGHT_FIRST_ROW_X.
-            float t = (y - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) / (Y_MIN_POS_CALIBRATION_POINT_ACCURATE - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
-            w = (1.f - t) * WEIGHT_FIRST_ROW_X_LOW + t * WEIGHT_FIRST_ROW_X_HIGH;
-        }
-    }
-    return w;
-}
-
-// Weight of a point coordinate in a least squares optimization.
-// The first row of points may not be fully reachable
-// and the y values may be shortened a bit by the bed carriage
-// pulling the belt up.
-static inline float point_weight_y(const uint8_t i, const uint8_t npts, const float &y)
-{
-    float w = 1.f;
-    if (point_on_1st_row(i, npts)) {
-        if (y >= Y_MIN_POS_CALIBRATION_POINT_ACCURATE) {
-            w = WEIGHT_FIRST_ROW_Y_HIGH;
-        } else if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
-            // If the point is fully outside, give it some weight.
-            w = WEIGHT_FIRST_ROW_Y_LOW;
-        } else {
-            // Linearly interpolate the weight from 1 to WEIGHT_FIRST_ROW_X.
-            float t = (y - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) / (Y_MIN_POS_CALIBRATION_POINT_ACCURATE - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
-            w = (1.f - t) * WEIGHT_FIRST_ROW_Y_LOW + t * WEIGHT_FIRST_ROW_Y_HIGH;
-        }
-    }
-    return w;
-}
-
-// Non-Linear Least Squares fitting of the bed to the measured induction points
-// using the Gauss-Newton method.
-// This method will maintain a unity length of the machine axes,
-// which is the correct approach if the sensor points are not measured precisely.
-BedSkewOffsetDetectionResultType calculate_machine_skew_and_offset_LS(
-    // Matrix of maximum 9 2D points (18 floats)
-    const float  *measured_pts,
-    uint8_t       npts,
-    const float  *true_pts,
-    // Resulting correction matrix.
-    float        *vec_x,
-    float        *vec_y,
-    float        *cntr,
-    // Temporary values, 49-18-(2*3)=25 floats
-    //    , float *temp
-    int8_t        verbosity_level
-    )
-{
-    if (verbosity_level >= 10) {
-		SERIAL_ECHOLNPGM("calculate machine skew and offset LS");
-
-        // Show the initial state, before the fitting.
-        SERIAL_ECHOPGM("X vector, initial: ");
-        MYSERIAL.print(vec_x[0], 5);
-        SERIAL_ECHOPGM(", ");
-        MYSERIAL.print(vec_x[1], 5);
-        SERIAL_ECHOLNPGM("");
-
-        SERIAL_ECHOPGM("Y vector, initial: ");
-        MYSERIAL.print(vec_y[0], 5);
-        SERIAL_ECHOPGM(", ");
-        MYSERIAL.print(vec_y[1], 5);
-        SERIAL_ECHOLNPGM("");
-
-        SERIAL_ECHOPGM("center, initial: ");
-        MYSERIAL.print(cntr[0], 5);
-        SERIAL_ECHOPGM(", ");
-        MYSERIAL.print(cntr[1], 5);
-        SERIAL_ECHOLNPGM("");
-
-        for (uint8_t i = 0; i < npts; ++i) {
-            SERIAL_ECHOPGM("point #");
-            MYSERIAL.print(int(i));
-            SERIAL_ECHOPGM(" measured: (");
-            MYSERIAL.print(measured_pts[i * 2], 5);
-            SERIAL_ECHOPGM(", ");
-            MYSERIAL.print(measured_pts[i * 2 + 1], 5);
-            SERIAL_ECHOPGM("); target: (");
-            MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
-            SERIAL_ECHOPGM(", ");
-            MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
-            SERIAL_ECHOPGM("), error: ");
-            MYSERIAL.print(sqrt(
-                sqr(pgm_read_float(true_pts + i * 2) - measured_pts[i * 2]) +
-                sqr(pgm_read_float(true_pts + i * 2 + 1) - measured_pts[i * 2 + 1])), 5);
-            SERIAL_ECHOLNPGM("");
-        }
-        delay_keep_alive(100);
-    }
-
-    // Run some iterations of the Gauss-Newton method of non-linear least squares.
-    // Initial set of parameters:
-    // X,Y offset
-    cntr[0] = 0.f;
-    cntr[1] = 0.f;
-    // Rotation of the machine X axis from the bed X axis.
-    float a1 = 0;
-    // Rotation of the machine Y axis from the bed Y axis.
-    float a2 = 0;
-    for (int8_t iter = 0; iter < 100; ++iter) {
-        float c1 = cos(a1) * MACHINE_AXIS_SCALE_X;
-        float s1 = sin(a1) * MACHINE_AXIS_SCALE_X;
-        float c2 = cos(a2) * MACHINE_AXIS_SCALE_Y;
-        float s2 = sin(a2) * MACHINE_AXIS_SCALE_Y;
-        // Prepare the Normal equation for the Gauss-Newton method.
-        float A[4][4] = { 0.f };
-        float b[4] = { 0.f };
-        float acc;
-        for (uint8_t r = 0; r < 4; ++r) {
-            for (uint8_t c = 0; c < 4; ++c) {
-                acc = 0;
-                // J^T times J
-                for (uint8_t i = 0; i < npts; ++i) {
-                    // First for the residuum in the x axis:
-                    if (r != 1 && c != 1) {
-                        float a = 
-                             (r == 0) ? 1.f :
-                            ((r == 2) ? (-s1 * measured_pts[2 * i]) :
-                                        (-c2 * measured_pts[2 * i + 1]));
-                        float b = 
-                             (c == 0) ? 1.f :
-                            ((c == 2) ? (-s1 * measured_pts[2 * i]) :
-                                        (-c2 * measured_pts[2 * i + 1]));
-                        float w = point_weight_x(i, npts, measured_pts[2 * i + 1]);
-                        acc += a * b * w;
-                    }
-                    // Second for the residuum in the y axis. 
-                    // The first row of the points have a low weight, because their position may not be known
-                    // with a sufficient accuracy.
-                    if (r != 0 && c != 0) {
-                        float a = 
-                             (r == 1) ? 1.f :
-                            ((r == 2) ? ( c1 * measured_pts[2 * i]) :
-                                        (-s2 * measured_pts[2 * i + 1]));
-                        float b = 
-                             (c == 1) ? 1.f :
-                            ((c == 2) ? ( c1 * measured_pts[2 * i]) :
-                                        (-s2 * measured_pts[2 * i + 1]));
-                        float w = point_weight_y(i, npts, measured_pts[2 * i + 1]);
-                        acc += a * b * w;
-                    }
-                }
-                A[r][c] = acc;
-            }
-            // J^T times f(x)
-            acc = 0.f;
-            for (uint8_t i = 0; i < npts; ++i) {
-                {
-                    float j = 
-                         (r == 0) ? 1.f :
-                        ((r == 1) ? 0.f :
-                        ((r == 2) ? (-s1 * measured_pts[2 * i]) :
-                                    (-c2 * measured_pts[2 * i + 1])));
-                    float fx = c1 * measured_pts[2 * i] - s2 * measured_pts[2 * i + 1] + cntr[0] - pgm_read_float(true_pts + i * 2);
-                    float w = point_weight_x(i, npts, measured_pts[2 * i + 1]);
-                    acc += j * fx * w;
-                }
-                {
-                    float j = 
-                         (r == 0) ? 0.f :
-                        ((r == 1) ? 1.f :
-                        ((r == 2) ? ( c1 * measured_pts[2 * i]) :
-                                    (-s2 * measured_pts[2 * i + 1])));
-                    float fy = s1 * measured_pts[2 * i] + c2 * measured_pts[2 * i + 1] + cntr[1] - pgm_read_float(true_pts + i * 2 + 1);
-                    float w = point_weight_y(i, npts, measured_pts[2 * i + 1]);
-                    acc += j * fy * w;
-                }
-            }
-            b[r] = -acc;
-        }
-
-        // Solve for h by a Gauss iteration method.
-        float h[4] = { 0.f };
-        for (uint8_t gauss_iter = 0; gauss_iter < 100; ++gauss_iter) {
-            h[0] = (b[0] - A[0][1] * h[1] - A[0][2] * h[2] - A[0][3] * h[3]) / A[0][0];
-            h[1] = (b[1] - A[1][0] * h[0] - A[1][2] * h[2] - A[1][3] * h[3]) / A[1][1];
-            h[2] = (b[2] - A[2][0] * h[0] - A[2][1] * h[1] - A[2][3] * h[3]) / A[2][2];
-            h[3] = (b[3] - A[3][0] * h[0] - A[3][1] * h[1] - A[3][2] * h[2]) / A[3][3];
-        }
-
-        // and update the current position with h.
-        // It may be better to use the Levenberg-Marquart method here,
-        // but because we are very close to the solution alread,
-        // the simple Gauss-Newton non-linear Least Squares method works well enough.
-        cntr[0] += h[0];
-        cntr[1] += h[1];
-        a1 += h[2];
-        a2 += h[3];
-
-        if (verbosity_level >= 20) {
-            SERIAL_ECHOPGM("iteration: ");
-            MYSERIAL.print(int(iter));
-			SERIAL_ECHOPGM("; correction vector: ");
-            MYSERIAL.print(h[0], 5);
-            SERIAL_ECHOPGM(", ");
-            MYSERIAL.print(h[1], 5);
-            SERIAL_ECHOPGM(", ");
-            MYSERIAL.print(h[2], 5);
-            SERIAL_ECHOPGM(", ");
-            MYSERIAL.print(h[3], 5);
-            SERIAL_ECHOLNPGM("");
-            SERIAL_ECHOPGM("corrected x/y: ");
-            MYSERIAL.print(cntr[0], 5);
-            SERIAL_ECHOPGM(", ");
-            MYSERIAL.print(cntr[0], 5);
-            SERIAL_ECHOLNPGM("");
-            SERIAL_ECHOPGM("corrected angles: ");
-            MYSERIAL.print(180.f * a1 / M_PI, 5);
-            SERIAL_ECHOPGM(", ");
-            MYSERIAL.print(180.f * a2 / M_PI, 5);
-            SERIAL_ECHOLNPGM("");
-        }
-    }
-
-    vec_x[0] =  cos(a1) * MACHINE_AXIS_SCALE_X;
-    vec_x[1] =  sin(a1) * MACHINE_AXIS_SCALE_X;
-    vec_y[0] = -sin(a2) * MACHINE_AXIS_SCALE_Y;
-    vec_y[1] =  cos(a2) * MACHINE_AXIS_SCALE_Y;
-
-    BedSkewOffsetDetectionResultType result = BED_SKEW_OFFSET_DETECTION_PERFECT;
-    {
-        float angleDiff = fabs(a2 - a1);
-        if (angleDiff > BED_SKEW_ANGLE_MILD)
-            result = (angleDiff > BED_SKEW_ANGLE_EXTREME) ?
-                BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME :
-                BED_SKEW_OFFSET_DETECTION_SKEW_MILD;
-        if (fabs(a1) > BED_SKEW_ANGLE_EXTREME ||
-            fabs(a2) > BED_SKEW_ANGLE_EXTREME)
-            result = BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME;
-    }
-
-    if (verbosity_level >= 1) {
-        SERIAL_ECHOPGM("correction angles: ");
-        MYSERIAL.print(180.f * a1 / M_PI, 5);
-        SERIAL_ECHOPGM(", ");
-        MYSERIAL.print(180.f * a2 / M_PI, 5);
-        SERIAL_ECHOLNPGM("");
-    }
-
-    if (verbosity_level >= 10) {
-        // Show the adjusted state, before the fitting.
-        SERIAL_ECHOPGM("X vector new, inverted: ");
-        MYSERIAL.print(vec_x[0], 5);
-        SERIAL_ECHOPGM(", ");
-        MYSERIAL.print(vec_x[1], 5);
-        SERIAL_ECHOLNPGM("");
-
-        SERIAL_ECHOPGM("Y vector new, inverted: ");
-        MYSERIAL.print(vec_y[0], 5);
-        SERIAL_ECHOPGM(", ");
-        MYSERIAL.print(vec_y[1], 5);
-        SERIAL_ECHOLNPGM("");
-
-        SERIAL_ECHOPGM("center new, inverted: ");
-        MYSERIAL.print(cntr[0], 5);
-        SERIAL_ECHOPGM(", ");
-        MYSERIAL.print(cntr[1], 5);
-        SERIAL_ECHOLNPGM("");
-        delay_keep_alive(100);
-
-        SERIAL_ECHOLNPGM("Error after correction: ");
-    }
-
-    // Measure the error after correction.
-    for (uint8_t i = 0; i < npts; ++i) {
-        float x = vec_x[0] * measured_pts[i * 2] + vec_y[0] * measured_pts[i * 2 + 1] + cntr[0];
-        float y = vec_x[1] * measured_pts[i * 2] + vec_y[1] * measured_pts[i * 2 + 1] + cntr[1];
-        float errX = sqr(pgm_read_float(true_pts + i * 2) - x);
-        float errY = sqr(pgm_read_float(true_pts + i * 2 + 1) - y);
-        float err = sqrt(errX + errY);
-		if (verbosity_level >= 10) {
-			SERIAL_ECHOPGM("point #");
-			MYSERIAL.print(int(i));
-			SERIAL_ECHOLNPGM(":");
-		}
-
-		if (point_on_1st_row(i, npts)) {
-				if(verbosity_level >= 20) SERIAL_ECHOPGM("Point on first row");
-				float w = point_weight_y(i, npts, measured_pts[2 * i + 1]);
-				if (sqrt(errX) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X ||
-					(w != 0.f && sqrt(errY) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y)) {
-					result = BED_SKEW_OFFSET_DETECTION_FITTING_FAILED;
-					if (verbosity_level >= 20) {
-						SERIAL_ECHOPGM(", weigth Y: ");
-						MYSERIAL.print(w);
-						if (sqrt(errX) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X) SERIAL_ECHOPGM(", error X > max. error X");
-						if (w != 0.f && sqrt(errY) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y) SERIAL_ECHOPGM(", error Y > max. error Y");
-					}
-				}
-		}
-		else {
-			if(verbosity_level >=20 ) SERIAL_ECHOPGM("Point not on first row");
-			if (err > BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN) {
-				result = BED_SKEW_OFFSET_DETECTION_FITTING_FAILED;
-				if(verbosity_level >= 20) SERIAL_ECHOPGM(", error > max. error euclidian"); 
-			}
-        }
-        if (verbosity_level >= 10) {
-			SERIAL_ECHOLNPGM("");
-            SERIAL_ECHOPGM("measured: (");
-            MYSERIAL.print(measured_pts[i * 2], 5);
-            SERIAL_ECHOPGM(", ");
-            MYSERIAL.print(measured_pts[i * 2 + 1], 5);
-            SERIAL_ECHOPGM("); corrected: (");
-            MYSERIAL.print(x, 5);
-            SERIAL_ECHOPGM(", ");
-            MYSERIAL.print(y, 5);
-            SERIAL_ECHOPGM("); target: (");
-            MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
-            SERIAL_ECHOPGM(", ");
-            MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
-			SERIAL_ECHOLNPGM(")");
-			SERIAL_ECHOPGM("error: ");
-            MYSERIAL.print(err);
-			SERIAL_ECHOPGM(", error X: ");
-			MYSERIAL.print(sqrt(errX));
-			SERIAL_ECHOPGM(", error Y: ");
-			MYSERIAL.print(sqrt(errY));
-			SERIAL_ECHOLNPGM("");
-			SERIAL_ECHOLNPGM("");
-        }
-    }
-	if (verbosity_level >= 20) {
-		SERIAL_ECHOLNPGM("Max. errors:");
-		SERIAL_ECHOPGM("Max. error X:");
-		MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X);
-		SERIAL_ECHOPGM("Max. error Y:");
-		MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y);
-		SERIAL_ECHOPGM("Max. error euclidian:");
-		MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN);
-		SERIAL_ECHOLNPGM("");
-	}
-
-    #if 0
-    if (result == BED_SKEW_OFFSET_DETECTION_PERFECT && fabs(a1) < BED_SKEW_ANGLE_MILD && fabs(a2) < BED_SKEW_ANGLE_MILD) {
-        if (verbosity_level > 0)
-            SERIAL_ECHOLNPGM("Very little skew detected. Disabling skew correction.");
-        // Just disable the skew correction.
-        vec_x[0] = MACHINE_AXIS_SCALE_X;
-        vec_x[1] = 0.f;
-        vec_y[0] = 0.f;
-        vec_y[1] = MACHINE_AXIS_SCALE_Y;
-    }
-    #else
-    if (result == BED_SKEW_OFFSET_DETECTION_PERFECT) {
-        if (verbosity_level > 0)
-            SERIAL_ECHOLNPGM("Very little skew detected. Orthogonalizing the axes.");
-        // Orthogonalize the axes.
-        a1 = 0.5f * (a1 + a2);
-        vec_x[0] =  cos(a1) * MACHINE_AXIS_SCALE_X;
-        vec_x[1] =  sin(a1) * MACHINE_AXIS_SCALE_X;
-        vec_y[0] = -sin(a1) * MACHINE_AXIS_SCALE_Y;
-        vec_y[1] =  cos(a1) * MACHINE_AXIS_SCALE_Y;
-        // Refresh the offset.
-        cntr[0] = 0.f;
-        cntr[1] = 0.f;
-        float wx = 0.f;
-        float wy = 0.f;
-        for (int8_t i = 0; i < npts; ++ i) {
-            float x = vec_x[0] * measured_pts[i * 2] + vec_y[0] * measured_pts[i * 2 + 1];
-            float y = vec_x[1] * measured_pts[i * 2] + vec_y[1] * measured_pts[i * 2 + 1];
-            float w = point_weight_x(i, npts, y);
-			cntr[0] += w * (pgm_read_float(true_pts + i * 2) - x);
-			wx += w;
-			if (verbosity_level >= 20) {
-				MYSERIAL.print(i);
-				SERIAL_ECHOLNPGM("");
-				SERIAL_ECHOLNPGM("Weight_x:");
-				MYSERIAL.print(w);
-				SERIAL_ECHOLNPGM("");
-				SERIAL_ECHOLNPGM("cntr[0]:");
-				MYSERIAL.print(cntr[0]);
-				SERIAL_ECHOLNPGM("");
-				SERIAL_ECHOLNPGM("wx:");
-				MYSERIAL.print(wx);
-			}
-            w = point_weight_y(i, npts, y);
-			cntr[1] += w * (pgm_read_float(true_pts + i * 2 + 1) - y);
-			wy += w;
-
-			if (verbosity_level >= 20) {
-				SERIAL_ECHOLNPGM("");
-				SERIAL_ECHOLNPGM("Weight_y:");
-				MYSERIAL.print(w);
-				SERIAL_ECHOLNPGM("");
-				SERIAL_ECHOLNPGM("cntr[1]:");
-				MYSERIAL.print(cntr[1]);
-				SERIAL_ECHOLNPGM("");
-				SERIAL_ECHOLNPGM("wy:");
-				MYSERIAL.print(wy);
-				SERIAL_ECHOLNPGM("");
-				SERIAL_ECHOLNPGM("");
-			}
-		}
-        cntr[0] /= wx;
-        cntr[1] /= wy;
-		if (verbosity_level >= 20) {
-			SERIAL_ECHOLNPGM("");
-			SERIAL_ECHOLNPGM("Final cntr values:");
-			SERIAL_ECHOLNPGM("cntr[0]:");
-			MYSERIAL.print(cntr[0]);
-			SERIAL_ECHOLNPGM("");
-			SERIAL_ECHOLNPGM("cntr[1]:");
-			MYSERIAL.print(cntr[1]);
-			SERIAL_ECHOLNPGM("");
-		}
-
-    }
-    #endif
-
-    // Invert the transformation matrix made of vec_x, vec_y and cntr.
-    {
-        float d = vec_x[0] * vec_y[1] - vec_x[1] * vec_y[0];
-        float Ainv[2][2] = {
-            { vec_y[1] / d, -vec_y[0] / d },
-            { -vec_x[1] / d, vec_x[0] / d }
-        };
-        float cntrInv[2] = {
-            -Ainv[0][0] * cntr[0] - Ainv[0][1] * cntr[1],
-            -Ainv[1][0] * cntr[0] - Ainv[1][1] * cntr[1]
-        };
-        vec_x[0] = Ainv[0][0];
-        vec_x[1] = Ainv[1][0];
-        vec_y[0] = Ainv[0][1];
-        vec_y[1] = Ainv[1][1];
-        cntr[0] = cntrInv[0];
-        cntr[1] = cntrInv[1];
-    }
-
-    if (verbosity_level >= 1) {
-        // Show the adjusted state, before the fitting.
-        SERIAL_ECHOPGM("X vector, adjusted: ");
-        MYSERIAL.print(vec_x[0], 5);
-        SERIAL_ECHOPGM(", ");
-        MYSERIAL.print(vec_x[1], 5);
-        SERIAL_ECHOLNPGM("");
-
-        SERIAL_ECHOPGM("Y vector, adjusted: ");
-        MYSERIAL.print(vec_y[0], 5);
-        SERIAL_ECHOPGM(", ");
-        MYSERIAL.print(vec_y[1], 5);
-        SERIAL_ECHOLNPGM("");
-
-        SERIAL_ECHOPGM("center, adjusted: ");
-        MYSERIAL.print(cntr[0], 5);
-        SERIAL_ECHOPGM(", ");
-        MYSERIAL.print(cntr[1], 5);
-        SERIAL_ECHOLNPGM("");
-        delay_keep_alive(100);
-    }
-
-    if (verbosity_level >= 2) {
-        SERIAL_ECHOLNPGM("Difference after correction: ");
-        for (uint8_t i = 0; i < npts; ++i) {
-            float x = vec_x[0] * pgm_read_float(true_pts + i * 2) + vec_y[0] * pgm_read_float(true_pts + i * 2 + 1) + cntr[0];
-            float y = vec_x[1] * pgm_read_float(true_pts + i * 2) + vec_y[1] * pgm_read_float(true_pts + i * 2 + 1) + cntr[1];
-            SERIAL_ECHOPGM("point #");
-            MYSERIAL.print(int(i));
-            SERIAL_ECHOPGM("measured: (");
-            MYSERIAL.print(measured_pts[i * 2], 5);
-            SERIAL_ECHOPGM(", ");
-            MYSERIAL.print(measured_pts[i * 2 + 1], 5);
-            SERIAL_ECHOPGM("); measured-corrected: (");
-            MYSERIAL.print(x, 5);
-            SERIAL_ECHOPGM(", ");
-            MYSERIAL.print(y, 5);
-            SERIAL_ECHOPGM("); target: (");
-            MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
-            SERIAL_ECHOPGM(", ");
-            MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
-            SERIAL_ECHOPGM("), error: ");
-            MYSERIAL.print(sqrt(sqr(measured_pts[i * 2] - x) + sqr(measured_pts[i * 2 + 1] - y)));
-            SERIAL_ECHOLNPGM("");
-        }
-		if (verbosity_level >= 20) {
-			SERIAL_ECHOLNPGM("");
-			SERIAL_ECHOLNPGM("Calculate offset and skew returning result:");
-			MYSERIAL.print(int(result));
-			SERIAL_ECHOLNPGM("");
-			SERIAL_ECHOLNPGM("");
-		}
-        delay_keep_alive(100);
-    }
-
-    return result;
-}
-
-void reset_bed_offset_and_skew()
-{
-    eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+0), 0x0FFFFFFFF);
-    eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+4), 0x0FFFFFFFF);
-    eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +0), 0x0FFFFFFFF);
-    eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +4), 0x0FFFFFFFF);
-    eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +0), 0x0FFFFFFFF);
-    eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +4), 0x0FFFFFFFF);
-
-    // Reset the 8 16bit offsets.
-    for (int8_t i = 0; i < 4; ++ i)
-        eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_Z_JITTER+i*4), 0x0FFFFFFFF);
-}
-
-bool is_bed_z_jitter_data_valid()
-// offsets of the Z heiths of the calibration points from the first point are saved as 16bit signed int, scaled to tenths of microns
-{
-    for (int8_t i = 0; i < 8; ++ i)
-        if (eeprom_read_word((uint16_t*)(EEPROM_BED_CALIBRATION_Z_JITTER+i*2)) == 0x0FFFF)
-            return false;
-    return true;
-}
-
-static void world2machine_update(const float vec_x[2], const float vec_y[2], const float cntr[2])
-{
-    world2machine_rotation_and_skew[0][0] = vec_x[0];
-    world2machine_rotation_and_skew[1][0] = vec_x[1];
-    world2machine_rotation_and_skew[0][1] = vec_y[0];
-    world2machine_rotation_and_skew[1][1] = vec_y[1];
-    world2machine_shift[0] = cntr[0];
-    world2machine_shift[1] = cntr[1];
-    // No correction.
-    world2machine_correction_mode = WORLD2MACHINE_CORRECTION_NONE;
-    if (world2machine_shift[0] != 0.f || world2machine_shift[1] != 0.f)
-        // Shift correction.
-        world2machine_correction_mode |= WORLD2MACHINE_CORRECTION_SHIFT;
-    if (world2machine_rotation_and_skew[0][0] != 1.f || world2machine_rotation_and_skew[0][1] != 0.f ||
-        world2machine_rotation_and_skew[1][0] != 0.f || world2machine_rotation_and_skew[1][1] != 1.f) {
-        // Rotation & skew correction.
-        world2machine_correction_mode |= WORLD2MACHINE_CORRECTION_SKEW;
-        // Invert the world2machine matrix.
-        float d = world2machine_rotation_and_skew[0][0] * world2machine_rotation_and_skew[1][1] - world2machine_rotation_and_skew[1][0] * world2machine_rotation_and_skew[0][1];
-        world2machine_rotation_and_skew_inv[0][0] =  world2machine_rotation_and_skew[1][1] / d;
-        world2machine_rotation_and_skew_inv[0][1] = -world2machine_rotation_and_skew[0][1] / d;
-        world2machine_rotation_and_skew_inv[1][0] = -world2machine_rotation_and_skew[1][0] / d;
-        world2machine_rotation_and_skew_inv[1][1] =  world2machine_rotation_and_skew[0][0] / d;
-    } else {
-        world2machine_rotation_and_skew_inv[0][0] = 1.f;
-        world2machine_rotation_and_skew_inv[0][1] = 0.f;
-        world2machine_rotation_and_skew_inv[1][0] = 0.f;
-        world2machine_rotation_and_skew_inv[1][1] = 1.f;
-    }
-}
-
-void world2machine_reset()
-{
-    const float vx[] = { 1.f, 0.f };
-    const float vy[] = { 0.f, 1.f };
-    const float cntr[] = { 0.f, 0.f };
-    world2machine_update(vx, vy, cntr);
-}
-
-void world2machine_revert_to_uncorrected()
-{
-    if (world2machine_correction_mode != WORLD2MACHINE_CORRECTION_NONE) {
-        // Reset the machine correction matrix.
-        const float vx[] = { 1.f, 0.f };
-        const float vy[] = { 0.f, 1.f };
-        const float cntr[] = { 0.f, 0.f };
-        world2machine_update(vx, vy, cntr);
-        // Wait for the motors to stop and update the current position with the absolute values.
-        st_synchronize();
-        current_position[X_AXIS] = st_get_position_mm(X_AXIS);
-        current_position[Y_AXIS] = st_get_position_mm(Y_AXIS);
-    }
-}
-
-static inline bool vec_undef(const float v[2])
-{
-    const uint32_t *vx = (const uint32_t*)v;
-    return vx[0] == 0x0FFFFFFFF || vx[1] == 0x0FFFFFFFF;
-}
-
-void world2machine_initialize()
-{
-    SERIAL_ECHOLNPGM("world2machine_initialize");
-    float cntr[2] = {
-        eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0)),
-        eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4))
-    };
-    float vec_x[2] = {
-        eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0)),
-        eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4))
-    };
-    float vec_y[2] = {
-        eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0)),
-        eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4))
-    };
-
-    bool reset = false;
-    if (vec_undef(cntr) || vec_undef(vec_x) || vec_undef(vec_y)) {
-        SERIAL_ECHOLNPGM("Undefined bed correction matrix.");
-        reset = true;
-    }
-    else {
-        // Length of the vec_x shall be close to unity.
-        float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]);
-        if (l < 0.9 || l > 1.1) {
-			SERIAL_ECHOLNPGM("X vector length:");
-			MYSERIAL.println(l);
-            SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the X vector out of range.");
-            reset = true;
-        }
-        // Length of the vec_y shall be close to unity.
-        l = sqrt(vec_y[0] * vec_y[0] + vec_y[1] * vec_y[1]);
-        if (l < 0.9 || l > 1.1) {
-			SERIAL_ECHOLNPGM("Y vector length:");
-			MYSERIAL.println(l);
-            SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the Y vector out of range.");
-            reset = true;
-        }
-        // Correction of the zero point shall be reasonably small.
-        l = sqrt(cntr[0] * cntr[0] + cntr[1] * cntr[1]);
-        if (l > 15.f) {
-			SERIAL_ECHOLNPGM("Zero point correction:");
-			MYSERIAL.println(l);
-            SERIAL_ECHOLNPGM("Invalid bed correction matrix. Shift out of range.");
-            reset = true;
-        }
-        // vec_x and vec_y shall be nearly perpendicular.
-        l = vec_x[0] * vec_y[0] + vec_x[1] * vec_y[1];
-        if (fabs(l) > 0.1f) {
-            SERIAL_ECHOLNPGM("Invalid bed correction matrix. X/Y axes are far from being perpendicular.");
-            reset = true;
-        }
-    }
-
-    if (reset) {
-        SERIAL_ECHOLNPGM("Invalid bed correction matrix. Resetting to identity.");
-        reset_bed_offset_and_skew();
-        world2machine_reset();
-    } else {
-        world2machine_update(vec_x, vec_y, cntr);
-        /*
-        SERIAL_ECHOPGM("world2machine_initialize() loaded: ");
-        MYSERIAL.print(world2machine_rotation_and_skew[0][0], 5);
-        SERIAL_ECHOPGM(", ");
-        MYSERIAL.print(world2machine_rotation_and_skew[0][1], 5);
-        SERIAL_ECHOPGM(", ");
-        MYSERIAL.print(world2machine_rotation_and_skew[1][0], 5);
-        SERIAL_ECHOPGM(", ");
-        MYSERIAL.print(world2machine_rotation_and_skew[1][1], 5);
-        SERIAL_ECHOPGM(", offset ");
-        MYSERIAL.print(world2machine_shift[0], 5);
-        SERIAL_ECHOPGM(", ");
-        MYSERIAL.print(world2machine_shift[1], 5);
-        SERIAL_ECHOLNPGM("");
-        */
-    }
-}
-
-// When switching from absolute to corrected coordinates,
-// this will get the absolute coordinates from the servos,
-// applies the inverse world2machine transformation
-// and stores the result into current_position[x,y].
-void world2machine_update_current()
-{
-    float x = current_position[X_AXIS] - world2machine_shift[0];
-    float y = current_position[Y_AXIS] - world2machine_shift[1];
-    current_position[X_AXIS] = world2machine_rotation_and_skew_inv[0][0] * x + world2machine_rotation_and_skew_inv[0][1] * y;
-    current_position[Y_AXIS] = world2machine_rotation_and_skew_inv[1][0] * x + world2machine_rotation_and_skew_inv[1][1] * y;
-}
-
-static inline void go_xyz(float x, float y, float z, float fr)
-{
-    plan_buffer_line(x, y, z, current_position[E_AXIS], fr, active_extruder);
-    st_synchronize();
-}
-
-static inline void go_xy(float x, float y, float fr)
-{
-    plan_buffer_line(x, y, current_position[Z_AXIS], current_position[E_AXIS], fr, active_extruder);
-    st_synchronize();
-}
-
-static inline void go_to_current(float fr)
-{
-    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], fr, active_extruder);
-    st_synchronize();
-}
-
-static inline void update_current_position_xyz()
-{
-      current_position[X_AXIS] = st_get_position_mm(X_AXIS);
-      current_position[Y_AXIS] = st_get_position_mm(Y_AXIS);
-      current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
-      plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-}
-
-static inline void update_current_position_z()
-{
-      current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
-      plan_set_z_position(current_position[Z_AXIS]);
-}
-
-// At the current position, find the Z stop.
-inline bool find_bed_induction_sensor_point_z(float minimum_z, uint8_t n_iter) 
-{
-    SERIAL_ECHOLNPGM("find bed induction sensor point z");
-    bool endstops_enabled  = enable_endstops(true);
-    bool endstop_z_enabled = enable_z_endstop(false);
-    float z = 0.f;
-    endstop_z_hit_on_purpose();
-
-    // move down until you find the bed
-    current_position[Z_AXIS] = minimum_z;
-    go_to_current(homing_feedrate[Z_AXIS]/60);
-    // we have to let the planner know where we are right now as it is not where we said to go.
-    update_current_position_z();
-    if (! endstop_z_hit_on_purpose())
-        goto error;
-
-    for (uint8_t i = 0; i < n_iter; ++ i) {
-        // Move up the retract distance.
-        current_position[Z_AXIS] += .5f;
-        go_to_current(homing_feedrate[Z_AXIS]/60);
-        // Move back down slowly to find bed.
-        current_position[Z_AXIS] = minimum_z;
-        go_to_current(homing_feedrate[Z_AXIS]/(4*60));
-        // we have to let the planner know where we are right now as it is not where we said to go.
-        update_current_position_z();
-        if (! endstop_z_hit_on_purpose())
-            goto error;
-//        SERIAL_ECHOPGM("Bed find_bed_induction_sensor_point_z low, height: ");
-//        MYSERIAL.print(current_position[Z_AXIS], 5);
-//        SERIAL_ECHOLNPGM("");
-        z += current_position[Z_AXIS];
-    }
-    current_position[Z_AXIS] = z;
-    if (n_iter > 1)
-        current_position[Z_AXIS] /= float(n_iter);
-
-    enable_endstops(endstops_enabled);
-    enable_z_endstop(endstop_z_enabled);
-//    SERIAL_ECHOLNPGM("find_bed_induction_sensor_point_z 3");
-    return true;
-
-error:
-//    SERIAL_ECHOLNPGM("find_bed_induction_sensor_point_z 4");
-    enable_endstops(endstops_enabled);
-    enable_z_endstop(endstop_z_enabled);
-    return false;
-}
-
-// Search around the current_position[X,Y],
-// look for the induction sensor response.
-// Adjust the  current_position[X,Y,Z] to the center of the target dot and its response Z coordinate.
-#define FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS (8.f)
-#define FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS (6.f)
-#define FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP  (1.f)
-#define FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP   (0.2f)
-inline bool find_bed_induction_sensor_point_xy()
-{
-	MYSERIAL.println("find bed induction sensor point xy");
-    float feedrate = homing_feedrate[X_AXIS] / 60.f;
-    bool found = false;
-
-    {
-        float x0 = current_position[X_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
-        float x1 = current_position[X_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
-        float y0 = current_position[Y_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
-        float y1 = current_position[Y_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
-        uint8_t nsteps_y;
-        uint8_t i;
-        if (x0 < X_MIN_POS)
-            x0 = X_MIN_POS;
-        if (x1 > X_MAX_POS)
-            x1 = X_MAX_POS;
-        if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
-            y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
-        if (y1 > Y_MAX_POS)
-            y1 = Y_MAX_POS;
-        nsteps_y = int(ceil((y1 - y0) / FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP));
-
-        enable_endstops(false);
-        bool  dir_positive = true;
-
-//        go_xyz(current_position[X_AXIS], current_position[Y_AXIS], MESH_HOME_Z_SEARCH, homing_feedrate[Z_AXIS]/60);
-        go_xyz(x0, y0, current_position[Z_AXIS], feedrate);
-        // Continously lower the Z axis.
-        endstops_hit_on_purpose();
-        enable_z_endstop(true);
-        while (current_position[Z_AXIS] > -10.f) {
-            // Do nsteps_y zig-zag movements.
-            current_position[Y_AXIS] = y0;
-            for (i = 0; i < nsteps_y; current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++ i) {
-                // Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop.
-                current_position[Z_AXIS] -= FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP / float(nsteps_y);
-                go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
-                dir_positive = ! dir_positive;
-                if (endstop_z_hit_on_purpose())
-                    goto endloop;
-            }
-            for (i = 0; i < nsteps_y; current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1), ++ i) {
-                // Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop.
-                current_position[Z_AXIS] -= FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP / float(nsteps_y);
-                go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
-                dir_positive = ! dir_positive;
-                if (endstop_z_hit_on_purpose())
-                    goto endloop;
-            }
-        }
-        endloop:
-//        SERIAL_ECHOLN("First hit");
-
-        // we have to let the planner know where we are right now as it is not where we said to go.
-        update_current_position_xyz();
-
-        // Search in this plane for the first hit. Zig-zag first in X, then in Y axis.
-        for (int8_t iter = 0; iter < 3; ++ iter) {
-            if (iter > 0) {
-                // Slightly lower the Z axis to get a reliable trigger.
-                current_position[Z_AXIS] -= 0.02f;
-                go_xyz(current_position[X_AXIS], current_position[Y_AXIS], MESH_HOME_Z_SEARCH, homing_feedrate[Z_AXIS]/60);
-            }
-
-            // Do nsteps_y zig-zag movements.
-            float a, b;
-            enable_endstops(false);
-            enable_z_endstop(false);
-            current_position[Y_AXIS] = y0;
-            go_xy(x0, current_position[Y_AXIS], feedrate);
-            enable_z_endstop(true);
-            found = false;
-            for (i = 0, dir_positive = true; i < nsteps_y; current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++ i, dir_positive = ! dir_positive) {
-                go_xy(dir_positive ? x1 : x0, current_position[Y_AXIS], feedrate);
-                if (endstop_z_hit_on_purpose()) {
-                    found = true;
-                    break;
-                }
-            }
-            update_current_position_xyz();
-            if (! found) {
-//                SERIAL_ECHOLN("Search in Y - not found");
-                continue;
-            }
-//            SERIAL_ECHOLN("Search in Y - found");
-            a = current_position[Y_AXIS];
-
-            enable_z_endstop(false);
-            current_position[Y_AXIS] = y1;
-            go_xy(x0, current_position[Y_AXIS], feedrate);
-            enable_z_endstop(true);
-            found = false;
-            for (i = 0, dir_positive = true; i < nsteps_y; current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1), ++ i, dir_positive = ! dir_positive) {
-                go_xy(dir_positive ? x1 : x0, current_position[Y_AXIS], feedrate);
-                if (endstop_z_hit_on_purpose()) {
-                    found = true;
-                    break;
-                }
-            }
-            update_current_position_xyz();
-            if (! found) {
-//                SERIAL_ECHOLN("Search in Y2 - not found");
-                continue;
-            }
-//            SERIAL_ECHOLN("Search in Y2 - found");
-            b = current_position[Y_AXIS];
-            current_position[Y_AXIS] = 0.5f * (a + b);
-
-            // Search in the X direction along a cross.
-            found = false;
-            enable_z_endstop(false);
-            go_xy(x0, current_position[Y_AXIS], feedrate);
-            enable_z_endstop(true);
-            go_xy(x1, current_position[Y_AXIS], feedrate);
-            update_current_position_xyz();
-            if (! endstop_z_hit_on_purpose()) {
-//                SERIAL_ECHOLN("Search X span 0 - not found");
-                continue;
-            }
-//            SERIAL_ECHOLN("Search X span 0 - found");
-            a = current_position[X_AXIS];
-            enable_z_endstop(false);
-            go_xy(x1, current_position[Y_AXIS], feedrate);
-            enable_z_endstop(true);
-            go_xy(x0, current_position[Y_AXIS], feedrate);
-            update_current_position_xyz();
-            if (! endstop_z_hit_on_purpose()) {
-//                SERIAL_ECHOLN("Search X span 1 - not found");
-                continue;
-            }
-//            SERIAL_ECHOLN("Search X span 1 - found");
-            b = current_position[X_AXIS];
-            // Go to the center.
-            enable_z_endstop(false);
-            current_position[X_AXIS] = 0.5f * (a + b);
-            go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
-            found = true;
-
-#if 1
-            // Search in the Y direction along a cross.
-            found = false;
-            enable_z_endstop(false);
-            go_xy(current_position[X_AXIS], y0, feedrate);
-            enable_z_endstop(true);
-            go_xy(current_position[X_AXIS], y1, feedrate);
-            update_current_position_xyz();
-            if (! endstop_z_hit_on_purpose()) {
-//                SERIAL_ECHOLN("Search Y2 span 0 - not found");
-                continue;
-            }
-//            SERIAL_ECHOLN("Search Y2 span 0 - found");
-            a = current_position[Y_AXIS];
-            enable_z_endstop(false);
-            go_xy(current_position[X_AXIS], y1, feedrate);
-            enable_z_endstop(true);
-            go_xy(current_position[X_AXIS], y0, feedrate);
-            update_current_position_xyz();
-            if (! endstop_z_hit_on_purpose()) {
-//                SERIAL_ECHOLN("Search Y2 span 1 - not found");
-                continue;
-            }
-//            SERIAL_ECHOLN("Search Y2 span 1 - found");
-            b = current_position[Y_AXIS];
-            // Go to the center.
-            enable_z_endstop(false);
-            current_position[Y_AXIS] = 0.5f * (a + b);
-            go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
-            found = true;
-#endif
-            break;
-        }
-    }
-
-    enable_z_endstop(false);
-    return found;
-}
-
-// Search around the current_position[X,Y,Z].
-// It is expected, that the induction sensor is switched on at the current position.
-// Look around this center point by painting a star around the point.
-inline bool improve_bed_induction_sensor_point()
-{
-    static const float search_radius = 8.f;
-
-    bool  endstops_enabled  = enable_endstops(false);
-    bool  endstop_z_enabled = enable_z_endstop(false);
-    bool  found = false;
-    float feedrate = homing_feedrate[X_AXIS] / 60.f;
-    float center_old_x = current_position[X_AXIS];
-    float center_old_y = current_position[Y_AXIS];
-    float center_x = 0.f;
-    float center_y = 0.f;
-
-    for (uint8_t iter = 0; iter < 4; ++ iter) {
-        switch (iter) {
-        case 0:
-            destination[X_AXIS] = center_old_x - search_radius * 0.707;
-            destination[Y_AXIS] = center_old_y - search_radius * 0.707;
-            break;
-        case 1:
-            destination[X_AXIS] = center_old_x + search_radius * 0.707;
-            destination[Y_AXIS] = center_old_y + search_radius * 0.707;
-            break;
-        case 2:
-            destination[X_AXIS] = center_old_x + search_radius * 0.707;
-            destination[Y_AXIS] = center_old_y - search_radius * 0.707;
-            break;
-        case 3:
-        default:
-            destination[X_AXIS] = center_old_x - search_radius * 0.707;
-            destination[Y_AXIS] = center_old_y + search_radius * 0.707;
-            break;
-        }
-
-        // Trim the vector from center_old_[x,y] to destination[x,y] by the bed dimensions.
-        float vx = destination[X_AXIS] - center_old_x;
-        float vy = destination[Y_AXIS] - center_old_y;
-        float l  = sqrt(vx*vx+vy*vy);
-        float t;
-        if (destination[X_AXIS] < X_MIN_POS) {
-            // Exiting the bed at xmin.
-            t = (center_x - X_MIN_POS) / l;
-            destination[X_AXIS] = X_MIN_POS;
-            destination[Y_AXIS] = center_old_y + t * vy;
-        } else if (destination[X_AXIS] > X_MAX_POS) {
-            // Exiting the bed at xmax.
-            t = (X_MAX_POS - center_x) / l;
-            destination[X_AXIS] = X_MAX_POS;
-            destination[Y_AXIS] = center_old_y + t * vy;
-        }
-        if (destination[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION) {
-            // Exiting the bed at ymin.
-            t = (center_y - Y_MIN_POS_FOR_BED_CALIBRATION) / l;
-            destination[X_AXIS] = center_old_x + t * vx;
-            destination[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
-        } else if (destination[Y_AXIS] > Y_MAX_POS) {
-            // Exiting the bed at xmax.
-            t = (Y_MAX_POS - center_y) / l;
-            destination[X_AXIS] = center_old_x + t * vx;
-            destination[Y_AXIS] = Y_MAX_POS;
-        }
-
-        // Move away from the measurement point.
-        enable_endstops(false);
-        go_xy(destination[X_AXIS], destination[Y_AXIS], feedrate);
-        // Move towards the measurement point, until the induction sensor triggers.
-        enable_endstops(true);
-        go_xy(center_old_x, center_old_y, feedrate);
-        update_current_position_xyz();
-//        if (! endstop_z_hit_on_purpose()) return false;
-        center_x += current_position[X_AXIS];
-        center_y += current_position[Y_AXIS];
-    }
-
-    // Calculate the new center, move to the new center.
-    center_x /= 4.f;
-    center_y /= 4.f;
-    current_position[X_AXIS] = center_x;
-    current_position[Y_AXIS] = center_y;
-    enable_endstops(false);
-    go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
-
-    enable_endstops(endstops_enabled);
-    enable_z_endstop(endstop_z_enabled);
-    return found;
-}
-
-static inline void debug_output_point(const char *type, const float &x, const float &y, const float &z)
-{
-    SERIAL_ECHOPGM("Measured ");
-    SERIAL_ECHORPGM(type);
-    SERIAL_ECHOPGM(" ");
-    MYSERIAL.print(x, 5);
-    SERIAL_ECHOPGM(", ");
-    MYSERIAL.print(y, 5);
-    SERIAL_ECHOPGM(", ");
-    MYSERIAL.print(z, 5);
-    SERIAL_ECHOLNPGM("");
-}
-
-// Search around the current_position[X,Y,Z].
-// It is expected, that the induction sensor is switched on at the current position.
-// Look around this center point by painting a star around the point.
-#define IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS (8.f)
-inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y, int8_t verbosity_level)
-{
-    float center_old_x = current_position[X_AXIS];
-    float center_old_y = current_position[Y_AXIS];
-    float a, b;
-    bool  point_small = false;
-
-    enable_endstops(false);
-
-    {
-        float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
-        float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
-        if (x0 < X_MIN_POS)
-            x0 = X_MIN_POS;
-        if (x1 > X_MAX_POS)
-            x1 = X_MAX_POS;
-
-        // Search in the X direction along a cross.
-        enable_z_endstop(false);
-        go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
-        enable_z_endstop(true);
-        go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
-        update_current_position_xyz();
-        if (! endstop_z_hit_on_purpose()) {
-            current_position[X_AXIS] = center_old_x;
-            goto canceled;
-        }
-        a = current_position[X_AXIS];
-        enable_z_endstop(false);
-        go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
-        enable_z_endstop(true);
-        go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
-        update_current_position_xyz();
-        if (! endstop_z_hit_on_purpose()) {
-            current_position[X_AXIS] = center_old_x;
-            goto canceled;
-        }
-        b = current_position[X_AXIS];
-        if (b - a < MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
-            if (verbosity_level >= 5) {
-                SERIAL_ECHOPGM("Point width too small: ");
-                SERIAL_ECHO(b - a);
-                SERIAL_ECHOLNPGM("");
-            }
-            // We force the calibration routine to move the Z axis slightly down to make the response more pronounced.
-            if (b - a < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
-                // Don't use the new X value.
-                current_position[X_AXIS] = center_old_x;
-                goto canceled;
-            } else {
-                // Use the new value, but force the Z axis to go a bit lower.
-                point_small = true;
-            }
-        }
-        if (verbosity_level >= 5) {
-            debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
-            debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
-        }
-
-        // Go to the center.
-        enable_z_endstop(false);
-        current_position[X_AXIS] = 0.5f * (a + b);
-        go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
-    }
-
-    {
-        float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
-        float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
-        if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
-            y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
-        if (y1 > Y_MAX_POS)
-            y1 = Y_MAX_POS;
-
-        // Search in the Y direction along a cross.
-        enable_z_endstop(false);
-        go_xy(current_position[X_AXIS], y0, homing_feedrate[X_AXIS] / 60.f);
-        if (lift_z_on_min_y) {
-            // The first row of points are very close to the end stop.
-            // Lift the sensor to disengage the trigger. This is necessary because of the sensor hysteresis.
-            go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS]+1.5f, homing_feedrate[Z_AXIS] / 60.f);
-            // and go back.
-            go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS], homing_feedrate[Z_AXIS] / 60.f);
-        }
-        if (lift_z_on_min_y && (READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING) == 1) {
-            // Already triggering before we started the move.
-            // Shift the trigger point slightly outwards.
-            // a = current_position[Y_AXIS] - 1.5f;
-            a = current_position[Y_AXIS];
-        } else {
-            enable_z_endstop(true);
-            go_xy(current_position[X_AXIS], y1, homing_feedrate[X_AXIS] / 60.f);
-            update_current_position_xyz();
-            if (! endstop_z_hit_on_purpose()) {
-                current_position[Y_AXIS] = center_old_y;
-                goto canceled;
-            }
-            a = current_position[Y_AXIS];
-        }
-        enable_z_endstop(false);
-        go_xy(current_position[X_AXIS], y1, homing_feedrate[X_AXIS] / 60.f);
-        enable_z_endstop(true);
-        go_xy(current_position[X_AXIS], y0, homing_feedrate[X_AXIS] / 60.f);
-        update_current_position_xyz();
-        if (! endstop_z_hit_on_purpose()) {
-            current_position[Y_AXIS] = center_old_y;
-            goto canceled;
-        }
-        b = current_position[Y_AXIS];
-        if (b - a < MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
-            // We force the calibration routine to move the Z axis slightly down to make the response more pronounced.
-            if (verbosity_level >= 5) {
-                SERIAL_ECHOPGM("Point height too small: ");
-                SERIAL_ECHO(b - a);
-                SERIAL_ECHOLNPGM("");
-            }
-            if (b - a < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
-                // Don't use the new Y value.
-                current_position[Y_AXIS] = center_old_y;
-                goto canceled;
-            } else {
-                // Use the new value, but force the Z axis to go a bit lower.
-                point_small = true;
-            }
-        }
-        if (verbosity_level >= 5) {
-            debug_output_point(PSTR("top" ), current_position[X_AXIS], a, current_position[Z_AXIS]);
-            debug_output_point(PSTR("bottom"), current_position[X_AXIS], b, current_position[Z_AXIS]);
-        }
-
-        // Go to the center.
-        enable_z_endstop(false);
-        current_position[Y_AXIS] = 0.5f * (a + b);
-        go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
-    }
-
-    // If point is small but not too small, then force the Z axis to be lowered a bit,
-    // but use the new value. This is important when the initial position was off in one axis,
-    // for example if the initial calibration was shifted in the Y axis systematically.
-    // Then this first step will center.
-    return ! point_small;
-
-canceled:
-    // Go back to the center.
-    enable_z_endstop(false);
-    go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
-    return false;
-}
-
-// Searching the front points, where one cannot move the sensor head in front of the sensor point.
-// Searching in a zig-zag movement in a plane for the maximum width of the response.
-// This function may set the current_position[Y_AXIS] below Y_MIN_POS, if the function succeeded.
-// If this function failed, the Y coordinate will never be outside the working space.
-#define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS (4.f)
-#define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y (0.1f)
-inline bool improve_bed_induction_sensor_point3(int verbosity_level)
-{	
-    float center_old_x = current_position[X_AXIS];
-    float center_old_y = current_position[Y_AXIS];
-    float a, b;
-    bool  result = true;
-
-	if (verbosity_level >= 20) MYSERIAL.println("Improve bed induction sensor point3");
-    // Was the sensor point detected too far in the minus Y axis?
-    // If yes, the center of the induction point cannot be reached by the machine.
-    {
-        float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
-        float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
-        float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
-        float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
-        float y = y0;
-
-        if (x0 < X_MIN_POS)
-            x0 = X_MIN_POS;
-        if (x1 > X_MAX_POS)
-            x1 = X_MAX_POS;
-        if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
-            y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
-        if (y1 > Y_MAX_POS)
-            y1 = Y_MAX_POS;
-
-        if (verbosity_level >= 20) {
-            SERIAL_ECHOPGM("Initial position: ");
-            SERIAL_ECHO(center_old_x);
-            SERIAL_ECHOPGM(", ");
-            SERIAL_ECHO(center_old_y);
-            SERIAL_ECHOLNPGM("");
-        }
-
-        // Search in the positive Y direction, until a maximum diameter is found.
-        // (the next diameter is smaller than the current one.)
-        float dmax = 0.f;
-        float xmax1 = 0.f;
-        float xmax2 = 0.f;
-        for (y = y0; y < y1; y += IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
-            enable_z_endstop(false);
-            go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
-            enable_z_endstop(true);
-            go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
-            update_current_position_xyz();
-            if (! endstop_z_hit_on_purpose()) {
-                continue;
-                // SERIAL_PROTOCOLPGM("Failed 1\n");
-                // current_position[X_AXIS] = center_old_x;
-                // goto canceled;
-            }
-            a = current_position[X_AXIS];
-            enable_z_endstop(false);
-            go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
-            enable_z_endstop(true);
-            go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
-            update_current_position_xyz();
-            if (! endstop_z_hit_on_purpose()) {
-                continue;
-                // SERIAL_PROTOCOLPGM("Failed 2\n");
-                // current_position[X_AXIS] = center_old_x;
-                // goto canceled;
-            }
-            b = current_position[X_AXIS];
-            if (verbosity_level >= 5) {
-                debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
-                debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
-            }
-            float d = b - a;
-            if (d > dmax) {
-                xmax1 = 0.5f * (a + b);
-                dmax = d;
-            } else if (dmax > 0.) {
-                y0 = y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y;
-                break;
-            }
-        }
-        if (dmax == 0.) {
-            if (verbosity_level > 0)
-                SERIAL_PROTOCOLPGM("failed - not found\n");
-            current_position[X_AXIS] = center_old_x;
-            current_position[Y_AXIS] = center_old_y;
-            goto canceled;
-        }
-
-        {
-            // Find the positive Y hit. This gives the extreme Y value for the search of the maximum diameter in the -Y direction.
-            enable_z_endstop(false);
-            go_xy(xmax1, y0 + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, homing_feedrate[X_AXIS] / 60.f);
-            enable_z_endstop(true);
-            go_xy(xmax1, max(y0 - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, Y_MIN_POS_FOR_BED_CALIBRATION), homing_feedrate[X_AXIS] / 60.f);
-            update_current_position_xyz();
-            if (! endstop_z_hit_on_purpose()) {
-                current_position[Y_AXIS] = center_old_y;
-                goto canceled;
-            }
-            if (verbosity_level >= 5)
-                debug_output_point(PSTR("top" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
-            y1 = current_position[Y_AXIS];
-        }
-
-        if (y1 <= y0) {
-            // Either the induction sensor is too high, or the induction sensor target is out of reach.
-            current_position[Y_AXIS] = center_old_y;
-            goto canceled;
-        }
-
-        // Search in the negative Y direction, until a maximum diameter is found.
-        dmax = 0.f;
-        // if (y0 + 1.f < y1)
-        //    y1 = y0 + 1.f;
-        for (y = y1; y >= y0; y -= IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
-            enable_z_endstop(false);
-            go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
-            enable_z_endstop(true);
-            go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
-            update_current_position_xyz();
-            if (! endstop_z_hit_on_purpose()) {
-                continue;
-                /*
-                current_position[X_AXIS] = center_old_x;
-                SERIAL_PROTOCOLPGM("Failed 3\n");
-                goto canceled;
-                */
-            }
-            a = current_position[X_AXIS];
-            enable_z_endstop(false);
-            go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
-            enable_z_endstop(true);
-            go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
-            update_current_position_xyz();
-            if (! endstop_z_hit_on_purpose()) {
-                continue;
-                /*
-                current_position[X_AXIS] = center_old_x;
-                SERIAL_PROTOCOLPGM("Failed 4\n");
-                goto canceled;
-                */
-            }
-            b = current_position[X_AXIS];
-            if (verbosity_level >= 5) {
-                debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
-                debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
-            }
-            float d = b - a;
-            if (d > dmax) {
-                xmax2 = 0.5f * (a + b);
-                dmax = d;
-            } else if (dmax > 0.) {
-                y1 = y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y;
-                break;
-            }
-        }
-        float xmax, ymax;
-        if (dmax == 0.f) {
-            // Only the hit in the positive direction found.
-            xmax = xmax1;
-            ymax = y0;
-        } else {
-            // Both positive and negative directions found.
-            xmax = xmax2;
-            ymax = 0.5f * (y0 + y1);
-            for (; y >= y0; y -= IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
-                enable_z_endstop(false);
-                go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
-                enable_z_endstop(true);
-                go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
-                update_current_position_xyz();
-                if (! endstop_z_hit_on_purpose()) {
-                    continue;
-                    /*
-                    current_position[X_AXIS] = center_old_x;
-                    SERIAL_PROTOCOLPGM("Failed 3\n");
-                    goto canceled;
-                    */
-                }
-                a = current_position[X_AXIS];
-                enable_z_endstop(false);
-                go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
-                enable_z_endstop(true);
-                go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
-                update_current_position_xyz();
-                if (! endstop_z_hit_on_purpose()) {
-                    continue;
-                    /*
-                    current_position[X_AXIS] = center_old_x;
-                    SERIAL_PROTOCOLPGM("Failed 4\n");
-                    goto canceled;
-                    */
-                }
-                b = current_position[X_AXIS];
-                if (verbosity_level >= 5) {
-                    debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
-                    debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
-                }
-                float d = b - a;
-                if (d > dmax) {
-                    xmax = 0.5f * (a + b);
-                    ymax = y;
-                    dmax = d;
-                }
-            }
-        }
-
-        {
-            // Compare the distance in the Y+ direction with the diameter in the X direction.
-            // Find the positive Y hit once again, this time along the Y axis going through the X point with the highest diameter.
-            enable_z_endstop(false);
-            go_xy(xmax, ymax + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, homing_feedrate[X_AXIS] / 60.f);
-            enable_z_endstop(true);
-            go_xy(xmax, max(ymax - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, Y_MIN_POS_FOR_BED_CALIBRATION), homing_feedrate[X_AXIS] / 60.f);
-            update_current_position_xyz();
-            if (! endstop_z_hit_on_purpose()) {
-                current_position[Y_AXIS] = center_old_y;
-                goto canceled;
-            }
-            if (verbosity_level >= 5)
-                debug_output_point(PSTR("top" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
-            if (current_position[Y_AXIS] - Y_MIN_POS_FOR_BED_CALIBRATION < 0.5f * dmax) {
-                // Probably not even a half circle was detected. The induction point is likely too far in the minus Y direction.
-                // First verify, if the measurement has been done at a sufficient height. If no, lower the Z axis a bit.
-                if (current_position[Y_AXIS] < ymax || dmax < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
-                    if (verbosity_level >= 5) {
-                        SERIAL_ECHOPGM("Partial point diameter too small: ");
-                        SERIAL_ECHO(dmax);
-                        SERIAL_ECHOLNPGM("");
-                    }
-                    result = false;
-                } else {
-                    // Estimate the circle radius from the maximum diameter and height:
-                    float h = current_position[Y_AXIS] - ymax;
-                    float r = dmax * dmax / (8.f * h) + 0.5f * h;
-                    if (r < 0.8f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
-                        if (verbosity_level >= 5) {
-                            SERIAL_ECHOPGM("Partial point estimated radius too small: ");
-                            SERIAL_ECHO(r);
-                            SERIAL_ECHOPGM(", dmax:");
-                            SERIAL_ECHO(dmax);
-                            SERIAL_ECHOPGM(", h:");
-                            SERIAL_ECHO(h);
-                            SERIAL_ECHOLNPGM("");
-                        }
-                        result = false;
-                    } else {
-                        // The point may end up outside of the machine working space.
-                        // That is all right as it helps to improve the accuracy of the measurement point
-                        // due to averaging.
-                        // For the y correction, use an average of dmax/2 and the estimated radius.
-                        r = 0.5f * (0.5f * dmax + r);
-                        ymax = current_position[Y_AXIS] - r;
-                    }
-                }
-            } else {
-                // If the diameter of the detected spot was smaller than a minimum allowed,
-                // the induction sensor is probably too high. Returning false will force
-                // the sensor to be lowered a tiny bit.
-                result = xmax >= MIN_BED_SENSOR_POINT_RESPONSE_DMR;
-                if (y0 > Y_MIN_POS_FOR_BED_CALIBRATION + 0.2f)
-                    // Only in case both left and right y tangents are known, use them.
-                    // If y0 is close to the bed edge, it may not be symmetric to the right tangent.
-                    ymax = 0.5f * ymax + 0.25f * (y0 + y1);
-            }
-        }
-
-        // Go to the center.
-        enable_z_endstop(false);
-        current_position[X_AXIS] = xmax;
-        current_position[Y_AXIS] = ymax;
-        if (verbosity_level >= 20) {
-            SERIAL_ECHOPGM("Adjusted position: ");
-            SERIAL_ECHO(current_position[X_AXIS]);
-            SERIAL_ECHOPGM(", ");
-            SERIAL_ECHO(current_position[Y_AXIS]);
-            SERIAL_ECHOLNPGM("");
-        }
-
-        // Don't clamp current_position[Y_AXIS], because the out-of-reach Y coordinate may actually be true.
-        // Only clamp the coordinate to go.
-        go_xy(current_position[X_AXIS], max(Y_MIN_POS, current_position[Y_AXIS]), homing_feedrate[X_AXIS] / 60.f);
-        // delay_keep_alive(3000);
-    }
-
-    if (result)
-        return true;
-    // otherwise clamp the Y coordinate
-
-canceled:
-    // Go back to the center.
-    enable_z_endstop(false);
-    if (current_position[Y_AXIS] < Y_MIN_POS)
-        current_position[Y_AXIS] = Y_MIN_POS;
-    go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
-    return false;
-}
-
-// Scan the mesh bed induction points one by one by a left-right zig-zag movement,
-// write the trigger coordinates to the serial line.
-// Useful for visualizing the behavior of the bed induction detector.
-inline void scan_bed_induction_sensor_point()
-{
-    float center_old_x = current_position[X_AXIS];
-    float center_old_y = current_position[Y_AXIS];
-    float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
-    float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
-    float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
-    float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
-    float y = y0;
-
-    if (x0 < X_MIN_POS)
-        x0 = X_MIN_POS;
-    if (x1 > X_MAX_POS)
-        x1 = X_MAX_POS;
-    if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
-        y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
-    if (y1 > Y_MAX_POS)
-        y1 = Y_MAX_POS;
-
-    for (float y = y0; y < y1; y += IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
-        enable_z_endstop(false);
-        go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
-        enable_z_endstop(true);
-        go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
-        update_current_position_xyz();
-        if (endstop_z_hit_on_purpose())
-            debug_output_point(PSTR("left" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
-        enable_z_endstop(false);
-        go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
-        enable_z_endstop(true);
-        go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
-        update_current_position_xyz();
-        if (endstop_z_hit_on_purpose())
-            debug_output_point(PSTR("right"), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
-    }
-
-    enable_z_endstop(false);
-    current_position[X_AXIS] = center_old_x;
-    current_position[Y_AXIS] = center_old_y;
-    go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
-}
-
-#define MESH_BED_CALIBRATION_SHOW_LCD
-
-BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level, uint8_t &too_far_mask)
-{	
-    // Don't let the manage_inactivity() function remove power from the motors.
-    refresh_cmd_timeout();
-
-    // Reusing the z_values memory for the measurement cache.
-    // 7x7=49 floats, good for 16 (x,y,z) vectors.
-    float *pts = &mbl.z_values[0][0];
-    float *vec_x = pts + 2 * 4;
-    float *vec_y = vec_x + 2;
-    float *cntr  = vec_y + 2;
-    memset(pts, 0, sizeof(float) * 7 * 7);
-	uint8_t iteration = 0; 
-	BedSkewOffsetDetectionResultType result;
-
-//    SERIAL_ECHOLNPGM("find_bed_offset_and_skew verbosity level: ");
-//    SERIAL_ECHO(int(verbosity_level));
-//    SERIAL_ECHOPGM("");
-	
-	while (iteration < 3) {
-
-		SERIAL_ECHOPGM("Iteration: ");
-		MYSERIAL.println(int(iteration + 1));
-		if (verbosity_level >= 20) {
-		SERIAL_ECHOLNPGM("Vectors: ");
-		
-			SERIAL_ECHOPGM("vec_x[0]:");
-			MYSERIAL.print(vec_x[0], 5);
-			SERIAL_ECHOLNPGM("");
-			SERIAL_ECHOPGM("vec_x[1]:");
-			MYSERIAL.print(vec_x[1], 5);
-			SERIAL_ECHOLNPGM("");
-			SERIAL_ECHOPGM("vec_y[0]:");
-			MYSERIAL.print(vec_y[0], 5);
-			SERIAL_ECHOLNPGM("");
-			SERIAL_ECHOPGM("vec_y[1]:");
-			MYSERIAL.print(vec_y[1], 5);
-			SERIAL_ECHOLNPGM("");
-			SERIAL_ECHOPGM("cntr[0]:");
-			MYSERIAL.print(cntr[0], 5);
-			SERIAL_ECHOLNPGM("");
-			SERIAL_ECHOPGM("cntr[1]:");
-			MYSERIAL.print(cntr[1], 5);
-			SERIAL_ECHOLNPGM("");
-		}
-#ifdef MESH_BED_CALIBRATION_SHOW_LCD
-    uint8_t next_line;
-    lcd_display_message_fullscreen_P(MSG_FIND_BED_OFFSET_AND_SKEW_LINE1, next_line);
-    if (next_line > 3)
-        next_line = 3;
-#endif /* MESH_BED_CALIBRATION_SHOW_LCD */
-
-    // Collect the rear 2x3 points.
-	current_position[Z_AXIS] = MESH_HOME_Z_SEARCH + FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP * iteration * 0.3;
-	for (int k = 0; k < 4; ++k) {
-		// Don't let the manage_inactivity() function remove power from the motors.
-		refresh_cmd_timeout();
-#ifdef MESH_BED_CALIBRATION_SHOW_LCD
-		lcd_implementation_print_at(0, next_line, k + 1);
-		lcd_printPGM(MSG_FIND_BED_OFFSET_AND_SKEW_LINE2);
-
-		if (iteration > 0) {
-			lcd_print_at_PGM(0, next_line + 1, MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION);
-			lcd_implementation_print(int(iteration + 1));
-		}
-#endif /* MESH_BED_CALIBRATION_SHOW_LCD */
-		float *pt = pts + k * 2;
-		// Go up to z_initial.
-
-		go_to_current(homing_feedrate[Z_AXIS] / 60.f);
-		if (verbosity_level >= 20) {
-			// Go to Y0, wait, then go to Y-4.
-			current_position[Y_AXIS] = 0.f;
-			go_to_current(homing_feedrate[X_AXIS] / 60.f);
-			SERIAL_ECHOLNPGM("At Y0");
-			delay_keep_alive(5000);
-			current_position[Y_AXIS] = Y_MIN_POS;
-			go_to_current(homing_feedrate[X_AXIS] / 60.f);
-			SERIAL_ECHOLNPGM("At Y-4");
-			delay_keep_alive(5000);
-		}
-		// Go to the measurement point position.
-		//if (iteration == 0) {
-			current_position[X_AXIS] = pgm_read_float(bed_ref_points_4 + k * 2);
-			current_position[Y_AXIS] = pgm_read_float(bed_ref_points_4 + k * 2 + 1);
-		/*}
-		else {
-			// if first iteration failed, count corrected point coordinates as initial
-			// Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
-			
-			current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points_4 + k * 2) + vec_y[0] * pgm_read_float(bed_ref_points_4 + k * 2 + 1) + cntr[0];
-			current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points_4 + k * 2) + vec_y[1] * pgm_read_float(bed_ref_points_4 + k * 2 + 1) + cntr[1];
-
-			// The calibration points are very close to the min Y.
-			if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
-				current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
-
-		}*/
-		if (verbosity_level >= 20) {
-			SERIAL_ECHOPGM("current_position[X_AXIS]:");
-			MYSERIAL.print(current_position[X_AXIS], 5);
-			SERIAL_ECHOLNPGM("");
-			SERIAL_ECHOPGM("current_position[Y_AXIS]:");
-			MYSERIAL.print(current_position[Y_AXIS], 5);
-			SERIAL_ECHOLNPGM("");
-			SERIAL_ECHOPGM("current_position[Z_AXIS]:");
-			MYSERIAL.print(current_position[Z_AXIS], 5);
-			SERIAL_ECHOLNPGM("");
-		}
-
-
-		go_to_current(homing_feedrate[X_AXIS] / 60.f);
-		if (verbosity_level >= 10)
-			delay_keep_alive(3000);
-		if (!find_bed_induction_sensor_point_xy())
-			return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
-#if 1
-		
-			if (k == 0) {
-				// Improve the position of the 1st row sensor points by a zig-zag movement.
-				find_bed_induction_sensor_point_z();
-				int8_t i = 4;
-				for (;;) {
-					if (improve_bed_induction_sensor_point3(verbosity_level))
-						break;
-					if (--i == 0)
-						return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
-					// Try to move the Z axis down a bit to increase a chance of the sensor to trigger.
-					current_position[Z_AXIS] -= 0.025f;
-					enable_endstops(false);
-					enable_z_endstop(false);
-					go_to_current(homing_feedrate[Z_AXIS]);
-				}
-				if (i == 0)
-					// not found
-					return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
-			}
-#endif
-			if (verbosity_level >= 10)
-				delay_keep_alive(3000);
-			// Save the detected point position and then clamp the Y coordinate, which may have been estimated
-			// to lie outside the machine working space.
-			if (verbosity_level >= 20) {
-				SERIAL_ECHOLNPGM("Measured:");
-				MYSERIAL.println(current_position[X_AXIS]);
-				MYSERIAL.println(current_position[Y_AXIS]);
-			}
-			pt[0] = (pt[0] * iteration) / (iteration + 1);
-			pt[0] += (current_position[X_AXIS]/(iteration + 1)); //count average
-			pt[1] = (pt[1] * iteration) / (iteration + 1);
-			pt[1] += (current_position[Y_AXIS] / (iteration + 1));
-			
-			
-			//pt[0] += current_position[X_AXIS];
-			//if(iteration > 0) pt[0] = pt[0] / 2;
-						
-			//pt[1] += current_position[Y_AXIS];
-			//if (iteration > 0) pt[1] = pt[1] / 2;
-
-			if (verbosity_level >= 20) {
-				SERIAL_ECHOLNPGM("");
-				SERIAL_ECHOPGM("pt[0]:");
-				MYSERIAL.println(pt[0]);
-				SERIAL_ECHOPGM("pt[1]:");
-				MYSERIAL.println(pt[1]);
-			}
-
-			if (current_position[Y_AXIS] < Y_MIN_POS)
-				current_position[Y_AXIS] = Y_MIN_POS;
-			// Start searching for the other points at 3mm above the last point.
-			current_position[Z_AXIS] += 3.f + FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP * iteration * 0.3;
-			//cntr[0] += pt[0];
-			//cntr[1] += pt[1];
-			if (verbosity_level >= 10 && k == 0) {
-				// Show the zero. Test, whether the Y motor skipped steps.
-				current_position[Y_AXIS] = MANUAL_Y_HOME_POS;
-				go_to_current(homing_feedrate[X_AXIS] / 60.f);
-				delay_keep_alive(3000);
-			}
-		}
-
-		if (verbosity_level >= 20) {
-			// Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
-			delay_keep_alive(3000);
-			for (int8_t mesh_point = 0; mesh_point < 4; ++mesh_point) {
-				// Don't let the manage_inactivity() function remove power from the motors.
-				refresh_cmd_timeout();
-				// Go to the measurement point.
-				// Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
-				current_position[X_AXIS] = pts[mesh_point * 2];
-				current_position[Y_AXIS] = pts[mesh_point * 2 + 1];
-				go_to_current(homing_feedrate[X_AXIS] / 60);
-				delay_keep_alive(3000);
-			}
-		}
-
-		if (pts[1] < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
-			too_far_mask |= 1 << 1; //front center point is out of reach
-				SERIAL_ECHOLNPGM("");
-				SERIAL_ECHOPGM("WARNING: Front point not reachable. Y coordinate:");
-				MYSERIAL.print(pts[1]);
-				SERIAL_ECHOPGM(" < ");
-				MYSERIAL.println(Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
-		}
-
-		result = calculate_machine_skew_and_offset_LS(pts, 4, bed_ref_points_4, vec_x, vec_y, cntr, verbosity_level);
-		if (result >= 0) {
-			world2machine_update(vec_x, vec_y, cntr);
-#if 1
-			// Fearlessly store the calibration values into the eeprom.
-			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 0), cntr[0]);
-			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 4), cntr[1]);
-			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 0), vec_x[0]);
-			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 4), vec_x[1]);
-			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 0), vec_y[0]);
-			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 4), vec_y[1]);
-#endif
-			if (verbosity_level >= 10) {
-				// Length of the vec_x
-				float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]);
-				SERIAL_ECHOLNPGM("X vector length:");
-				MYSERIAL.println(l);
-
-				// Length of the vec_y
-				l = sqrt(vec_y[0] * vec_y[0] + vec_y[1] * vec_y[1]);
-				SERIAL_ECHOLNPGM("Y vector length:");
-				MYSERIAL.println(l);
-				// Zero point correction
-				l = sqrt(cntr[0] * cntr[0] + cntr[1] * cntr[1]);
-				SERIAL_ECHOLNPGM("Zero point correction:");
-				MYSERIAL.println(l);
-
-				// vec_x and vec_y shall be nearly perpendicular.
-				l = vec_x[0] * vec_y[0] + vec_x[1] * vec_y[1];
-				SERIAL_ECHOLNPGM("Perpendicularity");
-				MYSERIAL.println(fabs(l));
-				SERIAL_ECHOLNPGM("Saving bed calibration vectors to EEPROM");
-			}
-			// Correct the current_position to match the transformed coordinate system after world2machine_rotation_and_skew and world2machine_shift were set.
-			world2machine_update_current();
-
-			
-			if (verbosity_level >= 20) {
-				// Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
-				delay_keep_alive(3000);
-				for (int8_t mesh_point = 0; mesh_point < 9; ++mesh_point) {
-					// Don't let the manage_inactivity() function remove power from the motors.
-					refresh_cmd_timeout();
-					// Go to the measurement point.
-					// Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
-					current_position[X_AXIS] = pgm_read_float(bed_ref_points + mesh_point * 2);
-					current_position[Y_AXIS] = pgm_read_float(bed_ref_points + mesh_point * 2 + 1);
-					go_to_current(homing_feedrate[X_AXIS] / 60);
-					delay_keep_alive(3000);
-				}
-			}
-			return result;
-		}		
-		if (result == BED_SKEW_OFFSET_DETECTION_FITTING_FAILED && too_far_mask == 2) return result; //if fitting failed and front center point is out of reach, terminate calibration and inform user
-		iteration++;
-	}
-	return result;    
-}
-
-BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level, uint8_t &too_far_mask)
-{
-    // Don't let the manage_inactivity() function remove power from the motors.
-    refresh_cmd_timeout();
-
-    // Mask of the first three points. Are they too far?
-    too_far_mask = 0;
-
-    // Reusing the z_values memory for the measurement cache.
-    // 7x7=49 floats, good for 16 (x,y,z) vectors.
-    float *pts = &mbl.z_values[0][0];
-    float *vec_x = pts + 2 * 9;
-    float *vec_y = vec_x + 2;
-    float *cntr  = vec_y + 2;
-    memset(pts, 0, sizeof(float) * 7 * 7);
-
-    // Cache the current correction matrix.
-    world2machine_initialize();
-    vec_x[0] = world2machine_rotation_and_skew[0][0];
-    vec_x[1] = world2machine_rotation_and_skew[1][0];
-    vec_y[0] = world2machine_rotation_and_skew[0][1];
-    vec_y[1] = world2machine_rotation_and_skew[1][1];
-    cntr[0] = world2machine_shift[0];
-    cntr[1] = world2machine_shift[1];
-    // and reset the correction matrix, so the planner will not do anything.
-    world2machine_reset();
-
-    bool endstops_enabled  = enable_endstops(false);
-    bool endstop_z_enabled = enable_z_endstop(false);
-
-#ifdef MESH_BED_CALIBRATION_SHOW_LCD
-    uint8_t next_line;
-    lcd_display_message_fullscreen_P(MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE1, next_line);
-    if (next_line > 3)
-        next_line = 3;
-#endif /* MESH_BED_CALIBRATION_SHOW_LCD */
-
-    // Collect a matrix of 9x9 points.
-    BedSkewOffsetDetectionResultType result = BED_SKEW_OFFSET_DETECTION_PERFECT;
-    for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
-        // Don't let the manage_inactivity() function remove power from the motors.
-        refresh_cmd_timeout();
-        // Print the decrasing ID of the measurement point.
-#ifdef MESH_BED_CALIBRATION_SHOW_LCD
-        lcd_implementation_print_at(0, next_line, mesh_point+1);
-        lcd_printPGM(MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE2);
-#endif /* MESH_BED_CALIBRATION_SHOW_LCD */
-
-        // Move up.
-        current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-        enable_endstops(false);
-        enable_z_endstop(false);
-        go_to_current(homing_feedrate[Z_AXIS]/60);
-        if (verbosity_level >= 20) {
-            // Go to Y0, wait, then go to Y-4.
-            current_position[Y_AXIS] = 0.f;
-            go_to_current(homing_feedrate[X_AXIS] / 60.f);
-            SERIAL_ECHOLNPGM("At Y0");
-            delay_keep_alive(5000);
-            current_position[Y_AXIS] = Y_MIN_POS;
-            go_to_current(homing_feedrate[X_AXIS] / 60.f);
-            SERIAL_ECHOLNPGM("At Y-4");
-            delay_keep_alive(5000);
-        }
-        // Go to the measurement point.
-        // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
-        current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[0] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[0];
-        current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
-        // The calibration points are very close to the min Y.
-        if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
-            current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
-        go_to_current(homing_feedrate[X_AXIS]/60);
-        // Find its Z position by running the normal vertical search.
-        if (verbosity_level >= 10)
-            delay_keep_alive(3000);
-        find_bed_induction_sensor_point_z();
-        if (verbosity_level >= 10)
-            delay_keep_alive(3000);
-        // Try to move the Z axis down a bit to increase a chance of the sensor to trigger.
-        current_position[Z_AXIS] -= 0.025f;
-        // Improve the point position by searching its center in a current plane.
-        int8_t n_errors = 3;
-        for (int8_t iter = 0; iter < 8; ) {
-            if (verbosity_level > 20) {
-                SERIAL_ECHOPGM("Improving bed point ");
-                SERIAL_ECHO(mesh_point);
-                SERIAL_ECHOPGM(", iteration ");
-                SERIAL_ECHO(iter);
-                SERIAL_ECHOPGM(", z");
-                MYSERIAL.print(current_position[Z_AXIS], 5);
-                SERIAL_ECHOLNPGM("");
-            }
-            bool found = false;
-            if (mesh_point < 3) {
-                // Because the sensor cannot move in front of the first row
-                // of the sensor points, the y position cannot be measured
-                // by a cross center method.
-                // Use a zig-zag search for the first row of the points.
-                found = improve_bed_induction_sensor_point3(verbosity_level);
-            } else {
-                switch (method) {
-                    case 0: found = improve_bed_induction_sensor_point(); break;
-                    case 1: found = improve_bed_induction_sensor_point2(mesh_point < 3, verbosity_level); break;
-                    default: break;
-                }
-            }
-            if (found) {
-                if (iter > 3) {
-                    // Average the last 4 measurements.
-                    pts[mesh_point*2  ] += current_position[X_AXIS];
-                    pts[mesh_point*2+1] += current_position[Y_AXIS];
-                }
-                if (current_position[Y_AXIS] < Y_MIN_POS)
-                    current_position[Y_AXIS] = Y_MIN_POS;
-                ++ iter;
-            } else if (n_errors -- == 0) {
-                // Give up.
-                result = BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
-                goto canceled;
-            } else {
-                // Try to move the Z axis down a bit to increase a chance of the sensor to trigger.
-                current_position[Z_AXIS] -= 0.05f;
-                enable_endstops(false);
-                enable_z_endstop(false);
-                go_to_current(homing_feedrate[Z_AXIS]);
-                if (verbosity_level >= 5) {
-                    SERIAL_ECHOPGM("Improving bed point ");
-                    SERIAL_ECHO(mesh_point);
-                    SERIAL_ECHOPGM(", iteration ");
-                    SERIAL_ECHO(iter);
-                    SERIAL_ECHOPGM(" failed. Lowering z to ");
-                    MYSERIAL.print(current_position[Z_AXIS], 5);
-                    SERIAL_ECHOLNPGM("");
-                }
-            }
-        }
-        if (verbosity_level >= 10)
-            delay_keep_alive(3000);
-    }
-    // Don't let the manage_inactivity() function remove power from the motors.
-    refresh_cmd_timeout();
-
-    // Average the last 4 measurements.
-    for (int8_t i = 0; i < 18; ++ i)
-        pts[i] *= (1.f/4.f);
-
-    enable_endstops(false);
-    enable_z_endstop(false);
-
-    if (verbosity_level >= 5) {
-        // Test the positions. Are the positions reproducible?
-		current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-        for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
-            // Don't let the manage_inactivity() function remove power from the motors.
-            refresh_cmd_timeout();
-            // Go to the measurement point.
-            // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
-            current_position[X_AXIS] = pts[mesh_point*2];
-            current_position[Y_AXIS] = pts[mesh_point*2+1];
-            if (verbosity_level >= 10) {
-                go_to_current(homing_feedrate[X_AXIS]/60);
-                delay_keep_alive(3000);
-            }
-            SERIAL_ECHOPGM("Final measured bed point ");
-            SERIAL_ECHO(mesh_point);
-            SERIAL_ECHOPGM(": ");
-            MYSERIAL.print(current_position[X_AXIS], 5);
-            SERIAL_ECHOPGM(", ");
-            MYSERIAL.print(current_position[Y_AXIS], 5);
-            SERIAL_ECHOLNPGM("");
-        }
-    }
-
-    {
-        // First fill in the too_far_mask from the measured points.
-        for (uint8_t mesh_point = 0; mesh_point < 3; ++ mesh_point)
-            if (pts[mesh_point * 2 + 1] < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH)
-                too_far_mask |= 1 << mesh_point;
-        result = calculate_machine_skew_and_offset_LS(pts, 9, bed_ref_points, vec_x, vec_y, cntr, verbosity_level);
-        if (result < 0) {
-            SERIAL_ECHOLNPGM("Calculation of the machine skew and offset failed.");
-            goto canceled;
-        }
-        // In case of success, update the too_far_mask from the calculated points.
-        for (uint8_t mesh_point = 0; mesh_point < 3; ++ mesh_point) {
-            float y = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
-            if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH)
-                too_far_mask |= 1 << mesh_point;
-        }
-    }
-
-    world2machine_update(vec_x, vec_y, cntr);
-#if 1
-    // Fearlessly store the calibration values into the eeprom.
-    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0), cntr [0]);
-    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4), cntr [1]);
-    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0), vec_x[0]);
-    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4), vec_x[1]);
-    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0), vec_y[0]);
-    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
-#endif
-
-    // Correct the current_position to match the transformed coordinate system after world2machine_rotation_and_skew and world2machine_shift were set.
-    world2machine_update_current();
-
-    enable_endstops(false);
-    enable_z_endstop(false);
-
-    if (verbosity_level >= 5) {
-        // Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
-        delay_keep_alive(3000);
-		current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-        for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
-            // Don't let the manage_inactivity() function remove power from the motors.
-            refresh_cmd_timeout();
-            // Go to the measurement point.
-            // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
-            current_position[X_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2);
-            current_position[Y_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2+1);
-            if (verbosity_level >= 10) {
-                go_to_current(homing_feedrate[X_AXIS]/60);
-                delay_keep_alive(3000);
-            }
-            {
-                float x, y;
-                world2machine(current_position[X_AXIS], current_position[Y_AXIS], x, y);
-                SERIAL_ECHOPGM("Final calculated bed point ");
-                SERIAL_ECHO(mesh_point);
-                SERIAL_ECHOPGM(": ");
-                MYSERIAL.print(x, 5);
-                SERIAL_ECHOPGM(", ");
-                MYSERIAL.print(y, 5);
-                SERIAL_ECHOLNPGM("");
-            }
-        }
-    }
-
-    // Sample Z heights for the mesh bed leveling.
-    // In addition, store the results into an eeprom, to be used later for verification of the bed leveling process.
-    if (! sample_mesh_and_store_reference())
-        goto canceled;
-
-    enable_endstops(endstops_enabled);
-    enable_z_endstop(endstop_z_enabled);
-    // Don't let the manage_inactivity() function remove power from the motors.
-    refresh_cmd_timeout();
-    return result;
-
-canceled:
-    // Don't let the manage_inactivity() function remove power from the motors.
-    refresh_cmd_timeout();
-    // Print head up.
-    current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-    go_to_current(homing_feedrate[Z_AXIS]/60);
-    // Store the identity matrix to EEPROM.
-    reset_bed_offset_and_skew();
-    enable_endstops(endstops_enabled);
-    enable_z_endstop(endstop_z_enabled);
-    return result;
-}
-
-void go_home_with_z_lift()
-{
-    // Don't let the manage_inactivity() function remove power from the motors.
-    refresh_cmd_timeout();
-    // Go home.
-    // First move up to a safe height.
-    current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-    go_to_current(homing_feedrate[Z_AXIS]/60);
-    // Second move to XY [0, 0].
-    current_position[X_AXIS] = X_MIN_POS+0.2;
-    current_position[Y_AXIS] = Y_MIN_POS+0.2;
-    // Clamp to the physical coordinates.
-    world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
-    go_to_current(homing_feedrate[X_AXIS]/60);
-    // Third move up to a safe height.
-    current_position[Z_AXIS] = Z_MIN_POS;
-    go_to_current(homing_feedrate[Z_AXIS]/60);    
-}
-
-// Sample the 9 points of the bed and store them into the EEPROM as a reference.
-// When calling this function, the X, Y, Z axes should be already homed,
-// and the world2machine correction matrix should be active.
-// Returns false if the reference values are more than 3mm far away.
-bool sample_mesh_and_store_reference()
-{
-    bool endstops_enabled  = enable_endstops(false);
-    bool endstop_z_enabled = enable_z_endstop(false);
-
-    // Don't let the manage_inactivity() function remove power from the motors.
-    refresh_cmd_timeout();
-
-#ifdef MESH_BED_CALIBRATION_SHOW_LCD
-    uint8_t next_line;
-    lcd_display_message_fullscreen_P(MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE1, next_line);
-    if (next_line > 3)
-        next_line = 3;
-    // display "point xx of yy"
-    lcd_implementation_print_at(0, next_line, 1);
-    lcd_printPGM(MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE2);
-#endif /* MESH_BED_CALIBRATION_SHOW_LCD */
-
-    // Sample Z heights for the mesh bed leveling.
-    // In addition, store the results into an eeprom, to be used later for verification of the bed leveling process.
-    {
-        // The first point defines the reference.
-        current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-        go_to_current(homing_feedrate[Z_AXIS]/60);
-        current_position[X_AXIS] = pgm_read_float(bed_ref_points);
-        current_position[Y_AXIS] = pgm_read_float(bed_ref_points+1);
-        world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
-        go_to_current(homing_feedrate[X_AXIS]/60);
-        memcpy(destination, current_position, sizeof(destination));
-        enable_endstops(true);
-        homeaxis(Z_AXIS);
-        enable_endstops(false);
-        find_bed_induction_sensor_point_z();
-        mbl.set_z(0, 0, current_position[Z_AXIS]);
-    }
-    for (int8_t mesh_point = 1; mesh_point != MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS; ++ mesh_point) {
-        // Don't let the manage_inactivity() function remove power from the motors.
-        refresh_cmd_timeout();
-        // Print the decrasing ID of the measurement point.
-        current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-        go_to_current(homing_feedrate[Z_AXIS]/60);
-        current_position[X_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point);
-        current_position[Y_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point+1);
-        world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
-        go_to_current(homing_feedrate[X_AXIS]/60);
-#ifdef MESH_BED_CALIBRATION_SHOW_LCD
-        // display "point xx of yy"
-        lcd_implementation_print_at(0, next_line, mesh_point+1);
-        lcd_printPGM(MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE2);
-#endif /* MESH_BED_CALIBRATION_SHOW_LCD */
-        find_bed_induction_sensor_point_z();
-        // Get cords of measuring point
-        int8_t ix = mesh_point % MESH_MEAS_NUM_X_POINTS;
-        int8_t iy = mesh_point / MESH_MEAS_NUM_X_POINTS;
-        if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix; // Zig zag
-        mbl.set_z(ix, iy, current_position[Z_AXIS]);
-    }
-    {
-        // Verify the span of the Z values.
-        float zmin = mbl.z_values[0][0];
-        float zmax = zmax;
-        for (int8_t j = 0; j < 3; ++ j)
-           for (int8_t i = 0; i < 3; ++ i) {
-                zmin = min(zmin, mbl.z_values[j][i]);
-                zmax = min(zmax, mbl.z_values[j][i]);
-           }
-        if (zmax - zmin > 3.f) {
-            // The span of the Z offsets is extreme. Give up.
-            // Homing failed on some of the points.
-            SERIAL_PROTOCOLLNPGM("Exreme span of the Z values!");
-            return false;
-        }
-    }
-
-    // Store the correction values to EEPROM.
-    // Offsets of the Z heiths of the calibration points from the first point.
-    // The offsets are saved as 16bit signed int, scaled to tenths of microns.
-    {
-        uint16_t addr = EEPROM_BED_CALIBRATION_Z_JITTER;
-        for (int8_t j = 0; j < 3; ++ j)
-            for (int8_t i = 0; i < 3; ++ i) {
-                if (i == 0 && j == 0)
-                    continue;
-                float dif = mbl.z_values[j][i] - mbl.z_values[0][0];
-                int16_t dif_quantized = int16_t(floor(dif * 100.f + 0.5f));
-                eeprom_update_word((uint16_t*)addr, *reinterpret_cast<uint16_t*>(&dif_quantized));
-                #if 0
-                {
-                    uint16_t z_offset_u = eeprom_read_word((uint16_t*)addr);
-                    float dif2 = *reinterpret_cast<int16_t*>(&z_offset_u) * 0.01;
-
-                    SERIAL_ECHOPGM("Bed point ");
-                    SERIAL_ECHO(i);
-                    SERIAL_ECHOPGM(",");
-                    SERIAL_ECHO(j);
-                    SERIAL_ECHOPGM(", differences: written ");
-                    MYSERIAL.print(dif, 5);
-                    SERIAL_ECHOPGM(", read: ");
-                    MYSERIAL.print(dif2, 5);
-                    SERIAL_ECHOLNPGM("");
-                }
-                #endif
-                addr += 2;
-            }
-    }
-
-    mbl.upsample_3x3();
-    mbl.active = true;
-
-    go_home_with_z_lift();
-
-    enable_endstops(endstops_enabled);
-    enable_z_endstop(endstop_z_enabled);
-    return true;
-}
-
-bool scan_bed_induction_points(int8_t verbosity_level)
-{
-    // Don't let the manage_inactivity() function remove power from the motors.
-    refresh_cmd_timeout();
-
-    // Reusing the z_values memory for the measurement cache.
-    // 7x7=49 floats, good for 16 (x,y,z) vectors.
-    float *pts = &mbl.z_values[0][0];
-    float *vec_x = pts + 2 * 9;
-    float *vec_y = vec_x + 2;
-    float *cntr  = vec_y + 2;
-    memset(pts, 0, sizeof(float) * 7 * 7);
-
-    // Cache the current correction matrix.
-    world2machine_initialize();
-    vec_x[0] = world2machine_rotation_and_skew[0][0];
-    vec_x[1] = world2machine_rotation_and_skew[1][0];
-    vec_y[0] = world2machine_rotation_and_skew[0][1];
-    vec_y[1] = world2machine_rotation_and_skew[1][1];
-    cntr[0] = world2machine_shift[0];
-    cntr[1] = world2machine_shift[1];
-    // and reset the correction matrix, so the planner will not do anything.
-    world2machine_reset();
-
-    bool endstops_enabled  = enable_endstops(false);
-    bool endstop_z_enabled = enable_z_endstop(false);
-
-    // Collect a matrix of 9x9 points.
-    for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
-        // Don't let the manage_inactivity() function remove power from the motors.
-        refresh_cmd_timeout();
-
-        // Move up.
-        current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-        enable_endstops(false);
-        enable_z_endstop(false);
-        go_to_current(homing_feedrate[Z_AXIS]/60);
-        // Go to the measurement point.
-        // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
-        current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[0] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[0];
-        current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
-        // The calibration points are very close to the min Y.
-        if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
-            current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
-        go_to_current(homing_feedrate[X_AXIS]/60);
-        find_bed_induction_sensor_point_z();
-        scan_bed_induction_sensor_point();
-    }
-    // Don't let the manage_inactivity() function remove power from the motors.
-    refresh_cmd_timeout();
-
-    enable_endstops(false);
-    enable_z_endstop(false);
-
-    // Don't let the manage_inactivity() function remove power from the motors.
-    refresh_cmd_timeout();
-
-    enable_endstops(endstops_enabled);
-    enable_z_endstop(endstop_z_enabled);
-    return true;
-}
-
-// Shift a Z axis by a given delta.
-// To replace loading of the babystep correction.
-static void shift_z(float delta)
-{
-    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] - delta, current_position[E_AXIS], homing_feedrate[Z_AXIS]/40, active_extruder);
-    st_synchronize();
-    plan_set_z_position(current_position[Z_AXIS]);
-}
-
-#define BABYSTEP_LOADZ_BY_PLANNER
-
-// Number of baby steps applied
-static int babystepLoadZ = 0;
-
-void babystep_apply()
-{
-    // Apply Z height correction aka baby stepping before mesh bed leveling gets activated.
-    if(calibration_status() < CALIBRATION_STATUS_LIVE_ADJUST)
-	{
-		check_babystep(); //checking if babystep is in allowed range, otherwise setting babystep to 0
-		
-		// End of G80: Apply the baby stepping value.
-        EEPROM_read_B(EEPROM_BABYSTEP_Z,&babystepLoadZ);
-							
-    #if 0
-        SERIAL_ECHO("Z baby step: ");
-        SERIAL_ECHO(babystepLoadZ);
-        SERIAL_ECHO(", current Z: ");
-        SERIAL_ECHO(current_position[Z_AXIS]);
-        SERIAL_ECHO("correction: ");
-        SERIAL_ECHO(float(babystepLoadZ) / float(axis_steps_per_unit[Z_AXIS]));
-        SERIAL_ECHOLN("");
-    #endif
-    #ifdef BABYSTEP_LOADZ_BY_PLANNER
-        shift_z(- float(babystepLoadZ) / float(axis_steps_per_unit[Z_AXIS]));
-    #else
-        babystepsTodoZadd(babystepLoadZ);
-    #endif /* BABYSTEP_LOADZ_BY_PLANNER */
-    }
-}
-
-void babystep_undo()
-{
-#ifdef BABYSTEP_LOADZ_BY_PLANNER
-      shift_z(float(babystepLoadZ) / float(axis_steps_per_unit[Z_AXIS]));
-#else
-      babystepsTodoZsubtract(babystepLoadZ);
-#endif /* BABYSTEP_LOADZ_BY_PLANNER */
-      babystepLoadZ = 0;
-}
-
-void babystep_reset()
-{
-      babystepLoadZ = 0;    
+#include "Marlin.h"
+#include "Configuration.h"
+#include "ConfigurationStore.h"
+#include "language_all.h"
+#include "mesh_bed_calibration.h"
+#include "mesh_bed_leveling.h"
+#include "stepper.h"
+#include "ultralcd.h"
+
+uint8_t world2machine_correction_mode;
+float   world2machine_rotation_and_skew[2][2];
+float   world2machine_rotation_and_skew_inv[2][2];
+float   world2machine_shift[2];
+
+// Weight of the Y coordinate for the least squares fitting of the bed induction sensor targets.
+// Only used for the first row of the points, which may not befully in reach of the sensor.
+#define WEIGHT_FIRST_ROW_X_HIGH (1.f)
+#define WEIGHT_FIRST_ROW_X_LOW  (0.35f)
+#define WEIGHT_FIRST_ROW_Y_HIGH (0.3f)
+#define WEIGHT_FIRST_ROW_Y_LOW  (0.0f)
+
+#define BED_ZERO_REF_X (- 22.f + X_PROBE_OFFSET_FROM_EXTRUDER)
+#define BED_ZERO_REF_Y (- 0.6f + Y_PROBE_OFFSET_FROM_EXTRUDER)
+
+// Scaling of the real machine axes against the programmed dimensions in the firmware.
+// The correction is tiny, here around 0.5mm on 250mm length.
+//#define MACHINE_AXIS_SCALE_X ((250.f - 0.5f) / 250.f)
+//#define MACHINE_AXIS_SCALE_Y ((250.f - 0.5f) / 250.f)
+#define MACHINE_AXIS_SCALE_X 1.f
+#define MACHINE_AXIS_SCALE_Y 1.f
+
+// 0.12 degrees equals to an offset of 0.5mm on 250mm length. 
+#define BED_SKEW_ANGLE_MILD         (0.12f * M_PI / 180.f)
+// 0.25 degrees equals to an offset of 1.1mm on 250mm length.
+#define BED_SKEW_ANGLE_EXTREME      (0.25f * M_PI / 180.f)
+
+#define BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN  (0.8f)
+#define BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X  (0.8f)
+#define BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y  (1.5f)
+
+#define MIN_BED_SENSOR_POINT_RESPONSE_DMR           (2.0f)
+
+//#define Y_MIN_POS_FOR_BED_CALIBRATION (MANUAL_Y_HOME_POS-0.2f)
+#define Y_MIN_POS_FOR_BED_CALIBRATION (Y_MIN_POS)
+// Distances toward the print bed edge may not be accurate.
+#define Y_MIN_POS_CALIBRATION_POINT_ACCURATE (Y_MIN_POS + 3.f)
+// When the measured point center is out of reach of the sensor, Y coordinate will be ignored
+// by the Least Squares fitting and the X coordinate will be weighted low.
+#define Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH (Y_MIN_POS - 0.5f)
+
+// Positions of the bed reference points in the machine coordinates, referenced to the P.I.N.D.A sensor.
+// The points are ordered in a zig-zag fashion to speed up the calibration.
+const float bed_ref_points[] PROGMEM = {
+    13.f  - BED_ZERO_REF_X,   6.4f - BED_ZERO_REF_Y,
+    115.f - BED_ZERO_REF_X,   6.4f - BED_ZERO_REF_Y,
+    216.f - BED_ZERO_REF_X,   6.4f - BED_ZERO_REF_Y,
+
+    216.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
+    115.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
+    13.f  - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
+
+    13.f  - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
+    115.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
+    216.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y
+};
+
+// Positions of the bed reference points in the machine coordinates, referenced to the P.I.N.D.A sensor.
+// The points are the following: center front, center right, center rear, center left.
+const float bed_ref_points_4[] PROGMEM = {
+    115.f - BED_ZERO_REF_X,   6.4f - BED_ZERO_REF_Y,
+    216.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
+    115.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
+    13.f  - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y
+};
+
+static inline float sqr(float x) { return x * x; }
+
+static inline bool point_on_1st_row(const uint8_t i, const uint8_t npts)
+{
+	if (npts == 4) return (i == 0);
+	else return (i < 3);
+}
+
+// Weight of a point coordinate in a least squares optimization.
+// The first row of points may not be fully reachable
+// and the y values may be shortened a bit by the bed carriage
+// pulling the belt up.
+static inline float point_weight_x(const uint8_t i, const uint8_t npts, const float &y)
+{
+    float w = 1.f;
+    if (point_on_1st_row(i, npts)) {
+		if (y >= Y_MIN_POS_CALIBRATION_POINT_ACCURATE) {
+            w = WEIGHT_FIRST_ROW_X_HIGH;
+        } else if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
+            // If the point is fully outside, give it some weight.
+            w = WEIGHT_FIRST_ROW_X_LOW;
+        } else {
+            // Linearly interpolate the weight from 1 to WEIGHT_FIRST_ROW_X.
+            float t = (y - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) / (Y_MIN_POS_CALIBRATION_POINT_ACCURATE - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
+            w = (1.f - t) * WEIGHT_FIRST_ROW_X_LOW + t * WEIGHT_FIRST_ROW_X_HIGH;
+        }
+    }
+    return w;
+}
+
+// Weight of a point coordinate in a least squares optimization.
+// The first row of points may not be fully reachable
+// and the y values may be shortened a bit by the bed carriage
+// pulling the belt up.
+static inline float point_weight_y(const uint8_t i, const uint8_t npts, const float &y)
+{
+    float w = 1.f;
+    if (point_on_1st_row(i, npts)) {
+        if (y >= Y_MIN_POS_CALIBRATION_POINT_ACCURATE) {
+            w = WEIGHT_FIRST_ROW_Y_HIGH;
+        } else if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
+            // If the point is fully outside, give it some weight.
+            w = WEIGHT_FIRST_ROW_Y_LOW;
+        } else {
+            // Linearly interpolate the weight from 1 to WEIGHT_FIRST_ROW_X.
+            float t = (y - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) / (Y_MIN_POS_CALIBRATION_POINT_ACCURATE - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
+            w = (1.f - t) * WEIGHT_FIRST_ROW_Y_LOW + t * WEIGHT_FIRST_ROW_Y_HIGH;
+        }
+    }
+    return w;
+}
+
+// Non-Linear Least Squares fitting of the bed to the measured induction points
+// using the Gauss-Newton method.
+// This method will maintain a unity length of the machine axes,
+// which is the correct approach if the sensor points are not measured precisely.
+BedSkewOffsetDetectionResultType calculate_machine_skew_and_offset_LS(
+    // Matrix of maximum 9 2D points (18 floats)
+    const float  *measured_pts,
+    uint8_t       npts,
+    const float  *true_pts,
+    // Resulting correction matrix.
+    float        *vec_x,
+    float        *vec_y,
+    float        *cntr,
+    // Temporary values, 49-18-(2*3)=25 floats
+    //    , float *temp
+    int8_t        verbosity_level
+    )
+{
+    if (verbosity_level >= 10) {
+		SERIAL_ECHOLNPGM("calculate machine skew and offset LS");
+
+        // Show the initial state, before the fitting.
+        SERIAL_ECHOPGM("X vector, initial: ");
+        MYSERIAL.print(vec_x[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(vec_x[1], 5);
+        SERIAL_ECHOLNPGM("");
+
+        SERIAL_ECHOPGM("Y vector, initial: ");
+        MYSERIAL.print(vec_y[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(vec_y[1], 5);
+        SERIAL_ECHOLNPGM("");
+
+        SERIAL_ECHOPGM("center, initial: ");
+        MYSERIAL.print(cntr[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(cntr[1], 5);
+        SERIAL_ECHOLNPGM("");
+
+        for (uint8_t i = 0; i < npts; ++i) {
+            SERIAL_ECHOPGM("point #");
+            MYSERIAL.print(int(i));
+            SERIAL_ECHOPGM(" measured: (");
+            MYSERIAL.print(measured_pts[i * 2], 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(measured_pts[i * 2 + 1], 5);
+            SERIAL_ECHOPGM("); target: (");
+            MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
+            SERIAL_ECHOPGM("), error: ");
+            MYSERIAL.print(sqrt(
+                sqr(pgm_read_float(true_pts + i * 2) - measured_pts[i * 2]) +
+                sqr(pgm_read_float(true_pts + i * 2 + 1) - measured_pts[i * 2 + 1])), 5);
+            SERIAL_ECHOLNPGM("");
+        }
+        delay_keep_alive(100);
+    }
+
+    // Run some iterations of the Gauss-Newton method of non-linear least squares.
+    // Initial set of parameters:
+    // X,Y offset
+    cntr[0] = 0.f;
+    cntr[1] = 0.f;
+    // Rotation of the machine X axis from the bed X axis.
+    float a1 = 0;
+    // Rotation of the machine Y axis from the bed Y axis.
+    float a2 = 0;
+    for (int8_t iter = 0; iter < 100; ++iter) {
+        float c1 = cos(a1) * MACHINE_AXIS_SCALE_X;
+        float s1 = sin(a1) * MACHINE_AXIS_SCALE_X;
+        float c2 = cos(a2) * MACHINE_AXIS_SCALE_Y;
+        float s2 = sin(a2) * MACHINE_AXIS_SCALE_Y;
+        // Prepare the Normal equation for the Gauss-Newton method.
+        float A[4][4] = { 0.f };
+        float b[4] = { 0.f };
+        float acc;
+        for (uint8_t r = 0; r < 4; ++r) {
+            for (uint8_t c = 0; c < 4; ++c) {
+                acc = 0;
+                // J^T times J
+                for (uint8_t i = 0; i < npts; ++i) {
+                    // First for the residuum in the x axis:
+                    if (r != 1 && c != 1) {
+                        float a = 
+                             (r == 0) ? 1.f :
+                            ((r == 2) ? (-s1 * measured_pts[2 * i]) :
+                                        (-c2 * measured_pts[2 * i + 1]));
+                        float b = 
+                             (c == 0) ? 1.f :
+                            ((c == 2) ? (-s1 * measured_pts[2 * i]) :
+                                        (-c2 * measured_pts[2 * i + 1]));
+                        float w = point_weight_x(i, npts, measured_pts[2 * i + 1]);
+                        acc += a * b * w;
+                    }
+                    // Second for the residuum in the y axis. 
+                    // The first row of the points have a low weight, because their position may not be known
+                    // with a sufficient accuracy.
+                    if (r != 0 && c != 0) {
+                        float a = 
+                             (r == 1) ? 1.f :
+                            ((r == 2) ? ( c1 * measured_pts[2 * i]) :
+                                        (-s2 * measured_pts[2 * i + 1]));
+                        float b = 
+                             (c == 1) ? 1.f :
+                            ((c == 2) ? ( c1 * measured_pts[2 * i]) :
+                                        (-s2 * measured_pts[2 * i + 1]));
+                        float w = point_weight_y(i, npts, measured_pts[2 * i + 1]);
+                        acc += a * b * w;
+                    }
+                }
+                A[r][c] = acc;
+            }
+            // J^T times f(x)
+            acc = 0.f;
+            for (uint8_t i = 0; i < npts; ++i) {
+                {
+                    float j = 
+                         (r == 0) ? 1.f :
+                        ((r == 1) ? 0.f :
+                        ((r == 2) ? (-s1 * measured_pts[2 * i]) :
+                                    (-c2 * measured_pts[2 * i + 1])));
+                    float fx = c1 * measured_pts[2 * i] - s2 * measured_pts[2 * i + 1] + cntr[0] - pgm_read_float(true_pts + i * 2);
+                    float w = point_weight_x(i, npts, measured_pts[2 * i + 1]);
+                    acc += j * fx * w;
+                }
+                {
+                    float j = 
+                         (r == 0) ? 0.f :
+                        ((r == 1) ? 1.f :
+                        ((r == 2) ? ( c1 * measured_pts[2 * i]) :
+                                    (-s2 * measured_pts[2 * i + 1])));
+                    float fy = s1 * measured_pts[2 * i] + c2 * measured_pts[2 * i + 1] + cntr[1] - pgm_read_float(true_pts + i * 2 + 1);
+                    float w = point_weight_y(i, npts, measured_pts[2 * i + 1]);
+                    acc += j * fy * w;
+                }
+            }
+            b[r] = -acc;
+        }
+
+        // Solve for h by a Gauss iteration method.
+        float h[4] = { 0.f };
+        for (uint8_t gauss_iter = 0; gauss_iter < 100; ++gauss_iter) {
+            h[0] = (b[0] - A[0][1] * h[1] - A[0][2] * h[2] - A[0][3] * h[3]) / A[0][0];
+            h[1] = (b[1] - A[1][0] * h[0] - A[1][2] * h[2] - A[1][3] * h[3]) / A[1][1];
+            h[2] = (b[2] - A[2][0] * h[0] - A[2][1] * h[1] - A[2][3] * h[3]) / A[2][2];
+            h[3] = (b[3] - A[3][0] * h[0] - A[3][1] * h[1] - A[3][2] * h[2]) / A[3][3];
+        }
+
+        // and update the current position with h.
+        // It may be better to use the Levenberg-Marquart method here,
+        // but because we are very close to the solution alread,
+        // the simple Gauss-Newton non-linear Least Squares method works well enough.
+        cntr[0] += h[0];
+        cntr[1] += h[1];
+        a1 += h[2];
+        a2 += h[3];
+
+        if (verbosity_level >= 20) {
+            SERIAL_ECHOPGM("iteration: ");
+            MYSERIAL.print(int(iter));
+			SERIAL_ECHOPGM("; correction vector: ");
+            MYSERIAL.print(h[0], 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(h[1], 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(h[2], 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(h[3], 5);
+            SERIAL_ECHOLNPGM("");
+            SERIAL_ECHOPGM("corrected x/y: ");
+            MYSERIAL.print(cntr[0], 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(cntr[0], 5);
+            SERIAL_ECHOLNPGM("");
+            SERIAL_ECHOPGM("corrected angles: ");
+            MYSERIAL.print(180.f * a1 / M_PI, 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(180.f * a2 / M_PI, 5);
+            SERIAL_ECHOLNPGM("");
+        }
+    }
+
+    vec_x[0] =  cos(a1) * MACHINE_AXIS_SCALE_X;
+    vec_x[1] =  sin(a1) * MACHINE_AXIS_SCALE_X;
+    vec_y[0] = -sin(a2) * MACHINE_AXIS_SCALE_Y;
+    vec_y[1] =  cos(a2) * MACHINE_AXIS_SCALE_Y;
+
+    BedSkewOffsetDetectionResultType result = BED_SKEW_OFFSET_DETECTION_PERFECT;
+    {
+        float angleDiff = fabs(a2 - a1);
+        if (angleDiff > BED_SKEW_ANGLE_MILD)
+            result = (angleDiff > BED_SKEW_ANGLE_EXTREME) ?
+                BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME :
+                BED_SKEW_OFFSET_DETECTION_SKEW_MILD;
+        if (fabs(a1) > BED_SKEW_ANGLE_EXTREME ||
+            fabs(a2) > BED_SKEW_ANGLE_EXTREME)
+            result = BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME;
+    }
+
+    if (verbosity_level >= 1) {
+        SERIAL_ECHOPGM("correction angles: ");
+        MYSERIAL.print(180.f * a1 / M_PI, 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(180.f * a2 / M_PI, 5);
+        SERIAL_ECHOLNPGM("");
+    }
+
+    if (verbosity_level >= 10) {
+        // Show the adjusted state, before the fitting.
+        SERIAL_ECHOPGM("X vector new, inverted: ");
+        MYSERIAL.print(vec_x[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(vec_x[1], 5);
+        SERIAL_ECHOLNPGM("");
+
+        SERIAL_ECHOPGM("Y vector new, inverted: ");
+        MYSERIAL.print(vec_y[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(vec_y[1], 5);
+        SERIAL_ECHOLNPGM("");
+
+        SERIAL_ECHOPGM("center new, inverted: ");
+        MYSERIAL.print(cntr[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(cntr[1], 5);
+        SERIAL_ECHOLNPGM("");
+        delay_keep_alive(100);
+
+        SERIAL_ECHOLNPGM("Error after correction: ");
+    }
+
+    // Measure the error after correction.
+    for (uint8_t i = 0; i < npts; ++i) {
+        float x = vec_x[0] * measured_pts[i * 2] + vec_y[0] * measured_pts[i * 2 + 1] + cntr[0];
+        float y = vec_x[1] * measured_pts[i * 2] + vec_y[1] * measured_pts[i * 2 + 1] + cntr[1];
+        float errX = sqr(pgm_read_float(true_pts + i * 2) - x);
+        float errY = sqr(pgm_read_float(true_pts + i * 2 + 1) - y);
+        float err = sqrt(errX + errY);
+		if (verbosity_level >= 10) {
+			SERIAL_ECHOPGM("point #");
+			MYSERIAL.print(int(i));
+			SERIAL_ECHOLNPGM(":");
+		}
+
+		if (point_on_1st_row(i, npts)) {
+				if(verbosity_level >= 20) SERIAL_ECHOPGM("Point on first row");
+				float w = point_weight_y(i, npts, measured_pts[2 * i + 1]);
+				if (sqrt(errX) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X ||
+					(w != 0.f && sqrt(errY) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y)) {
+					result = BED_SKEW_OFFSET_DETECTION_FITTING_FAILED;
+					if (verbosity_level >= 20) {
+						SERIAL_ECHOPGM(", weigth Y: ");
+						MYSERIAL.print(w);
+						if (sqrt(errX) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X) SERIAL_ECHOPGM(", error X > max. error X");
+						if (w != 0.f && sqrt(errY) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y) SERIAL_ECHOPGM(", error Y > max. error Y");
+					}
+				}
+		}
+		else {
+			if(verbosity_level >=20 ) SERIAL_ECHOPGM("Point not on first row");
+			if (err > BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN) {
+				result = BED_SKEW_OFFSET_DETECTION_FITTING_FAILED;
+				if(verbosity_level >= 20) SERIAL_ECHOPGM(", error > max. error euclidian"); 
+			}
+        }
+        if (verbosity_level >= 10) {
+			SERIAL_ECHOLNPGM("");
+            SERIAL_ECHOPGM("measured: (");
+            MYSERIAL.print(measured_pts[i * 2], 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(measured_pts[i * 2 + 1], 5);
+            SERIAL_ECHOPGM("); corrected: (");
+            MYSERIAL.print(x, 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(y, 5);
+            SERIAL_ECHOPGM("); target: (");
+            MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
+			SERIAL_ECHOLNPGM(")");
+			SERIAL_ECHOPGM("error: ");
+            MYSERIAL.print(err);
+			SERIAL_ECHOPGM(", error X: ");
+			MYSERIAL.print(sqrt(errX));
+			SERIAL_ECHOPGM(", error Y: ");
+			MYSERIAL.print(sqrt(errY));
+			SERIAL_ECHOLNPGM("");
+			SERIAL_ECHOLNPGM("");
+        }
+    }
+	if (verbosity_level >= 20) {
+		SERIAL_ECHOLNPGM("Max. errors:");
+		SERIAL_ECHOPGM("Max. error X:");
+		MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X);
+		SERIAL_ECHOPGM("Max. error Y:");
+		MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y);
+		SERIAL_ECHOPGM("Max. error euclidian:");
+		MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN);
+		SERIAL_ECHOLNPGM("");
+	}
+
+    #if 0
+    if (result == BED_SKEW_OFFSET_DETECTION_PERFECT && fabs(a1) < BED_SKEW_ANGLE_MILD && fabs(a2) < BED_SKEW_ANGLE_MILD) {
+        if (verbosity_level > 0)
+            SERIAL_ECHOLNPGM("Very little skew detected. Disabling skew correction.");
+        // Just disable the skew correction.
+        vec_x[0] = MACHINE_AXIS_SCALE_X;
+        vec_x[1] = 0.f;
+        vec_y[0] = 0.f;
+        vec_y[1] = MACHINE_AXIS_SCALE_Y;
+    }
+    #else
+    if (result == BED_SKEW_OFFSET_DETECTION_PERFECT) {
+        if (verbosity_level > 0)
+            SERIAL_ECHOLNPGM("Very little skew detected. Orthogonalizing the axes.");
+        // Orthogonalize the axes.
+        a1 = 0.5f * (a1 + a2);
+        vec_x[0] =  cos(a1) * MACHINE_AXIS_SCALE_X;
+        vec_x[1] =  sin(a1) * MACHINE_AXIS_SCALE_X;
+        vec_y[0] = -sin(a1) * MACHINE_AXIS_SCALE_Y;
+        vec_y[1] =  cos(a1) * MACHINE_AXIS_SCALE_Y;
+        // Refresh the offset.
+        cntr[0] = 0.f;
+        cntr[1] = 0.f;
+        float wx = 0.f;
+        float wy = 0.f;
+        for (int8_t i = 0; i < npts; ++ i) {
+            float x = vec_x[0] * measured_pts[i * 2] + vec_y[0] * measured_pts[i * 2 + 1];
+            float y = vec_x[1] * measured_pts[i * 2] + vec_y[1] * measured_pts[i * 2 + 1];
+            float w = point_weight_x(i, npts, y);
+			cntr[0] += w * (pgm_read_float(true_pts + i * 2) - x);
+			wx += w;
+			if (verbosity_level >= 20) {
+				MYSERIAL.print(i);
+				SERIAL_ECHOLNPGM("");
+				SERIAL_ECHOLNPGM("Weight_x:");
+				MYSERIAL.print(w);
+				SERIAL_ECHOLNPGM("");
+				SERIAL_ECHOLNPGM("cntr[0]:");
+				MYSERIAL.print(cntr[0]);
+				SERIAL_ECHOLNPGM("");
+				SERIAL_ECHOLNPGM("wx:");
+				MYSERIAL.print(wx);
+			}
+            w = point_weight_y(i, npts, y);
+			cntr[1] += w * (pgm_read_float(true_pts + i * 2 + 1) - y);
+			wy += w;
+
+			if (verbosity_level >= 20) {
+				SERIAL_ECHOLNPGM("");
+				SERIAL_ECHOLNPGM("Weight_y:");
+				MYSERIAL.print(w);
+				SERIAL_ECHOLNPGM("");
+				SERIAL_ECHOLNPGM("cntr[1]:");
+				MYSERIAL.print(cntr[1]);
+				SERIAL_ECHOLNPGM("");
+				SERIAL_ECHOLNPGM("wy:");
+				MYSERIAL.print(wy);
+				SERIAL_ECHOLNPGM("");
+				SERIAL_ECHOLNPGM("");
+			}
+		}
+        cntr[0] /= wx;
+        cntr[1] /= wy;
+		if (verbosity_level >= 20) {
+			SERIAL_ECHOLNPGM("");
+			SERIAL_ECHOLNPGM("Final cntr values:");
+			SERIAL_ECHOLNPGM("cntr[0]:");
+			MYSERIAL.print(cntr[0]);
+			SERIAL_ECHOLNPGM("");
+			SERIAL_ECHOLNPGM("cntr[1]:");
+			MYSERIAL.print(cntr[1]);
+			SERIAL_ECHOLNPGM("");
+		}
+
+    }
+    #endif
+
+    // Invert the transformation matrix made of vec_x, vec_y and cntr.
+    {
+        float d = vec_x[0] * vec_y[1] - vec_x[1] * vec_y[0];
+        float Ainv[2][2] = {
+            { vec_y[1] / d, -vec_y[0] / d },
+            { -vec_x[1] / d, vec_x[0] / d }
+        };
+        float cntrInv[2] = {
+            -Ainv[0][0] * cntr[0] - Ainv[0][1] * cntr[1],
+            -Ainv[1][0] * cntr[0] - Ainv[1][1] * cntr[1]
+        };
+        vec_x[0] = Ainv[0][0];
+        vec_x[1] = Ainv[1][0];
+        vec_y[0] = Ainv[0][1];
+        vec_y[1] = Ainv[1][1];
+        cntr[0] = cntrInv[0];
+        cntr[1] = cntrInv[1];
+    }
+
+    if (verbosity_level >= 1) {
+        // Show the adjusted state, before the fitting.
+        SERIAL_ECHOPGM("X vector, adjusted: ");
+        MYSERIAL.print(vec_x[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(vec_x[1], 5);
+        SERIAL_ECHOLNPGM("");
+
+        SERIAL_ECHOPGM("Y vector, adjusted: ");
+        MYSERIAL.print(vec_y[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(vec_y[1], 5);
+        SERIAL_ECHOLNPGM("");
+
+        SERIAL_ECHOPGM("center, adjusted: ");
+        MYSERIAL.print(cntr[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(cntr[1], 5);
+        SERIAL_ECHOLNPGM("");
+        delay_keep_alive(100);
+    }
+
+    if (verbosity_level >= 2) {
+        SERIAL_ECHOLNPGM("Difference after correction: ");
+        for (uint8_t i = 0; i < npts; ++i) {
+            float x = vec_x[0] * pgm_read_float(true_pts + i * 2) + vec_y[0] * pgm_read_float(true_pts + i * 2 + 1) + cntr[0];
+            float y = vec_x[1] * pgm_read_float(true_pts + i * 2) + vec_y[1] * pgm_read_float(true_pts + i * 2 + 1) + cntr[1];
+            SERIAL_ECHOPGM("point #");
+            MYSERIAL.print(int(i));
+            SERIAL_ECHOPGM("measured: (");
+            MYSERIAL.print(measured_pts[i * 2], 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(measured_pts[i * 2 + 1], 5);
+            SERIAL_ECHOPGM("); measured-corrected: (");
+            MYSERIAL.print(x, 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(y, 5);
+            SERIAL_ECHOPGM("); target: (");
+            MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
+            SERIAL_ECHOPGM("), error: ");
+            MYSERIAL.print(sqrt(sqr(measured_pts[i * 2] - x) + sqr(measured_pts[i * 2 + 1] - y)));
+            SERIAL_ECHOLNPGM("");
+        }
+		if (verbosity_level >= 20) {
+			SERIAL_ECHOLNPGM("");
+			SERIAL_ECHOLNPGM("Calculate offset and skew returning result:");
+			MYSERIAL.print(int(result));
+			SERIAL_ECHOLNPGM("");
+			SERIAL_ECHOLNPGM("");
+		}
+        delay_keep_alive(100);
+    }
+
+    return result;
+}
+
+void reset_bed_offset_and_skew()
+{
+    eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+0), 0x0FFFFFFFF);
+    eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+4), 0x0FFFFFFFF);
+    eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +0), 0x0FFFFFFFF);
+    eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +4), 0x0FFFFFFFF);
+    eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +0), 0x0FFFFFFFF);
+    eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +4), 0x0FFFFFFFF);
+
+    // Reset the 8 16bit offsets.
+    for (int8_t i = 0; i < 4; ++ i)
+        eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_Z_JITTER+i*4), 0x0FFFFFFFF);
+}
+
+bool is_bed_z_jitter_data_valid()
+// offsets of the Z heiths of the calibration points from the first point are saved as 16bit signed int, scaled to tenths of microns
+{
+    for (int8_t i = 0; i < 8; ++ i)
+        if (eeprom_read_word((uint16_t*)(EEPROM_BED_CALIBRATION_Z_JITTER+i*2)) == 0x0FFFF)
+            return false;
+    return true;
+}
+
+static void world2machine_update(const float vec_x[2], const float vec_y[2], const float cntr[2])
+{
+    world2machine_rotation_and_skew[0][0] = vec_x[0];
+    world2machine_rotation_and_skew[1][0] = vec_x[1];
+    world2machine_rotation_and_skew[0][1] = vec_y[0];
+    world2machine_rotation_and_skew[1][1] = vec_y[1];
+    world2machine_shift[0] = cntr[0];
+    world2machine_shift[1] = cntr[1];
+    // No correction.
+    world2machine_correction_mode = WORLD2MACHINE_CORRECTION_NONE;
+    if (world2machine_shift[0] != 0.f || world2machine_shift[1] != 0.f)
+        // Shift correction.
+        world2machine_correction_mode |= WORLD2MACHINE_CORRECTION_SHIFT;
+    if (world2machine_rotation_and_skew[0][0] != 1.f || world2machine_rotation_and_skew[0][1] != 0.f ||
+        world2machine_rotation_and_skew[1][0] != 0.f || world2machine_rotation_and_skew[1][1] != 1.f) {
+        // Rotation & skew correction.
+        world2machine_correction_mode |= WORLD2MACHINE_CORRECTION_SKEW;
+        // Invert the world2machine matrix.
+        float d = world2machine_rotation_and_skew[0][0] * world2machine_rotation_and_skew[1][1] - world2machine_rotation_and_skew[1][0] * world2machine_rotation_and_skew[0][1];
+        world2machine_rotation_and_skew_inv[0][0] =  world2machine_rotation_and_skew[1][1] / d;
+        world2machine_rotation_and_skew_inv[0][1] = -world2machine_rotation_and_skew[0][1] / d;
+        world2machine_rotation_and_skew_inv[1][0] = -world2machine_rotation_and_skew[1][0] / d;
+        world2machine_rotation_and_skew_inv[1][1] =  world2machine_rotation_and_skew[0][0] / d;
+    } else {
+        world2machine_rotation_and_skew_inv[0][0] = 1.f;
+        world2machine_rotation_and_skew_inv[0][1] = 0.f;
+        world2machine_rotation_and_skew_inv[1][0] = 0.f;
+        world2machine_rotation_and_skew_inv[1][1] = 1.f;
+    }
+}
+
+void world2machine_reset()
+{
+    const float vx[] = { 1.f, 0.f };
+    const float vy[] = { 0.f, 1.f };
+    const float cntr[] = { 0.f, 0.f };
+    world2machine_update(vx, vy, cntr);
+}
+
+void world2machine_revert_to_uncorrected()
+{
+    if (world2machine_correction_mode != WORLD2MACHINE_CORRECTION_NONE) {
+        // Reset the machine correction matrix.
+        const float vx[] = { 1.f, 0.f };
+        const float vy[] = { 0.f, 1.f };
+        const float cntr[] = { 0.f, 0.f };
+        world2machine_update(vx, vy, cntr);
+        // Wait for the motors to stop and update the current position with the absolute values.
+        st_synchronize();
+        current_position[X_AXIS] = st_get_position_mm(X_AXIS);
+        current_position[Y_AXIS] = st_get_position_mm(Y_AXIS);
+    }
+}
+
+static inline bool vec_undef(const float v[2])
+{
+    const uint32_t *vx = (const uint32_t*)v;
+    return vx[0] == 0x0FFFFFFFF || vx[1] == 0x0FFFFFFFF;
+}
+
+void world2machine_initialize()
+{
+    SERIAL_ECHOLNPGM("world2machine_initialize");
+    float cntr[2] = {
+        eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0)),
+        eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4))
+    };
+    float vec_x[2] = {
+        eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0)),
+        eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4))
+    };
+    float vec_y[2] = {
+        eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0)),
+        eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4))
+    };
+
+    bool reset = false;
+    if (vec_undef(cntr) || vec_undef(vec_x) || vec_undef(vec_y)) {
+        SERIAL_ECHOLNPGM("Undefined bed correction matrix.");
+        reset = true;
+    }
+    else {
+        // Length of the vec_x shall be close to unity.
+        float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]);
+        if (l < 0.9 || l > 1.1) {
+			SERIAL_ECHOLNPGM("X vector length:");
+			MYSERIAL.println(l);
+            SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the X vector out of range.");
+            reset = true;
+        }
+        // Length of the vec_y shall be close to unity.
+        l = sqrt(vec_y[0] * vec_y[0] + vec_y[1] * vec_y[1]);
+        if (l < 0.9 || l > 1.1) {
+			SERIAL_ECHOLNPGM("Y vector length:");
+			MYSERIAL.println(l);
+            SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the Y vector out of range.");
+            reset = true;
+        }
+        // Correction of the zero point shall be reasonably small.
+        l = sqrt(cntr[0] * cntr[0] + cntr[1] * cntr[1]);
+        if (l > 15.f) {
+			SERIAL_ECHOLNPGM("Zero point correction:");
+			MYSERIAL.println(l);
+            SERIAL_ECHOLNPGM("Invalid bed correction matrix. Shift out of range.");
+            reset = true;
+        }
+        // vec_x and vec_y shall be nearly perpendicular.
+        l = vec_x[0] * vec_y[0] + vec_x[1] * vec_y[1];
+        if (fabs(l) > 0.1f) {
+            SERIAL_ECHOLNPGM("Invalid bed correction matrix. X/Y axes are far from being perpendicular.");
+            reset = true;
+        }
+    }
+
+    if (reset) {
+        SERIAL_ECHOLNPGM("Invalid bed correction matrix. Resetting to identity.");
+        reset_bed_offset_and_skew();
+        world2machine_reset();
+    } else {
+        world2machine_update(vec_x, vec_y, cntr);
+        /*
+        SERIAL_ECHOPGM("world2machine_initialize() loaded: ");
+        MYSERIAL.print(world2machine_rotation_and_skew[0][0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(world2machine_rotation_and_skew[0][1], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(world2machine_rotation_and_skew[1][0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(world2machine_rotation_and_skew[1][1], 5);
+        SERIAL_ECHOPGM(", offset ");
+        MYSERIAL.print(world2machine_shift[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(world2machine_shift[1], 5);
+        SERIAL_ECHOLNPGM("");
+        */
+    }
+}
+
+// When switching from absolute to corrected coordinates,
+// this will get the absolute coordinates from the servos,
+// applies the inverse world2machine transformation
+// and stores the result into current_position[x,y].
+void world2machine_update_current()
+{
+    float x = current_position[X_AXIS] - world2machine_shift[0];
+    float y = current_position[Y_AXIS] - world2machine_shift[1];
+    current_position[X_AXIS] = world2machine_rotation_and_skew_inv[0][0] * x + world2machine_rotation_and_skew_inv[0][1] * y;
+    current_position[Y_AXIS] = world2machine_rotation_and_skew_inv[1][0] * x + world2machine_rotation_and_skew_inv[1][1] * y;
+}
+
+static inline void go_xyz(float x, float y, float z, float fr)
+{
+    plan_buffer_line(x, y, z, current_position[E_AXIS], fr, active_extruder);
+    st_synchronize();
+}
+
+static inline void go_xy(float x, float y, float fr)
+{
+    plan_buffer_line(x, y, current_position[Z_AXIS], current_position[E_AXIS], fr, active_extruder);
+    st_synchronize();
+}
+
+static inline void go_to_current(float fr)
+{
+    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], fr, active_extruder);
+    st_synchronize();
+}
+
+static inline void update_current_position_xyz()
+{
+      current_position[X_AXIS] = st_get_position_mm(X_AXIS);
+      current_position[Y_AXIS] = st_get_position_mm(Y_AXIS);
+      current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
+      plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+}
+
+static inline void update_current_position_z()
+{
+      current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
+      plan_set_z_position(current_position[Z_AXIS]);
+}
+
+// At the current position, find the Z stop.
+inline bool find_bed_induction_sensor_point_z(float minimum_z, uint8_t n_iter) 
+{
+    SERIAL_ECHOLNPGM("find bed induction sensor point z");
+    bool endstops_enabled  = enable_endstops(true);
+    bool endstop_z_enabled = enable_z_endstop(false);
+    float z = 0.f;
+    endstop_z_hit_on_purpose();
+
+    // move down until you find the bed
+    current_position[Z_AXIS] = minimum_z;
+    go_to_current(homing_feedrate[Z_AXIS]/60);
+    // we have to let the planner know where we are right now as it is not where we said to go.
+    update_current_position_z();
+    if (! endstop_z_hit_on_purpose())
+        goto error;
+
+    for (uint8_t i = 0; i < n_iter; ++ i) {
+        // Move up the retract distance.
+        current_position[Z_AXIS] += .5f;
+        go_to_current(homing_feedrate[Z_AXIS]/60);
+        // Move back down slowly to find bed.
+        current_position[Z_AXIS] = minimum_z;
+        go_to_current(homing_feedrate[Z_AXIS]/(4*60));
+        // we have to let the planner know where we are right now as it is not where we said to go.
+        update_current_position_z();
+        if (! endstop_z_hit_on_purpose())
+            goto error;
+//        SERIAL_ECHOPGM("Bed find_bed_induction_sensor_point_z low, height: ");
+//        MYSERIAL.print(current_position[Z_AXIS], 5);
+//        SERIAL_ECHOLNPGM("");
+        z += current_position[Z_AXIS];
+    }
+    current_position[Z_AXIS] = z;
+    if (n_iter > 1)
+        current_position[Z_AXIS] /= float(n_iter);
+
+    enable_endstops(endstops_enabled);
+    enable_z_endstop(endstop_z_enabled);
+//    SERIAL_ECHOLNPGM("find_bed_induction_sensor_point_z 3");
+    return true;
+
+error:
+//    SERIAL_ECHOLNPGM("find_bed_induction_sensor_point_z 4");
+    enable_endstops(endstops_enabled);
+    enable_z_endstop(endstop_z_enabled);
+    return false;
+}
+
+// Search around the current_position[X,Y],
+// look for the induction sensor response.
+// Adjust the  current_position[X,Y,Z] to the center of the target dot and its response Z coordinate.
+#define FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS (8.f)
+#define FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS (6.f)
+#define FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP  (1.f)
+#define FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP   (0.2f)
+inline bool find_bed_induction_sensor_point_xy()
+{
+	MYSERIAL.println("find bed induction sensor point xy");
+    float feedrate = homing_feedrate[X_AXIS] / 60.f;
+    bool found = false;
+
+    {
+        float x0 = current_position[X_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
+        float x1 = current_position[X_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
+        float y0 = current_position[Y_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
+        float y1 = current_position[Y_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
+        uint8_t nsteps_y;
+        uint8_t i;
+        if (x0 < X_MIN_POS)
+            x0 = X_MIN_POS;
+        if (x1 > X_MAX_POS)
+            x1 = X_MAX_POS;
+        if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
+            y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
+        if (y1 > Y_MAX_POS)
+            y1 = Y_MAX_POS;
+        nsteps_y = int(ceil((y1 - y0) / FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP));
+
+        enable_endstops(false);
+        bool  dir_positive = true;
+
+//        go_xyz(current_position[X_AXIS], current_position[Y_AXIS], MESH_HOME_Z_SEARCH, homing_feedrate[Z_AXIS]/60);
+        go_xyz(x0, y0, current_position[Z_AXIS], feedrate);
+        // Continously lower the Z axis.
+        endstops_hit_on_purpose();
+        enable_z_endstop(true);
+        while (current_position[Z_AXIS] > -10.f) {
+            // Do nsteps_y zig-zag movements.
+            current_position[Y_AXIS] = y0;
+            for (i = 0; i < nsteps_y; current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++ i) {
+                // Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop.
+                current_position[Z_AXIS] -= FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP / float(nsteps_y);
+                go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
+                dir_positive = ! dir_positive;
+                if (endstop_z_hit_on_purpose())
+                    goto endloop;
+            }
+            for (i = 0; i < nsteps_y; current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1), ++ i) {
+                // Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop.
+                current_position[Z_AXIS] -= FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP / float(nsteps_y);
+                go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
+                dir_positive = ! dir_positive;
+                if (endstop_z_hit_on_purpose())
+                    goto endloop;
+            }
+        }
+        endloop:
+//        SERIAL_ECHOLN("First hit");
+
+        // we have to let the planner know where we are right now as it is not where we said to go.
+        update_current_position_xyz();
+
+        // Search in this plane for the first hit. Zig-zag first in X, then in Y axis.
+        for (int8_t iter = 0; iter < 3; ++ iter) {
+            if (iter > 0) {
+                // Slightly lower the Z axis to get a reliable trigger.
+                current_position[Z_AXIS] -= 0.02f;
+                go_xyz(current_position[X_AXIS], current_position[Y_AXIS], MESH_HOME_Z_SEARCH, homing_feedrate[Z_AXIS]/60);
+            }
+
+            // Do nsteps_y zig-zag movements.
+            float a, b;
+            enable_endstops(false);
+            enable_z_endstop(false);
+            current_position[Y_AXIS] = y0;
+            go_xy(x0, current_position[Y_AXIS], feedrate);
+            enable_z_endstop(true);
+            found = false;
+            for (i = 0, dir_positive = true; i < nsteps_y; current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++ i, dir_positive = ! dir_positive) {
+                go_xy(dir_positive ? x1 : x0, current_position[Y_AXIS], feedrate);
+                if (endstop_z_hit_on_purpose()) {
+                    found = true;
+                    break;
+                }
+            }
+            update_current_position_xyz();
+            if (! found) {
+//                SERIAL_ECHOLN("Search in Y - not found");
+                continue;
+            }
+//            SERIAL_ECHOLN("Search in Y - found");
+            a = current_position[Y_AXIS];
+
+            enable_z_endstop(false);
+            current_position[Y_AXIS] = y1;
+            go_xy(x0, current_position[Y_AXIS], feedrate);
+            enable_z_endstop(true);
+            found = false;
+            for (i = 0, dir_positive = true; i < nsteps_y; current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1), ++ i, dir_positive = ! dir_positive) {
+                go_xy(dir_positive ? x1 : x0, current_position[Y_AXIS], feedrate);
+                if (endstop_z_hit_on_purpose()) {
+                    found = true;
+                    break;
+                }
+            }
+            update_current_position_xyz();
+            if (! found) {
+//                SERIAL_ECHOLN("Search in Y2 - not found");
+                continue;
+            }
+//            SERIAL_ECHOLN("Search in Y2 - found");
+            b = current_position[Y_AXIS];
+            current_position[Y_AXIS] = 0.5f * (a + b);
+
+            // Search in the X direction along a cross.
+            found = false;
+            enable_z_endstop(false);
+            go_xy(x0, current_position[Y_AXIS], feedrate);
+            enable_z_endstop(true);
+            go_xy(x1, current_position[Y_AXIS], feedrate);
+            update_current_position_xyz();
+            if (! endstop_z_hit_on_purpose()) {
+//                SERIAL_ECHOLN("Search X span 0 - not found");
+                continue;
+            }
+//            SERIAL_ECHOLN("Search X span 0 - found");
+            a = current_position[X_AXIS];
+            enable_z_endstop(false);
+            go_xy(x1, current_position[Y_AXIS], feedrate);
+            enable_z_endstop(true);
+            go_xy(x0, current_position[Y_AXIS], feedrate);
+            update_current_position_xyz();
+            if (! endstop_z_hit_on_purpose()) {
+//                SERIAL_ECHOLN("Search X span 1 - not found");
+                continue;
+            }
+//            SERIAL_ECHOLN("Search X span 1 - found");
+            b = current_position[X_AXIS];
+            // Go to the center.
+            enable_z_endstop(false);
+            current_position[X_AXIS] = 0.5f * (a + b);
+            go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
+            found = true;
+
+#if 1
+            // Search in the Y direction along a cross.
+            found = false;
+            enable_z_endstop(false);
+            go_xy(current_position[X_AXIS], y0, feedrate);
+            enable_z_endstop(true);
+            go_xy(current_position[X_AXIS], y1, feedrate);
+            update_current_position_xyz();
+            if (! endstop_z_hit_on_purpose()) {
+//                SERIAL_ECHOLN("Search Y2 span 0 - not found");
+                continue;
+            }
+//            SERIAL_ECHOLN("Search Y2 span 0 - found");
+            a = current_position[Y_AXIS];
+            enable_z_endstop(false);
+            go_xy(current_position[X_AXIS], y1, feedrate);
+            enable_z_endstop(true);
+            go_xy(current_position[X_AXIS], y0, feedrate);
+            update_current_position_xyz();
+            if (! endstop_z_hit_on_purpose()) {
+//                SERIAL_ECHOLN("Search Y2 span 1 - not found");
+                continue;
+            }
+//            SERIAL_ECHOLN("Search Y2 span 1 - found");
+            b = current_position[Y_AXIS];
+            // Go to the center.
+            enable_z_endstop(false);
+            current_position[Y_AXIS] = 0.5f * (a + b);
+            go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
+            found = true;
+#endif
+            break;
+        }
+    }
+
+    enable_z_endstop(false);
+    return found;
+}
+
+// Search around the current_position[X,Y,Z].
+// It is expected, that the induction sensor is switched on at the current position.
+// Look around this center point by painting a star around the point.
+inline bool improve_bed_induction_sensor_point()
+{
+    static const float search_radius = 8.f;
+
+    bool  endstops_enabled  = enable_endstops(false);
+    bool  endstop_z_enabled = enable_z_endstop(false);
+    bool  found = false;
+    float feedrate = homing_feedrate[X_AXIS] / 60.f;
+    float center_old_x = current_position[X_AXIS];
+    float center_old_y = current_position[Y_AXIS];
+    float center_x = 0.f;
+    float center_y = 0.f;
+
+    for (uint8_t iter = 0; iter < 4; ++ iter) {
+        switch (iter) {
+        case 0:
+            destination[X_AXIS] = center_old_x - search_radius * 0.707;
+            destination[Y_AXIS] = center_old_y - search_radius * 0.707;
+            break;
+        case 1:
+            destination[X_AXIS] = center_old_x + search_radius * 0.707;
+            destination[Y_AXIS] = center_old_y + search_radius * 0.707;
+            break;
+        case 2:
+            destination[X_AXIS] = center_old_x + search_radius * 0.707;
+            destination[Y_AXIS] = center_old_y - search_radius * 0.707;
+            break;
+        case 3:
+        default:
+            destination[X_AXIS] = center_old_x - search_radius * 0.707;
+            destination[Y_AXIS] = center_old_y + search_radius * 0.707;
+            break;
+        }
+
+        // Trim the vector from center_old_[x,y] to destination[x,y] by the bed dimensions.
+        float vx = destination[X_AXIS] - center_old_x;
+        float vy = destination[Y_AXIS] - center_old_y;
+        float l  = sqrt(vx*vx+vy*vy);
+        float t;
+        if (destination[X_AXIS] < X_MIN_POS) {
+            // Exiting the bed at xmin.
+            t = (center_x - X_MIN_POS) / l;
+            destination[X_AXIS] = X_MIN_POS;
+            destination[Y_AXIS] = center_old_y + t * vy;
+        } else if (destination[X_AXIS] > X_MAX_POS) {
+            // Exiting the bed at xmax.
+            t = (X_MAX_POS - center_x) / l;
+            destination[X_AXIS] = X_MAX_POS;
+            destination[Y_AXIS] = center_old_y + t * vy;
+        }
+        if (destination[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION) {
+            // Exiting the bed at ymin.
+            t = (center_y - Y_MIN_POS_FOR_BED_CALIBRATION) / l;
+            destination[X_AXIS] = center_old_x + t * vx;
+            destination[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
+        } else if (destination[Y_AXIS] > Y_MAX_POS) {
+            // Exiting the bed at xmax.
+            t = (Y_MAX_POS - center_y) / l;
+            destination[X_AXIS] = center_old_x + t * vx;
+            destination[Y_AXIS] = Y_MAX_POS;
+        }
+
+        // Move away from the measurement point.
+        enable_endstops(false);
+        go_xy(destination[X_AXIS], destination[Y_AXIS], feedrate);
+        // Move towards the measurement point, until the induction sensor triggers.
+        enable_endstops(true);
+        go_xy(center_old_x, center_old_y, feedrate);
+        update_current_position_xyz();
+//        if (! endstop_z_hit_on_purpose()) return false;
+        center_x += current_position[X_AXIS];
+        center_y += current_position[Y_AXIS];
+    }
+
+    // Calculate the new center, move to the new center.
+    center_x /= 4.f;
+    center_y /= 4.f;
+    current_position[X_AXIS] = center_x;
+    current_position[Y_AXIS] = center_y;
+    enable_endstops(false);
+    go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
+
+    enable_endstops(endstops_enabled);
+    enable_z_endstop(endstop_z_enabled);
+    return found;
+}
+
+static inline void debug_output_point(const char *type, const float &x, const float &y, const float &z)
+{
+    SERIAL_ECHOPGM("Measured ");
+    SERIAL_ECHORPGM(type);
+    SERIAL_ECHOPGM(" ");
+    MYSERIAL.print(x, 5);
+    SERIAL_ECHOPGM(", ");
+    MYSERIAL.print(y, 5);
+    SERIAL_ECHOPGM(", ");
+    MYSERIAL.print(z, 5);
+    SERIAL_ECHOLNPGM("");
+}
+
+// Search around the current_position[X,Y,Z].
+// It is expected, that the induction sensor is switched on at the current position.
+// Look around this center point by painting a star around the point.
+#define IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS (8.f)
+inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y, int8_t verbosity_level)
+{
+    float center_old_x = current_position[X_AXIS];
+    float center_old_y = current_position[Y_AXIS];
+    float a, b;
+    bool  point_small = false;
+
+    enable_endstops(false);
+
+    {
+        float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
+        float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
+        if (x0 < X_MIN_POS)
+            x0 = X_MIN_POS;
+        if (x1 > X_MAX_POS)
+            x1 = X_MAX_POS;
+
+        // Search in the X direction along a cross.
+        enable_z_endstop(false);
+        go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
+        enable_z_endstop(true);
+        go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
+        update_current_position_xyz();
+        if (! endstop_z_hit_on_purpose()) {
+            current_position[X_AXIS] = center_old_x;
+            goto canceled;
+        }
+        a = current_position[X_AXIS];
+        enable_z_endstop(false);
+        go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
+        enable_z_endstop(true);
+        go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
+        update_current_position_xyz();
+        if (! endstop_z_hit_on_purpose()) {
+            current_position[X_AXIS] = center_old_x;
+            goto canceled;
+        }
+        b = current_position[X_AXIS];
+        if (b - a < MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
+            if (verbosity_level >= 5) {
+                SERIAL_ECHOPGM("Point width too small: ");
+                SERIAL_ECHO(b - a);
+                SERIAL_ECHOLNPGM("");
+            }
+            // We force the calibration routine to move the Z axis slightly down to make the response more pronounced.
+            if (b - a < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
+                // Don't use the new X value.
+                current_position[X_AXIS] = center_old_x;
+                goto canceled;
+            } else {
+                // Use the new value, but force the Z axis to go a bit lower.
+                point_small = true;
+            }
+        }
+        if (verbosity_level >= 5) {
+            debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
+            debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
+        }
+
+        // Go to the center.
+        enable_z_endstop(false);
+        current_position[X_AXIS] = 0.5f * (a + b);
+        go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
+    }
+
+    {
+        float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
+        float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
+        if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
+            y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
+        if (y1 > Y_MAX_POS)
+            y1 = Y_MAX_POS;
+
+        // Search in the Y direction along a cross.
+        enable_z_endstop(false);
+        go_xy(current_position[X_AXIS], y0, homing_feedrate[X_AXIS] / 60.f);
+        if (lift_z_on_min_y) {
+            // The first row of points are very close to the end stop.
+            // Lift the sensor to disengage the trigger. This is necessary because of the sensor hysteresis.
+            go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS]+1.5f, homing_feedrate[Z_AXIS] / 60.f);
+            // and go back.
+            go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS], homing_feedrate[Z_AXIS] / 60.f);
+        }
+        if (lift_z_on_min_y && (READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING) == 1) {
+            // Already triggering before we started the move.
+            // Shift the trigger point slightly outwards.
+            // a = current_position[Y_AXIS] - 1.5f;
+            a = current_position[Y_AXIS];
+        } else {
+            enable_z_endstop(true);
+            go_xy(current_position[X_AXIS], y1, homing_feedrate[X_AXIS] / 60.f);
+            update_current_position_xyz();
+            if (! endstop_z_hit_on_purpose()) {
+                current_position[Y_AXIS] = center_old_y;
+                goto canceled;
+            }
+            a = current_position[Y_AXIS];
+        }
+        enable_z_endstop(false);
+        go_xy(current_position[X_AXIS], y1, homing_feedrate[X_AXIS] / 60.f);
+        enable_z_endstop(true);
+        go_xy(current_position[X_AXIS], y0, homing_feedrate[X_AXIS] / 60.f);
+        update_current_position_xyz();
+        if (! endstop_z_hit_on_purpose()) {
+            current_position[Y_AXIS] = center_old_y;
+            goto canceled;
+        }
+        b = current_position[Y_AXIS];
+        if (b - a < MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
+            // We force the calibration routine to move the Z axis slightly down to make the response more pronounced.
+            if (verbosity_level >= 5) {
+                SERIAL_ECHOPGM("Point height too small: ");
+                SERIAL_ECHO(b - a);
+                SERIAL_ECHOLNPGM("");
+            }
+            if (b - a < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
+                // Don't use the new Y value.
+                current_position[Y_AXIS] = center_old_y;
+                goto canceled;
+            } else {
+                // Use the new value, but force the Z axis to go a bit lower.
+                point_small = true;
+            }
+        }
+        if (verbosity_level >= 5) {
+            debug_output_point(PSTR("top" ), current_position[X_AXIS], a, current_position[Z_AXIS]);
+            debug_output_point(PSTR("bottom"), current_position[X_AXIS], b, current_position[Z_AXIS]);
+        }
+
+        // Go to the center.
+        enable_z_endstop(false);
+        current_position[Y_AXIS] = 0.5f * (a + b);
+        go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
+    }
+
+    // If point is small but not too small, then force the Z axis to be lowered a bit,
+    // but use the new value. This is important when the initial position was off in one axis,
+    // for example if the initial calibration was shifted in the Y axis systematically.
+    // Then this first step will center.
+    return ! point_small;
+
+canceled:
+    // Go back to the center.
+    enable_z_endstop(false);
+    go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
+    return false;
+}
+
+// Searching the front points, where one cannot move the sensor head in front of the sensor point.
+// Searching in a zig-zag movement in a plane for the maximum width of the response.
+// This function may set the current_position[Y_AXIS] below Y_MIN_POS, if the function succeeded.
+// If this function failed, the Y coordinate will never be outside the working space.
+#define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS (4.f)
+#define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y (0.1f)
+inline bool improve_bed_induction_sensor_point3(int verbosity_level)
+{	
+    float center_old_x = current_position[X_AXIS];
+    float center_old_y = current_position[Y_AXIS];
+    float a, b;
+    bool  result = true;
+
+	if (verbosity_level >= 20) MYSERIAL.println("Improve bed induction sensor point3");
+    // Was the sensor point detected too far in the minus Y axis?
+    // If yes, the center of the induction point cannot be reached by the machine.
+    {
+        float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
+        float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
+        float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
+        float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
+        float y = y0;
+
+        if (x0 < X_MIN_POS)
+            x0 = X_MIN_POS;
+        if (x1 > X_MAX_POS)
+            x1 = X_MAX_POS;
+        if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
+            y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
+        if (y1 > Y_MAX_POS)
+            y1 = Y_MAX_POS;
+
+        if (verbosity_level >= 20) {
+            SERIAL_ECHOPGM("Initial position: ");
+            SERIAL_ECHO(center_old_x);
+            SERIAL_ECHOPGM(", ");
+            SERIAL_ECHO(center_old_y);
+            SERIAL_ECHOLNPGM("");
+        }
+
+        // Search in the positive Y direction, until a maximum diameter is found.
+        // (the next diameter is smaller than the current one.)
+        float dmax = 0.f;
+        float xmax1 = 0.f;
+        float xmax2 = 0.f;
+        for (y = y0; y < y1; y += IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
+            enable_z_endstop(false);
+            go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
+            enable_z_endstop(true);
+            go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
+            update_current_position_xyz();
+            if (! endstop_z_hit_on_purpose()) {
+                continue;
+                // SERIAL_PROTOCOLPGM("Failed 1\n");
+                // current_position[X_AXIS] = center_old_x;
+                // goto canceled;
+            }
+            a = current_position[X_AXIS];
+            enable_z_endstop(false);
+            go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
+            enable_z_endstop(true);
+            go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
+            update_current_position_xyz();
+            if (! endstop_z_hit_on_purpose()) {
+                continue;
+                // SERIAL_PROTOCOLPGM("Failed 2\n");
+                // current_position[X_AXIS] = center_old_x;
+                // goto canceled;
+            }
+            b = current_position[X_AXIS];
+            if (verbosity_level >= 5) {
+                debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
+                debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
+            }
+            float d = b - a;
+            if (d > dmax) {
+                xmax1 = 0.5f * (a + b);
+                dmax = d;
+            } else if (dmax > 0.) {
+                y0 = y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y;
+                break;
+            }
+        }
+        if (dmax == 0.) {
+            if (verbosity_level > 0)
+                SERIAL_PROTOCOLPGM("failed - not found\n");
+            current_position[X_AXIS] = center_old_x;
+            current_position[Y_AXIS] = center_old_y;
+            goto canceled;
+        }
+
+        {
+            // Find the positive Y hit. This gives the extreme Y value for the search of the maximum diameter in the -Y direction.
+            enable_z_endstop(false);
+            go_xy(xmax1, y0 + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, homing_feedrate[X_AXIS] / 60.f);
+            enable_z_endstop(true);
+            go_xy(xmax1, max(y0 - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, Y_MIN_POS_FOR_BED_CALIBRATION), homing_feedrate[X_AXIS] / 60.f);
+            update_current_position_xyz();
+            if (! endstop_z_hit_on_purpose()) {
+                current_position[Y_AXIS] = center_old_y;
+                goto canceled;
+            }
+            if (verbosity_level >= 5)
+                debug_output_point(PSTR("top" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
+            y1 = current_position[Y_AXIS];
+        }
+
+        if (y1 <= y0) {
+            // Either the induction sensor is too high, or the induction sensor target is out of reach.
+            current_position[Y_AXIS] = center_old_y;
+            goto canceled;
+        }
+
+        // Search in the negative Y direction, until a maximum diameter is found.
+        dmax = 0.f;
+        // if (y0 + 1.f < y1)
+        //    y1 = y0 + 1.f;
+        for (y = y1; y >= y0; y -= IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
+            enable_z_endstop(false);
+            go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
+            enable_z_endstop(true);
+            go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
+            update_current_position_xyz();
+            if (! endstop_z_hit_on_purpose()) {
+                continue;
+                /*
+                current_position[X_AXIS] = center_old_x;
+                SERIAL_PROTOCOLPGM("Failed 3\n");
+                goto canceled;
+                */
+            }
+            a = current_position[X_AXIS];
+            enable_z_endstop(false);
+            go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
+            enable_z_endstop(true);
+            go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
+            update_current_position_xyz();
+            if (! endstop_z_hit_on_purpose()) {
+                continue;
+                /*
+                current_position[X_AXIS] = center_old_x;
+                SERIAL_PROTOCOLPGM("Failed 4\n");
+                goto canceled;
+                */
+            }
+            b = current_position[X_AXIS];
+            if (verbosity_level >= 5) {
+                debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
+                debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
+            }
+            float d = b - a;
+            if (d > dmax) {
+                xmax2 = 0.5f * (a + b);
+                dmax = d;
+            } else if (dmax > 0.) {
+                y1 = y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y;
+                break;
+            }
+        }
+        float xmax, ymax;
+        if (dmax == 0.f) {
+            // Only the hit in the positive direction found.
+            xmax = xmax1;
+            ymax = y0;
+        } else {
+            // Both positive and negative directions found.
+            xmax = xmax2;
+            ymax = 0.5f * (y0 + y1);
+            for (; y >= y0; y -= IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
+                enable_z_endstop(false);
+                go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
+                enable_z_endstop(true);
+                go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
+                update_current_position_xyz();
+                if (! endstop_z_hit_on_purpose()) {
+                    continue;
+                    /*
+                    current_position[X_AXIS] = center_old_x;
+                    SERIAL_PROTOCOLPGM("Failed 3\n");
+                    goto canceled;
+                    */
+                }
+                a = current_position[X_AXIS];
+                enable_z_endstop(false);
+                go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
+                enable_z_endstop(true);
+                go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
+                update_current_position_xyz();
+                if (! endstop_z_hit_on_purpose()) {
+                    continue;
+                    /*
+                    current_position[X_AXIS] = center_old_x;
+                    SERIAL_PROTOCOLPGM("Failed 4\n");
+                    goto canceled;
+                    */
+                }
+                b = current_position[X_AXIS];
+                if (verbosity_level >= 5) {
+                    debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
+                    debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
+                }
+                float d = b - a;
+                if (d > dmax) {
+                    xmax = 0.5f * (a + b);
+                    ymax = y;
+                    dmax = d;
+                }
+            }
+        }
+
+        {
+            // Compare the distance in the Y+ direction with the diameter in the X direction.
+            // Find the positive Y hit once again, this time along the Y axis going through the X point with the highest diameter.
+            enable_z_endstop(false);
+            go_xy(xmax, ymax + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, homing_feedrate[X_AXIS] / 60.f);
+            enable_z_endstop(true);
+            go_xy(xmax, max(ymax - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, Y_MIN_POS_FOR_BED_CALIBRATION), homing_feedrate[X_AXIS] / 60.f);
+            update_current_position_xyz();
+            if (! endstop_z_hit_on_purpose()) {
+                current_position[Y_AXIS] = center_old_y;
+                goto canceled;
+            }
+            if (verbosity_level >= 5)
+                debug_output_point(PSTR("top" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
+            if (current_position[Y_AXIS] - Y_MIN_POS_FOR_BED_CALIBRATION < 0.5f * dmax) {
+                // Probably not even a half circle was detected. The induction point is likely too far in the minus Y direction.
+                // First verify, if the measurement has been done at a sufficient height. If no, lower the Z axis a bit.
+                if (current_position[Y_AXIS] < ymax || dmax < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
+                    if (verbosity_level >= 5) {
+                        SERIAL_ECHOPGM("Partial point diameter too small: ");
+                        SERIAL_ECHO(dmax);
+                        SERIAL_ECHOLNPGM("");
+                    }
+                    result = false;
+                } else {
+                    // Estimate the circle radius from the maximum diameter and height:
+                    float h = current_position[Y_AXIS] - ymax;
+                    float r = dmax * dmax / (8.f * h) + 0.5f * h;
+                    if (r < 0.8f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
+                        if (verbosity_level >= 5) {
+                            SERIAL_ECHOPGM("Partial point estimated radius too small: ");
+                            SERIAL_ECHO(r);
+                            SERIAL_ECHOPGM(", dmax:");
+                            SERIAL_ECHO(dmax);
+                            SERIAL_ECHOPGM(", h:");
+                            SERIAL_ECHO(h);
+                            SERIAL_ECHOLNPGM("");
+                        }
+                        result = false;
+                    } else {
+                        // The point may end up outside of the machine working space.
+                        // That is all right as it helps to improve the accuracy of the measurement point
+                        // due to averaging.
+                        // For the y correction, use an average of dmax/2 and the estimated radius.
+                        r = 0.5f * (0.5f * dmax + r);
+                        ymax = current_position[Y_AXIS] - r;
+                    }
+                }
+            } else {
+                // If the diameter of the detected spot was smaller than a minimum allowed,
+                // the induction sensor is probably too high. Returning false will force
+                // the sensor to be lowered a tiny bit.
+                result = xmax >= MIN_BED_SENSOR_POINT_RESPONSE_DMR;
+                if (y0 > Y_MIN_POS_FOR_BED_CALIBRATION + 0.2f)
+                    // Only in case both left and right y tangents are known, use them.
+                    // If y0 is close to the bed edge, it may not be symmetric to the right tangent.
+                    ymax = 0.5f * ymax + 0.25f * (y0 + y1);
+            }
+        }
+
+        // Go to the center.
+        enable_z_endstop(false);
+        current_position[X_AXIS] = xmax;
+        current_position[Y_AXIS] = ymax;
+        if (verbosity_level >= 20) {
+            SERIAL_ECHOPGM("Adjusted position: ");
+            SERIAL_ECHO(current_position[X_AXIS]);
+            SERIAL_ECHOPGM(", ");
+            SERIAL_ECHO(current_position[Y_AXIS]);
+            SERIAL_ECHOLNPGM("");
+        }
+
+        // Don't clamp current_position[Y_AXIS], because the out-of-reach Y coordinate may actually be true.
+        // Only clamp the coordinate to go.
+        go_xy(current_position[X_AXIS], max(Y_MIN_POS, current_position[Y_AXIS]), homing_feedrate[X_AXIS] / 60.f);
+        // delay_keep_alive(3000);
+    }
+
+    if (result)
+        return true;
+    // otherwise clamp the Y coordinate
+
+canceled:
+    // Go back to the center.
+    enable_z_endstop(false);
+    if (current_position[Y_AXIS] < Y_MIN_POS)
+        current_position[Y_AXIS] = Y_MIN_POS;
+    go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
+    return false;
+}
+
+// Scan the mesh bed induction points one by one by a left-right zig-zag movement,
+// write the trigger coordinates to the serial line.
+// Useful for visualizing the behavior of the bed induction detector.
+inline void scan_bed_induction_sensor_point()
+{
+    float center_old_x = current_position[X_AXIS];
+    float center_old_y = current_position[Y_AXIS];
+    float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
+    float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
+    float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
+    float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
+    float y = y0;
+
+    if (x0 < X_MIN_POS)
+        x0 = X_MIN_POS;
+    if (x1 > X_MAX_POS)
+        x1 = X_MAX_POS;
+    if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
+        y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
+    if (y1 > Y_MAX_POS)
+        y1 = Y_MAX_POS;
+
+    for (float y = y0; y < y1; y += IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
+        enable_z_endstop(false);
+        go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
+        enable_z_endstop(true);
+        go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
+        update_current_position_xyz();
+        if (endstop_z_hit_on_purpose())
+            debug_output_point(PSTR("left" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
+        enable_z_endstop(false);
+        go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
+        enable_z_endstop(true);
+        go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
+        update_current_position_xyz();
+        if (endstop_z_hit_on_purpose())
+            debug_output_point(PSTR("right"), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
+    }
+
+    enable_z_endstop(false);
+    current_position[X_AXIS] = center_old_x;
+    current_position[Y_AXIS] = center_old_y;
+    go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
+}
+
+#define MESH_BED_CALIBRATION_SHOW_LCD
+
+BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level, uint8_t &too_far_mask)
+{	
+    // Don't let the manage_inactivity() function remove power from the motors.
+    refresh_cmd_timeout();
+
+    // Reusing the z_values memory for the measurement cache.
+    // 7x7=49 floats, good for 16 (x,y,z) vectors.
+    float *pts = &mbl.z_values[0][0];
+    float *vec_x = pts + 2 * 4;
+    float *vec_y = vec_x + 2;
+    float *cntr  = vec_y + 2;
+    memset(pts, 0, sizeof(float) * 7 * 7);
+	uint8_t iteration = 0; 
+	BedSkewOffsetDetectionResultType result;
+
+//    SERIAL_ECHOLNPGM("find_bed_offset_and_skew verbosity level: ");
+//    SERIAL_ECHO(int(verbosity_level));
+//    SERIAL_ECHOPGM("");
+	
+	while (iteration < 3) {
+
+		SERIAL_ECHOPGM("Iteration: ");
+		MYSERIAL.println(int(iteration + 1));
+		if (verbosity_level >= 20) {
+		SERIAL_ECHOLNPGM("Vectors: ");
+		
+			SERIAL_ECHOPGM("vec_x[0]:");
+			MYSERIAL.print(vec_x[0], 5);
+			SERIAL_ECHOLNPGM("");
+			SERIAL_ECHOPGM("vec_x[1]:");
+			MYSERIAL.print(vec_x[1], 5);
+			SERIAL_ECHOLNPGM("");
+			SERIAL_ECHOPGM("vec_y[0]:");
+			MYSERIAL.print(vec_y[0], 5);
+			SERIAL_ECHOLNPGM("");
+			SERIAL_ECHOPGM("vec_y[1]:");
+			MYSERIAL.print(vec_y[1], 5);
+			SERIAL_ECHOLNPGM("");
+			SERIAL_ECHOPGM("cntr[0]:");
+			MYSERIAL.print(cntr[0], 5);
+			SERIAL_ECHOLNPGM("");
+			SERIAL_ECHOPGM("cntr[1]:");
+			MYSERIAL.print(cntr[1], 5);
+			SERIAL_ECHOLNPGM("");
+		}
+#ifdef MESH_BED_CALIBRATION_SHOW_LCD
+    uint8_t next_line;
+    lcd_display_message_fullscreen_P(MSG_FIND_BED_OFFSET_AND_SKEW_LINE1, next_line);
+    if (next_line > 3)
+        next_line = 3;
+#endif /* MESH_BED_CALIBRATION_SHOW_LCD */
+
+    // Collect the rear 2x3 points.
+	current_position[Z_AXIS] = MESH_HOME_Z_SEARCH + FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP * iteration * 0.3;
+	for (int k = 0; k < 4; ++k) {
+		// Don't let the manage_inactivity() function remove power from the motors.
+		refresh_cmd_timeout();
+#ifdef MESH_BED_CALIBRATION_SHOW_LCD
+		lcd_implementation_print_at(0, next_line, k + 1);
+		lcd_printPGM(MSG_FIND_BED_OFFSET_AND_SKEW_LINE2);
+
+		if (iteration > 0) {
+			lcd_print_at_PGM(0, next_line + 1, MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION);
+			lcd_implementation_print(int(iteration + 1));
+		}
+#endif /* MESH_BED_CALIBRATION_SHOW_LCD */
+		float *pt = pts + k * 2;
+		// Go up to z_initial.
+
+		go_to_current(homing_feedrate[Z_AXIS] / 60.f);
+		if (verbosity_level >= 20) {
+			// Go to Y0, wait, then go to Y-4.
+			current_position[Y_AXIS] = 0.f;
+			go_to_current(homing_feedrate[X_AXIS] / 60.f);
+			SERIAL_ECHOLNPGM("At Y0");
+			delay_keep_alive(5000);
+			current_position[Y_AXIS] = Y_MIN_POS;
+			go_to_current(homing_feedrate[X_AXIS] / 60.f);
+			SERIAL_ECHOLNPGM("At Y-4");
+			delay_keep_alive(5000);
+		}
+		// Go to the measurement point position.
+		//if (iteration == 0) {
+			current_position[X_AXIS] = pgm_read_float(bed_ref_points_4 + k * 2);
+			current_position[Y_AXIS] = pgm_read_float(bed_ref_points_4 + k * 2 + 1);
+		/*}
+		else {
+			// if first iteration failed, count corrected point coordinates as initial
+			// Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
+			
+			current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points_4 + k * 2) + vec_y[0] * pgm_read_float(bed_ref_points_4 + k * 2 + 1) + cntr[0];
+			current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points_4 + k * 2) + vec_y[1] * pgm_read_float(bed_ref_points_4 + k * 2 + 1) + cntr[1];
+
+			// The calibration points are very close to the min Y.
+			if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
+				current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
+
+		}*/
+		if (verbosity_level >= 20) {
+			SERIAL_ECHOPGM("current_position[X_AXIS]:");
+			MYSERIAL.print(current_position[X_AXIS], 5);
+			SERIAL_ECHOLNPGM("");
+			SERIAL_ECHOPGM("current_position[Y_AXIS]:");
+			MYSERIAL.print(current_position[Y_AXIS], 5);
+			SERIAL_ECHOLNPGM("");
+			SERIAL_ECHOPGM("current_position[Z_AXIS]:");
+			MYSERIAL.print(current_position[Z_AXIS], 5);
+			SERIAL_ECHOLNPGM("");
+		}
+
+
+		go_to_current(homing_feedrate[X_AXIS] / 60.f);
+		if (verbosity_level >= 10)
+			delay_keep_alive(3000);
+		if (!find_bed_induction_sensor_point_xy())
+			return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
+#if 1
+		
+			if (k == 0) {
+				// Improve the position of the 1st row sensor points by a zig-zag movement.
+				find_bed_induction_sensor_point_z();
+				int8_t i = 4;
+				for (;;) {
+					if (improve_bed_induction_sensor_point3(verbosity_level))
+						break;
+					if (--i == 0)
+						return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
+					// Try to move the Z axis down a bit to increase a chance of the sensor to trigger.
+					current_position[Z_AXIS] -= 0.025f;
+					enable_endstops(false);
+					enable_z_endstop(false);
+					go_to_current(homing_feedrate[Z_AXIS]);
+				}
+				if (i == 0)
+					// not found
+					return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
+			}
+#endif
+			if (verbosity_level >= 10)
+				delay_keep_alive(3000);
+			// Save the detected point position and then clamp the Y coordinate, which may have been estimated
+			// to lie outside the machine working space.
+			if (verbosity_level >= 20) {
+				SERIAL_ECHOLNPGM("Measured:");
+				MYSERIAL.println(current_position[X_AXIS]);
+				MYSERIAL.println(current_position[Y_AXIS]);
+			}
+			pt[0] = (pt[0] * iteration) / (iteration + 1);
+			pt[0] += (current_position[X_AXIS]/(iteration + 1)); //count average
+			pt[1] = (pt[1] * iteration) / (iteration + 1);
+			pt[1] += (current_position[Y_AXIS] / (iteration + 1));
+			
+			
+			//pt[0] += current_position[X_AXIS];
+			//if(iteration > 0) pt[0] = pt[0] / 2;
+						
+			//pt[1] += current_position[Y_AXIS];
+			//if (iteration > 0) pt[1] = pt[1] / 2;
+
+			if (verbosity_level >= 20) {
+				SERIAL_ECHOLNPGM("");
+				SERIAL_ECHOPGM("pt[0]:");
+				MYSERIAL.println(pt[0]);
+				SERIAL_ECHOPGM("pt[1]:");
+				MYSERIAL.println(pt[1]);
+			}
+
+			if (current_position[Y_AXIS] < Y_MIN_POS)
+				current_position[Y_AXIS] = Y_MIN_POS;
+			// Start searching for the other points at 3mm above the last point.
+			current_position[Z_AXIS] += 3.f + FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP * iteration * 0.3;
+			//cntr[0] += pt[0];
+			//cntr[1] += pt[1];
+			if (verbosity_level >= 10 && k == 0) {
+				// Show the zero. Test, whether the Y motor skipped steps.
+				current_position[Y_AXIS] = MANUAL_Y_HOME_POS;
+				go_to_current(homing_feedrate[X_AXIS] / 60.f);
+				delay_keep_alive(3000);
+			}
+		}
+
+		if (verbosity_level >= 20) {
+			// Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
+			delay_keep_alive(3000);
+			for (int8_t mesh_point = 0; mesh_point < 4; ++mesh_point) {
+				// Don't let the manage_inactivity() function remove power from the motors.
+				refresh_cmd_timeout();
+				// Go to the measurement point.
+				// Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
+				current_position[X_AXIS] = pts[mesh_point * 2];
+				current_position[Y_AXIS] = pts[mesh_point * 2 + 1];
+				go_to_current(homing_feedrate[X_AXIS] / 60);
+				delay_keep_alive(3000);
+			}
+		}
+
+		if (pts[1] < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
+			too_far_mask |= 1 << 1; //front center point is out of reach
+				SERIAL_ECHOLNPGM("");
+				SERIAL_ECHOPGM("WARNING: Front point not reachable. Y coordinate:");
+				MYSERIAL.print(pts[1]);
+				SERIAL_ECHOPGM(" < ");
+				MYSERIAL.println(Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
+		}
+
+		result = calculate_machine_skew_and_offset_LS(pts, 4, bed_ref_points_4, vec_x, vec_y, cntr, verbosity_level);
+		if (result >= 0) {
+			world2machine_update(vec_x, vec_y, cntr);
+#if 1
+			// Fearlessly store the calibration values into the eeprom.
+			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 0), cntr[0]);
+			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 4), cntr[1]);
+			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 0), vec_x[0]);
+			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 4), vec_x[1]);
+			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 0), vec_y[0]);
+			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 4), vec_y[1]);
+#endif
+			if (verbosity_level >= 10) {
+				// Length of the vec_x
+				float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]);
+				SERIAL_ECHOLNPGM("X vector length:");
+				MYSERIAL.println(l);
+
+				// Length of the vec_y
+				l = sqrt(vec_y[0] * vec_y[0] + vec_y[1] * vec_y[1]);
+				SERIAL_ECHOLNPGM("Y vector length:");
+				MYSERIAL.println(l);
+				// Zero point correction
+				l = sqrt(cntr[0] * cntr[0] + cntr[1] * cntr[1]);
+				SERIAL_ECHOLNPGM("Zero point correction:");
+				MYSERIAL.println(l);
+
+				// vec_x and vec_y shall be nearly perpendicular.
+				l = vec_x[0] * vec_y[0] + vec_x[1] * vec_y[1];
+				SERIAL_ECHOLNPGM("Perpendicularity");
+				MYSERIAL.println(fabs(l));
+				SERIAL_ECHOLNPGM("Saving bed calibration vectors to EEPROM");
+			}
+			// Correct the current_position to match the transformed coordinate system after world2machine_rotation_and_skew and world2machine_shift were set.
+			world2machine_update_current();
+
+			
+			if (verbosity_level >= 20) {
+				// Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
+				delay_keep_alive(3000);
+				for (int8_t mesh_point = 0; mesh_point < 9; ++mesh_point) {
+					// Don't let the manage_inactivity() function remove power from the motors.
+					refresh_cmd_timeout();
+					// Go to the measurement point.
+					// Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
+					current_position[X_AXIS] = pgm_read_float(bed_ref_points + mesh_point * 2);
+					current_position[Y_AXIS] = pgm_read_float(bed_ref_points + mesh_point * 2 + 1);
+					go_to_current(homing_feedrate[X_AXIS] / 60);
+					delay_keep_alive(3000);
+				}
+			}
+			return result;
+		}		
+		if (result == BED_SKEW_OFFSET_DETECTION_FITTING_FAILED && too_far_mask == 2) return result; //if fitting failed and front center point is out of reach, terminate calibration and inform user
+		iteration++;
+	}
+	return result;    
+}
+
+BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level, uint8_t &too_far_mask)
+{
+    // Don't let the manage_inactivity() function remove power from the motors.
+    refresh_cmd_timeout();
+
+    // Mask of the first three points. Are they too far?
+    too_far_mask = 0;
+
+    // Reusing the z_values memory for the measurement cache.
+    // 7x7=49 floats, good for 16 (x,y,z) vectors.
+    float *pts = &mbl.z_values[0][0];
+    float *vec_x = pts + 2 * 9;
+    float *vec_y = vec_x + 2;
+    float *cntr  = vec_y + 2;
+    memset(pts, 0, sizeof(float) * 7 * 7);
+
+    // Cache the current correction matrix.
+    world2machine_initialize();
+    vec_x[0] = world2machine_rotation_and_skew[0][0];
+    vec_x[1] = world2machine_rotation_and_skew[1][0];
+    vec_y[0] = world2machine_rotation_and_skew[0][1];
+    vec_y[1] = world2machine_rotation_and_skew[1][1];
+    cntr[0] = world2machine_shift[0];
+    cntr[1] = world2machine_shift[1];
+    // and reset the correction matrix, so the planner will not do anything.
+    world2machine_reset();
+
+    bool endstops_enabled  = enable_endstops(false);
+    bool endstop_z_enabled = enable_z_endstop(false);
+
+#ifdef MESH_BED_CALIBRATION_SHOW_LCD
+    uint8_t next_line;
+    lcd_display_message_fullscreen_P(MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE1, next_line);
+    if (next_line > 3)
+        next_line = 3;
+#endif /* MESH_BED_CALIBRATION_SHOW_LCD */
+
+    // Collect a matrix of 9x9 points.
+    BedSkewOffsetDetectionResultType result = BED_SKEW_OFFSET_DETECTION_PERFECT;
+    for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
+        // Don't let the manage_inactivity() function remove power from the motors.
+        refresh_cmd_timeout();
+        // Print the decrasing ID of the measurement point.
+#ifdef MESH_BED_CALIBRATION_SHOW_LCD
+        lcd_implementation_print_at(0, next_line, mesh_point+1);
+        lcd_printPGM(MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE2);
+#endif /* MESH_BED_CALIBRATION_SHOW_LCD */
+
+        // Move up.
+        current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+        enable_endstops(false);
+        enable_z_endstop(false);
+        go_to_current(homing_feedrate[Z_AXIS]/60);
+        if (verbosity_level >= 20) {
+            // Go to Y0, wait, then go to Y-4.
+            current_position[Y_AXIS] = 0.f;
+            go_to_current(homing_feedrate[X_AXIS] / 60.f);
+            SERIAL_ECHOLNPGM("At Y0");
+            delay_keep_alive(5000);
+            current_position[Y_AXIS] = Y_MIN_POS;
+            go_to_current(homing_feedrate[X_AXIS] / 60.f);
+            SERIAL_ECHOLNPGM("At Y-4");
+            delay_keep_alive(5000);
+        }
+        // Go to the measurement point.
+        // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
+        current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[0] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[0];
+        current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
+        // The calibration points are very close to the min Y.
+        if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
+            current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
+        go_to_current(homing_feedrate[X_AXIS]/60);
+        // Find its Z position by running the normal vertical search.
+        if (verbosity_level >= 10)
+            delay_keep_alive(3000);
+        find_bed_induction_sensor_point_z();
+        if (verbosity_level >= 10)
+            delay_keep_alive(3000);
+        // Try to move the Z axis down a bit to increase a chance of the sensor to trigger.
+        current_position[Z_AXIS] -= 0.025f;
+        // Improve the point position by searching its center in a current plane.
+        int8_t n_errors = 3;
+        for (int8_t iter = 0; iter < 8; ) {
+            if (verbosity_level > 20) {
+                SERIAL_ECHOPGM("Improving bed point ");
+                SERIAL_ECHO(mesh_point);
+                SERIAL_ECHOPGM(", iteration ");
+                SERIAL_ECHO(iter);
+                SERIAL_ECHOPGM(", z");
+                MYSERIAL.print(current_position[Z_AXIS], 5);
+                SERIAL_ECHOLNPGM("");
+            }
+            bool found = false;
+            if (mesh_point < 3) {
+                // Because the sensor cannot move in front of the first row
+                // of the sensor points, the y position cannot be measured
+                // by a cross center method.
+                // Use a zig-zag search for the first row of the points.
+                found = improve_bed_induction_sensor_point3(verbosity_level);
+            } else {
+                switch (method) {
+                    case 0: found = improve_bed_induction_sensor_point(); break;
+                    case 1: found = improve_bed_induction_sensor_point2(mesh_point < 3, verbosity_level); break;
+                    default: break;
+                }
+            }
+            if (found) {
+                if (iter > 3) {
+                    // Average the last 4 measurements.
+                    pts[mesh_point*2  ] += current_position[X_AXIS];
+                    pts[mesh_point*2+1] += current_position[Y_AXIS];
+                }
+                if (current_position[Y_AXIS] < Y_MIN_POS)
+                    current_position[Y_AXIS] = Y_MIN_POS;
+                ++ iter;
+            } else if (n_errors -- == 0) {
+                // Give up.
+                result = BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
+                goto canceled;
+            } else {
+                // Try to move the Z axis down a bit to increase a chance of the sensor to trigger.
+                current_position[Z_AXIS] -= 0.05f;
+                enable_endstops(false);
+                enable_z_endstop(false);
+                go_to_current(homing_feedrate[Z_AXIS]);
+                if (verbosity_level >= 5) {
+                    SERIAL_ECHOPGM("Improving bed point ");
+                    SERIAL_ECHO(mesh_point);
+                    SERIAL_ECHOPGM(", iteration ");
+                    SERIAL_ECHO(iter);
+                    SERIAL_ECHOPGM(" failed. Lowering z to ");
+                    MYSERIAL.print(current_position[Z_AXIS], 5);
+                    SERIAL_ECHOLNPGM("");
+                }
+            }
+        }
+        if (verbosity_level >= 10)
+            delay_keep_alive(3000);
+    }
+    // Don't let the manage_inactivity() function remove power from the motors.
+    refresh_cmd_timeout();
+
+    // Average the last 4 measurements.
+    for (int8_t i = 0; i < 18; ++ i)
+        pts[i] *= (1.f/4.f);
+
+    enable_endstops(false);
+    enable_z_endstop(false);
+
+    if (verbosity_level >= 5) {
+        // Test the positions. Are the positions reproducible?
+		current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+        for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
+            // Don't let the manage_inactivity() function remove power from the motors.
+            refresh_cmd_timeout();
+            // Go to the measurement point.
+            // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
+            current_position[X_AXIS] = pts[mesh_point*2];
+            current_position[Y_AXIS] = pts[mesh_point*2+1];
+            if (verbosity_level >= 10) {
+                go_to_current(homing_feedrate[X_AXIS]/60);
+                delay_keep_alive(3000);
+            }
+            SERIAL_ECHOPGM("Final measured bed point ");
+            SERIAL_ECHO(mesh_point);
+            SERIAL_ECHOPGM(": ");
+            MYSERIAL.print(current_position[X_AXIS], 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(current_position[Y_AXIS], 5);
+            SERIAL_ECHOLNPGM("");
+        }
+    }
+
+    {
+        // First fill in the too_far_mask from the measured points.
+        for (uint8_t mesh_point = 0; mesh_point < 3; ++ mesh_point)
+            if (pts[mesh_point * 2 + 1] < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH)
+                too_far_mask |= 1 << mesh_point;
+        result = calculate_machine_skew_and_offset_LS(pts, 9, bed_ref_points, vec_x, vec_y, cntr, verbosity_level);
+        if (result < 0) {
+            SERIAL_ECHOLNPGM("Calculation of the machine skew and offset failed.");
+            goto canceled;
+        }
+        // In case of success, update the too_far_mask from the calculated points.
+        for (uint8_t mesh_point = 0; mesh_point < 3; ++ mesh_point) {
+            float y = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
+            if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH)
+                too_far_mask |= 1 << mesh_point;
+        }
+    }
+
+    world2machine_update(vec_x, vec_y, cntr);
+#if 1
+    // Fearlessly store the calibration values into the eeprom.
+    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0), cntr [0]);
+    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4), cntr [1]);
+    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0), vec_x[0]);
+    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4), vec_x[1]);
+    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0), vec_y[0]);
+    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
+#endif
+
+    // Correct the current_position to match the transformed coordinate system after world2machine_rotation_and_skew and world2machine_shift were set.
+    world2machine_update_current();
+
+    enable_endstops(false);
+    enable_z_endstop(false);
+
+    if (verbosity_level >= 5) {
+        // Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
+        delay_keep_alive(3000);
+		current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+        for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
+            // Don't let the manage_inactivity() function remove power from the motors.
+            refresh_cmd_timeout();
+            // Go to the measurement point.
+            // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
+            current_position[X_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2);
+            current_position[Y_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2+1);
+            if (verbosity_level >= 10) {
+                go_to_current(homing_feedrate[X_AXIS]/60);
+                delay_keep_alive(3000);
+            }
+            {
+                float x, y;
+                world2machine(current_position[X_AXIS], current_position[Y_AXIS], x, y);
+                SERIAL_ECHOPGM("Final calculated bed point ");
+                SERIAL_ECHO(mesh_point);
+                SERIAL_ECHOPGM(": ");
+                MYSERIAL.print(x, 5);
+                SERIAL_ECHOPGM(", ");
+                MYSERIAL.print(y, 5);
+                SERIAL_ECHOLNPGM("");
+            }
+        }
+    }
+
+    // Sample Z heights for the mesh bed leveling.
+    // In addition, store the results into an eeprom, to be used later for verification of the bed leveling process.
+    if (! sample_mesh_and_store_reference())
+        goto canceled;
+
+    enable_endstops(endstops_enabled);
+    enable_z_endstop(endstop_z_enabled);
+    // Don't let the manage_inactivity() function remove power from the motors.
+    refresh_cmd_timeout();
+    return result;
+
+canceled:
+    // Don't let the manage_inactivity() function remove power from the motors.
+    refresh_cmd_timeout();
+    // Print head up.
+    current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+    go_to_current(homing_feedrate[Z_AXIS]/60);
+    // Store the identity matrix to EEPROM.
+    reset_bed_offset_and_skew();
+    enable_endstops(endstops_enabled);
+    enable_z_endstop(endstop_z_enabled);
+    return result;
+}
+
+void go_home_with_z_lift()
+{
+    // Don't let the manage_inactivity() function remove power from the motors.
+    refresh_cmd_timeout();
+    // Go home.
+    // First move up to a safe height.
+    current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+    go_to_current(homing_feedrate[Z_AXIS]/60);
+    // Second move to XY [0, 0].
+    current_position[X_AXIS] = X_MIN_POS+0.2;
+    current_position[Y_AXIS] = Y_MIN_POS+0.2;
+    // Clamp to the physical coordinates.
+    world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
+    go_to_current(homing_feedrate[X_AXIS]/60);
+    // Third move up to a safe height.
+    current_position[Z_AXIS] = Z_MIN_POS;
+    go_to_current(homing_feedrate[Z_AXIS]/60);    
+}
+
+// Sample the 9 points of the bed and store them into the EEPROM as a reference.
+// When calling this function, the X, Y, Z axes should be already homed,
+// and the world2machine correction matrix should be active.
+// Returns false if the reference values are more than 3mm far away.
+bool sample_mesh_and_store_reference()
+{
+    bool endstops_enabled  = enable_endstops(false);
+    bool endstop_z_enabled = enable_z_endstop(false);
+
+    // Don't let the manage_inactivity() function remove power from the motors.
+    refresh_cmd_timeout();
+
+#ifdef MESH_BED_CALIBRATION_SHOW_LCD
+    uint8_t next_line;
+    lcd_display_message_fullscreen_P(MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE1, next_line);
+    if (next_line > 3)
+        next_line = 3;
+    // display "point xx of yy"
+    lcd_implementation_print_at(0, next_line, 1);
+    lcd_printPGM(MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE2);
+#endif /* MESH_BED_CALIBRATION_SHOW_LCD */
+
+    // Sample Z heights for the mesh bed leveling.
+    // In addition, store the results into an eeprom, to be used later for verification of the bed leveling process.
+    {
+        // The first point defines the reference.
+        current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+        go_to_current(homing_feedrate[Z_AXIS]/60);
+        current_position[X_AXIS] = pgm_read_float(bed_ref_points);
+        current_position[Y_AXIS] = pgm_read_float(bed_ref_points+1);
+        world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
+        go_to_current(homing_feedrate[X_AXIS]/60);
+        memcpy(destination, current_position, sizeof(destination));
+        enable_endstops(true);
+        homeaxis(Z_AXIS);
+        enable_endstops(false);
+        find_bed_induction_sensor_point_z();
+        mbl.set_z(0, 0, current_position[Z_AXIS]);
+    }
+    for (int8_t mesh_point = 1; mesh_point != MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS; ++ mesh_point) {
+        // Don't let the manage_inactivity() function remove power from the motors.
+        refresh_cmd_timeout();
+        // Print the decrasing ID of the measurement point.
+        current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+        go_to_current(homing_feedrate[Z_AXIS]/60);
+        current_position[X_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point);
+        current_position[Y_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point+1);
+        world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
+        go_to_current(homing_feedrate[X_AXIS]/60);
+#ifdef MESH_BED_CALIBRATION_SHOW_LCD
+        // display "point xx of yy"
+        lcd_implementation_print_at(0, next_line, mesh_point+1);
+        lcd_printPGM(MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE2);
+#endif /* MESH_BED_CALIBRATION_SHOW_LCD */
+        find_bed_induction_sensor_point_z();
+        // Get cords of measuring point
+        int8_t ix = mesh_point % MESH_MEAS_NUM_X_POINTS;
+        int8_t iy = mesh_point / MESH_MEAS_NUM_X_POINTS;
+        if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix; // Zig zag
+        mbl.set_z(ix, iy, current_position[Z_AXIS]);
+    }
+    {
+        // Verify the span of the Z values.
+        float zmin = mbl.z_values[0][0];
+        float zmax = zmax;
+        for (int8_t j = 0; j < 3; ++ j)
+           for (int8_t i = 0; i < 3; ++ i) {
+                zmin = min(zmin, mbl.z_values[j][i]);
+                zmax = min(zmax, mbl.z_values[j][i]);
+           }
+        if (zmax - zmin > 3.f) {
+            // The span of the Z offsets is extreme. Give up.
+            // Homing failed on some of the points.
+            SERIAL_PROTOCOLLNPGM("Exreme span of the Z values!");
+            return false;
+        }
+    }
+
+    // Store the correction values to EEPROM.
+    // Offsets of the Z heiths of the calibration points from the first point.
+    // The offsets are saved as 16bit signed int, scaled to tenths of microns.
+    {
+        uint16_t addr = EEPROM_BED_CALIBRATION_Z_JITTER;
+        for (int8_t j = 0; j < 3; ++ j)
+            for (int8_t i = 0; i < 3; ++ i) {
+                if (i == 0 && j == 0)
+                    continue;
+                float dif = mbl.z_values[j][i] - mbl.z_values[0][0];
+                int16_t dif_quantized = int16_t(floor(dif * 100.f + 0.5f));
+                eeprom_update_word((uint16_t*)addr, *reinterpret_cast<uint16_t*>(&dif_quantized));
+                #if 0
+                {
+                    uint16_t z_offset_u = eeprom_read_word((uint16_t*)addr);
+                    float dif2 = *reinterpret_cast<int16_t*>(&z_offset_u) * 0.01;
+
+                    SERIAL_ECHOPGM("Bed point ");
+                    SERIAL_ECHO(i);
+                    SERIAL_ECHOPGM(",");
+                    SERIAL_ECHO(j);
+                    SERIAL_ECHOPGM(", differences: written ");
+                    MYSERIAL.print(dif, 5);
+                    SERIAL_ECHOPGM(", read: ");
+                    MYSERIAL.print(dif2, 5);
+                    SERIAL_ECHOLNPGM("");
+                }
+                #endif
+                addr += 2;
+            }
+    }
+
+    mbl.upsample_3x3();
+    mbl.active = true;
+
+    go_home_with_z_lift();
+
+    enable_endstops(endstops_enabled);
+    enable_z_endstop(endstop_z_enabled);
+    return true;
+}
+
+bool scan_bed_induction_points(int8_t verbosity_level)
+{
+    // Don't let the manage_inactivity() function remove power from the motors.
+    refresh_cmd_timeout();
+
+    // Reusing the z_values memory for the measurement cache.
+    // 7x7=49 floats, good for 16 (x,y,z) vectors.
+    float *pts = &mbl.z_values[0][0];
+    float *vec_x = pts + 2 * 9;
+    float *vec_y = vec_x + 2;
+    float *cntr  = vec_y + 2;
+    memset(pts, 0, sizeof(float) * 7 * 7);
+
+    // Cache the current correction matrix.
+    world2machine_initialize();
+    vec_x[0] = world2machine_rotation_and_skew[0][0];
+    vec_x[1] = world2machine_rotation_and_skew[1][0];
+    vec_y[0] = world2machine_rotation_and_skew[0][1];
+    vec_y[1] = world2machine_rotation_and_skew[1][1];
+    cntr[0] = world2machine_shift[0];
+    cntr[1] = world2machine_shift[1];
+    // and reset the correction matrix, so the planner will not do anything.
+    world2machine_reset();
+
+    bool endstops_enabled  = enable_endstops(false);
+    bool endstop_z_enabled = enable_z_endstop(false);
+
+    // Collect a matrix of 9x9 points.
+    for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
+        // Don't let the manage_inactivity() function remove power from the motors.
+        refresh_cmd_timeout();
+
+        // Move up.
+        current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+        enable_endstops(false);
+        enable_z_endstop(false);
+        go_to_current(homing_feedrate[Z_AXIS]/60);
+        // Go to the measurement point.
+        // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
+        current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[0] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[0];
+        current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
+        // The calibration points are very close to the min Y.
+        if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
+            current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
+        go_to_current(homing_feedrate[X_AXIS]/60);
+        find_bed_induction_sensor_point_z();
+        scan_bed_induction_sensor_point();
+    }
+    // Don't let the manage_inactivity() function remove power from the motors.
+    refresh_cmd_timeout();
+
+    enable_endstops(false);
+    enable_z_endstop(false);
+
+    // Don't let the manage_inactivity() function remove power from the motors.
+    refresh_cmd_timeout();
+
+    enable_endstops(endstops_enabled);
+    enable_z_endstop(endstop_z_enabled);
+    return true;
+}
+
+// Shift a Z axis by a given delta.
+// To replace loading of the babystep correction.
+static void shift_z(float delta)
+{
+    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] - delta, current_position[E_AXIS], homing_feedrate[Z_AXIS]/40, active_extruder);
+    st_synchronize();
+    plan_set_z_position(current_position[Z_AXIS]);
+}
+
+#define BABYSTEP_LOADZ_BY_PLANNER
+
+// Number of baby steps applied
+static int babystepLoadZ = 0;
+
+void babystep_apply()
+{
+    // Apply Z height correction aka baby stepping before mesh bed leveling gets activated.
+    if(calibration_status() < CALIBRATION_STATUS_LIVE_ADJUST)
+	{
+		check_babystep(); //checking if babystep is in allowed range, otherwise setting babystep to 0
+		
+		// End of G80: Apply the baby stepping value.
+        EEPROM_read_B(EEPROM_BABYSTEP_Z,&babystepLoadZ);
+							
+    #if 0
+        SERIAL_ECHO("Z baby step: ");
+        SERIAL_ECHO(babystepLoadZ);
+        SERIAL_ECHO(", current Z: ");
+        SERIAL_ECHO(current_position[Z_AXIS]);
+        SERIAL_ECHO("correction: ");
+        SERIAL_ECHO(float(babystepLoadZ) / float(axis_steps_per_unit[Z_AXIS]));
+        SERIAL_ECHOLN("");
+    #endif
+    #ifdef BABYSTEP_LOADZ_BY_PLANNER
+        shift_z(- float(babystepLoadZ) / float(axis_steps_per_unit[Z_AXIS]));
+    #else
+        babystepsTodoZadd(babystepLoadZ);
+    #endif /* BABYSTEP_LOADZ_BY_PLANNER */
+    }
+}
+
+void babystep_undo()
+{
+#ifdef BABYSTEP_LOADZ_BY_PLANNER
+      shift_z(float(babystepLoadZ) / float(axis_steps_per_unit[Z_AXIS]));
+#else
+      babystepsTodoZsubtract(babystepLoadZ);
+#endif /* BABYSTEP_LOADZ_BY_PLANNER */
+      babystepLoadZ = 0;
+}
+
+void babystep_reset()
+{
+      babystepLoadZ = 0;    
 }

+ 182 - 182
Firmware/mesh_bed_calibration.h

@@ -1,182 +1,182 @@
-#ifndef MESH_BED_CALIBRATION_H
-#define MESH_BED_CALIBRATION_H
-
-// Exact positions of the print head above the bed reference points, in the world coordinates.
-// The world coordinates match the machine coordinates only in case, when the machine
-// is built properly, the end stops are at the correct positions and the axes are perpendicular.
-extern const float bed_ref_points[] PROGMEM;
-
-// Is the world2machine correction activated?
-enum World2MachineCorrectionMode
-{
-	WORLD2MACHINE_CORRECTION_NONE  = 0,
-	WORLD2MACHINE_CORRECTION_SHIFT = 1,
-	WORLD2MACHINE_CORRECTION_SKEW  = 2,
-};
-extern uint8_t world2machine_correction_mode;
-// 2x2 transformation matrix from the world coordinates to the machine coordinates.
-// Corrects for the rotation and skew of the machine axes.
-// Used by the planner's plan_buffer_line() and plan_set_position().
-extern float world2machine_rotation_and_skew[2][2];
-extern float world2machine_rotation_and_skew_inv[2][2];
-// Shift of the machine zero point, in the machine coordinates.
-extern float world2machine_shift[2];
-
-// Resets the transformation to identity.
-extern void world2machine_reset();
-// Resets the transformation to identity and update current_position[X,Y] from the servos.
-extern void world2machine_revert_to_uncorrected();
-// Loads the transformation from the EEPROM, if available.
-extern void world2machine_initialize();
-
-// When switching from absolute to corrected coordinates,
-// this will apply an inverse world2machine transformation
-// to current_position[x,y].
-extern void world2machine_update_current();
-
-inline void world2machine(const float &x, const float &y, float &out_x, float &out_y)
-{
-	if (world2machine_correction_mode == WORLD2MACHINE_CORRECTION_NONE) {
-		// No correction.
-		out_x = x;
-		out_y = y;
-	} else {
-		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SKEW) {
-			// Firs the skew & rotation correction.
-			out_x = world2machine_rotation_and_skew[0][0] * x + world2machine_rotation_and_skew[0][1] * y;
-			out_y = world2machine_rotation_and_skew[1][0] * x + world2machine_rotation_and_skew[1][1] * y;
-		}
-		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SHIFT) {
-			// Then add the offset.
-			out_x += world2machine_shift[0];
-			out_y += world2machine_shift[1];
-		}
-	}
-}
-
-inline void world2machine(float &x, float &y)
-{
-	if (world2machine_correction_mode == WORLD2MACHINE_CORRECTION_NONE) {
-		// No correction.
-	} else {
-		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SKEW) {
-			// Firs the skew & rotation correction.
-			float out_x = world2machine_rotation_and_skew[0][0] * x + world2machine_rotation_and_skew[0][1] * y;
-			float out_y = world2machine_rotation_and_skew[1][0] * x + world2machine_rotation_and_skew[1][1] * y;
-			x = out_x;
-			y = out_y;
-		}
-		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SHIFT) {
-			// Then add the offset.
-			x += world2machine_shift[0];
-			y += world2machine_shift[1];
-		}
-	}
-}
-
-inline void machine2world(float x, float y, float &out_x, float &out_y)
-{
-	if (world2machine_correction_mode == WORLD2MACHINE_CORRECTION_NONE) {
-		// No correction.
-		out_x = x;
-		out_y = y;
-	} else {
-		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SHIFT) {
-			// Then add the offset.
-			x -= world2machine_shift[0];
-			y -= world2machine_shift[1];
-		}
-		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SKEW) {
-			// Firs the skew & rotation correction.
-			out_x = world2machine_rotation_and_skew_inv[0][0] * x + world2machine_rotation_and_skew_inv[0][1] * y;
-			out_y = world2machine_rotation_and_skew_inv[1][0] * x + world2machine_rotation_and_skew_inv[1][1] * y;
-		}
-	}
-}
-
-inline void machine2world(float &x, float &y)
-{
-	if (world2machine_correction_mode == WORLD2MACHINE_CORRECTION_NONE) {
-		// No correction.
-	} else {
-		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SHIFT) {
-			// Then add the offset.
-			x -= world2machine_shift[0];
-			y -= world2machine_shift[1];
-		}
-		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SKEW) {
-			// Firs the skew & rotation correction.
-			float out_x = world2machine_rotation_and_skew_inv[0][0] * x + world2machine_rotation_and_skew_inv[0][1] * y;
-			float out_y = world2machine_rotation_and_skew_inv[1][0] * x + world2machine_rotation_and_skew_inv[1][1] * y;
-			x = out_x;
-			y = out_y;
-		}
-	}
-}
-
-inline bool world2machine_clamp(float &x, float &y)
-{
-	bool clamped = false;
-	float tmpx, tmpy;
-    world2machine(x, y, tmpx, tmpy);
-    if (tmpx < X_MIN_POS) {
-        tmpx = X_MIN_POS;
-        clamped = true;
-    }
-    if (tmpy < Y_MIN_POS) {
-        tmpy = Y_MIN_POS;
-        clamped = true;
-    }
-    if (tmpx > X_MAX_POS) {
-        tmpx = X_MAX_POS;
-        clamped = true;
-    }
-    if (tmpy > Y_MAX_POS) {
-        tmpy = Y_MAX_POS;
-        clamped = true;
-    }
-    if (clamped)
-        machine2world(tmpx, tmpy, x, y);
-    return clamped;
-}
-
-extern bool find_bed_induction_sensor_point_z(float minimum_z = -10.f, uint8_t n_iter = 3);
-extern bool find_bed_induction_sensor_point_xy();
-extern void go_home_with_z_lift();
-
-// Positive or zero: ok
-// Negative: failed
-enum BedSkewOffsetDetectionResultType {
-	// Detection failed, some point was not found.
-	BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND   = -1,
-	BED_SKEW_OFFSET_DETECTION_FITTING_FAILED    = -2,
-	
-	// Detection finished with success.
-	BED_SKEW_OFFSET_DETECTION_PERFECT 			= 0,
-	BED_SKEW_OFFSET_DETECTION_SKEW_MILD			= 1,
-	BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME		= 2
-};
-
-extern BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level, uint8_t &too_far_mask);
-extern BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level, uint8_t &too_far_mask);
-
-extern bool sample_mesh_and_store_reference();
-
-extern void reset_bed_offset_and_skew();
-extern bool is_bed_z_jitter_data_valid();
-
-// Scan the mesh bed induction points one by one by a left-right zig-zag movement,
-// write the trigger coordinates to the serial line.
-// Useful for visualizing the behavior of the bed induction detector.
-extern bool scan_bed_induction_points(int8_t verbosity_level);
-
-// Apply Z babystep value from the EEPROM through the planner.
-extern void babystep_apply();
-
-// Undo the current Z babystep value.
-extern void babystep_undo();
-
-// Reset the current babystep counter without moving the axes.
-extern void babystep_reset();
-
-#endif /* MESH_BED_CALIBRATION_H */
+#ifndef MESH_BED_CALIBRATION_H
+#define MESH_BED_CALIBRATION_H
+
+// Exact positions of the print head above the bed reference points, in the world coordinates.
+// The world coordinates match the machine coordinates only in case, when the machine
+// is built properly, the end stops are at the correct positions and the axes are perpendicular.
+extern const float bed_ref_points[] PROGMEM;
+
+// Is the world2machine correction activated?
+enum World2MachineCorrectionMode
+{
+	WORLD2MACHINE_CORRECTION_NONE  = 0,
+	WORLD2MACHINE_CORRECTION_SHIFT = 1,
+	WORLD2MACHINE_CORRECTION_SKEW  = 2,
+};
+extern uint8_t world2machine_correction_mode;
+// 2x2 transformation matrix from the world coordinates to the machine coordinates.
+// Corrects for the rotation and skew of the machine axes.
+// Used by the planner's plan_buffer_line() and plan_set_position().
+extern float world2machine_rotation_and_skew[2][2];
+extern float world2machine_rotation_and_skew_inv[2][2];
+// Shift of the machine zero point, in the machine coordinates.
+extern float world2machine_shift[2];
+
+// Resets the transformation to identity.
+extern void world2machine_reset();
+// Resets the transformation to identity and update current_position[X,Y] from the servos.
+extern void world2machine_revert_to_uncorrected();
+// Loads the transformation from the EEPROM, if available.
+extern void world2machine_initialize();
+
+// When switching from absolute to corrected coordinates,
+// this will apply an inverse world2machine transformation
+// to current_position[x,y].
+extern void world2machine_update_current();
+
+inline void world2machine(const float &x, const float &y, float &out_x, float &out_y)
+{
+	if (world2machine_correction_mode == WORLD2MACHINE_CORRECTION_NONE) {
+		// No correction.
+		out_x = x;
+		out_y = y;
+	} else {
+		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SKEW) {
+			// Firs the skew & rotation correction.
+			out_x = world2machine_rotation_and_skew[0][0] * x + world2machine_rotation_and_skew[0][1] * y;
+			out_y = world2machine_rotation_and_skew[1][0] * x + world2machine_rotation_and_skew[1][1] * y;
+		}
+		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SHIFT) {
+			// Then add the offset.
+			out_x += world2machine_shift[0];
+			out_y += world2machine_shift[1];
+		}
+	}
+}
+
+inline void world2machine(float &x, float &y)
+{
+	if (world2machine_correction_mode == WORLD2MACHINE_CORRECTION_NONE) {
+		// No correction.
+	} else {
+		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SKEW) {
+			// Firs the skew & rotation correction.
+			float out_x = world2machine_rotation_and_skew[0][0] * x + world2machine_rotation_and_skew[0][1] * y;
+			float out_y = world2machine_rotation_and_skew[1][0] * x + world2machine_rotation_and_skew[1][1] * y;
+			x = out_x;
+			y = out_y;
+		}
+		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SHIFT) {
+			// Then add the offset.
+			x += world2machine_shift[0];
+			y += world2machine_shift[1];
+		}
+	}
+}
+
+inline void machine2world(float x, float y, float &out_x, float &out_y)
+{
+	if (world2machine_correction_mode == WORLD2MACHINE_CORRECTION_NONE) {
+		// No correction.
+		out_x = x;
+		out_y = y;
+	} else {
+		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SHIFT) {
+			// Then add the offset.
+			x -= world2machine_shift[0];
+			y -= world2machine_shift[1];
+		}
+		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SKEW) {
+			// Firs the skew & rotation correction.
+			out_x = world2machine_rotation_and_skew_inv[0][0] * x + world2machine_rotation_and_skew_inv[0][1] * y;
+			out_y = world2machine_rotation_and_skew_inv[1][0] * x + world2machine_rotation_and_skew_inv[1][1] * y;
+		}
+	}
+}
+
+inline void machine2world(float &x, float &y)
+{
+	if (world2machine_correction_mode == WORLD2MACHINE_CORRECTION_NONE) {
+		// No correction.
+	} else {
+		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SHIFT) {
+			// Then add the offset.
+			x -= world2machine_shift[0];
+			y -= world2machine_shift[1];
+		}
+		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SKEW) {
+			// Firs the skew & rotation correction.
+			float out_x = world2machine_rotation_and_skew_inv[0][0] * x + world2machine_rotation_and_skew_inv[0][1] * y;
+			float out_y = world2machine_rotation_and_skew_inv[1][0] * x + world2machine_rotation_and_skew_inv[1][1] * y;
+			x = out_x;
+			y = out_y;
+		}
+	}
+}
+
+inline bool world2machine_clamp(float &x, float &y)
+{
+	bool clamped = false;
+	float tmpx, tmpy;
+    world2machine(x, y, tmpx, tmpy);
+    if (tmpx < X_MIN_POS) {
+        tmpx = X_MIN_POS;
+        clamped = true;
+    }
+    if (tmpy < Y_MIN_POS) {
+        tmpy = Y_MIN_POS;
+        clamped = true;
+    }
+    if (tmpx > X_MAX_POS) {
+        tmpx = X_MAX_POS;
+        clamped = true;
+    }
+    if (tmpy > Y_MAX_POS) {
+        tmpy = Y_MAX_POS;
+        clamped = true;
+    }
+    if (clamped)
+        machine2world(tmpx, tmpy, x, y);
+    return clamped;
+}
+
+extern bool find_bed_induction_sensor_point_z(float minimum_z = -10.f, uint8_t n_iter = 3);
+extern bool find_bed_induction_sensor_point_xy();
+extern void go_home_with_z_lift();
+
+// Positive or zero: ok
+// Negative: failed
+enum BedSkewOffsetDetectionResultType {
+	// Detection failed, some point was not found.
+	BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND   = -1,
+	BED_SKEW_OFFSET_DETECTION_FITTING_FAILED    = -2,
+	
+	// Detection finished with success.
+	BED_SKEW_OFFSET_DETECTION_PERFECT 			= 0,
+	BED_SKEW_OFFSET_DETECTION_SKEW_MILD			= 1,
+	BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME		= 2
+};
+
+extern BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level, uint8_t &too_far_mask);
+extern BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level, uint8_t &too_far_mask);
+
+extern bool sample_mesh_and_store_reference();
+
+extern void reset_bed_offset_and_skew();
+extern bool is_bed_z_jitter_data_valid();
+
+// Scan the mesh bed induction points one by one by a left-right zig-zag movement,
+// write the trigger coordinates to the serial line.
+// Useful for visualizing the behavior of the bed induction detector.
+extern bool scan_bed_induction_points(int8_t verbosity_level);
+
+// Apply Z babystep value from the EEPROM through the planner.
+extern void babystep_apply();
+
+// Undo the current Z babystep value.
+extern void babystep_undo();
+
+// Reset the current babystep counter without moving the axes.
+extern void babystep_reset();
+
+#endif /* MESH_BED_CALIBRATION_H */

+ 1986 - 1986
Firmware/temperature.cpp

@@ -1,1986 +1,1986 @@
-/*
-  temperature.c - temperature control
-  Part of Marlin
-  
- Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- 
- This program is free software: you can redistribute it and/or modify
- it under the terms of the GNU General Public License as published by
- the Free Software Foundation, either version 3 of the License, or
- (at your option) any later version.
- 
- This program is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- GNU General Public License for more details.
- 
- You should have received a copy of the GNU General Public License
- along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-/*
- This firmware is a mashup between Sprinter and grbl.
-  (https://github.com/kliment/Sprinter)
-  (https://github.com/simen/grbl/tree)
- 
- It has preliminary support for Matthew Roberts advance algorithm 
-    http://reprap.org/pipermail/reprap-dev/2011-May/003323.html
-
- */
-
-
-#include "Marlin.h"
-#include "ultralcd.h"
-#include "temperature.h"
-#include "watchdog.h"
-#include "cardreader.h"
-
-#include "Sd2PinMap.h"
-
-
-//===========================================================================
-//=============================public variables============================
-//===========================================================================
-int target_temperature[EXTRUDERS] = { 0 };
-int target_temperature_bed = 0;
-int current_temperature_raw[EXTRUDERS] = { 0 };
-float current_temperature[EXTRUDERS] = { 0.0 };
-int current_temperature_bed_raw = 0;
-float current_temperature_bed = 0.0;
-#ifdef TEMP_SENSOR_1_AS_REDUNDANT
-  int redundant_temperature_raw = 0;
-  float redundant_temperature = 0.0;
-#endif
-  
-
-#ifdef PIDTEMP
-  float _Kp, _Ki, _Kd;
-  int pid_cycle, pid_number_of_cycles;
-  bool pid_tuning_finished = false;
-  float Kp=DEFAULT_Kp;
-  float Ki=(DEFAULT_Ki*PID_dT);
-  float Kd=(DEFAULT_Kd/PID_dT);
-  #ifdef PID_ADD_EXTRUSION_RATE
-    float Kc=DEFAULT_Kc;
-  #endif
-#endif //PIDTEMP
-
-#ifdef PIDTEMPBED
-  float bedKp=DEFAULT_bedKp;
-  float bedKi=(DEFAULT_bedKi*PID_dT);
-  float bedKd=(DEFAULT_bedKd/PID_dT);
-#endif //PIDTEMPBED
-  
-#ifdef FAN_SOFT_PWM
-  unsigned char fanSpeedSoftPwm;
-#endif
-
-unsigned char soft_pwm_bed;
-  
-#ifdef BABYSTEPPING
-  volatile int babystepsTodo[3]={0,0,0};
-#endif
-
-#ifdef FILAMENT_SENSOR
-  int current_raw_filwidth = 0;  //Holds measured filament diameter - one extruder only
-#endif  
-//===========================================================================
-//=============================private variables============================
-//===========================================================================
-static volatile bool temp_meas_ready = false;
-
-#ifdef PIDTEMP
-  //static cannot be external:
-  static float temp_iState[EXTRUDERS] = { 0 };
-  static float temp_dState[EXTRUDERS] = { 0 };
-  static float pTerm[EXTRUDERS];
-  static float iTerm[EXTRUDERS];
-  static float dTerm[EXTRUDERS];
-  //int output;
-  static float pid_error[EXTRUDERS];
-  static float temp_iState_min[EXTRUDERS];
-  static float temp_iState_max[EXTRUDERS];
-  // static float pid_input[EXTRUDERS];
-  // static float pid_output[EXTRUDERS];
-  static bool pid_reset[EXTRUDERS];
-#endif //PIDTEMP
-#ifdef PIDTEMPBED
-  //static cannot be external:
-  static float temp_iState_bed = { 0 };
-  static float temp_dState_bed = { 0 };
-  static float pTerm_bed;
-  static float iTerm_bed;
-  static float dTerm_bed;
-  //int output;
-  static float pid_error_bed;
-  static float temp_iState_min_bed;
-  static float temp_iState_max_bed;
-#else //PIDTEMPBED
-	static unsigned long  previous_millis_bed_heater;
-#endif //PIDTEMPBED
-  static unsigned char soft_pwm[EXTRUDERS];
-
-#ifdef FAN_SOFT_PWM
-  static unsigned char soft_pwm_fan;
-#endif
-#if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1) || \
-    (defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1) || \
-    (defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1)
-  static unsigned long extruder_autofan_last_check;
-#endif  
-
-#if EXTRUDERS > 3
-  # error Unsupported number of extruders
-#elif EXTRUDERS > 2
-  # define ARRAY_BY_EXTRUDERS(v1, v2, v3) { v1, v2, v3 }
-#elif EXTRUDERS > 1
-  # define ARRAY_BY_EXTRUDERS(v1, v2, v3) { v1, v2 }
-#else
-  # define ARRAY_BY_EXTRUDERS(v1, v2, v3) { v1 }
-#endif
-
-// Init min and max temp with extreme values to prevent false errors during startup
-static int minttemp_raw[EXTRUDERS] = ARRAY_BY_EXTRUDERS( HEATER_0_RAW_LO_TEMP , HEATER_1_RAW_LO_TEMP , HEATER_2_RAW_LO_TEMP );
-static int maxttemp_raw[EXTRUDERS] = ARRAY_BY_EXTRUDERS( HEATER_0_RAW_HI_TEMP , HEATER_1_RAW_HI_TEMP , HEATER_2_RAW_HI_TEMP );
-static int minttemp[EXTRUDERS] = ARRAY_BY_EXTRUDERS( 0, 0, 0 );
-static int maxttemp[EXTRUDERS] = ARRAY_BY_EXTRUDERS( 16383, 16383, 16383 );
-#ifdef BED_MINTEMP
-static int bed_minttemp_raw = HEATER_BED_RAW_LO_TEMP;
-#endif
-#ifdef BED_MAXTEMP
-static int bed_maxttemp_raw = HEATER_BED_RAW_HI_TEMP;
-#endif
-
-#ifdef TEMP_SENSOR_1_AS_REDUNDANT
-  static void *heater_ttbl_map[2] = {(void *)HEATER_0_TEMPTABLE, (void *)HEATER_1_TEMPTABLE };
-  static uint8_t heater_ttbllen_map[2] = { HEATER_0_TEMPTABLE_LEN, HEATER_1_TEMPTABLE_LEN };
-#else
-  static void *heater_ttbl_map[EXTRUDERS] = ARRAY_BY_EXTRUDERS( (void *)HEATER_0_TEMPTABLE, (void *)HEATER_1_TEMPTABLE, (void *)HEATER_2_TEMPTABLE );
-  static uint8_t heater_ttbllen_map[EXTRUDERS] = ARRAY_BY_EXTRUDERS( HEATER_0_TEMPTABLE_LEN, HEATER_1_TEMPTABLE_LEN, HEATER_2_TEMPTABLE_LEN );
-#endif
-
-static float analog2temp(int raw, uint8_t e);
-static float analog2tempBed(int raw);
-static void updateTemperaturesFromRawValues();
-
-enum TempRunawayStates
-{
-	TempRunaway_INACTIVE = 0,
-	TempRunaway_PREHEAT = 1,
-	TempRunaway_ACTIVE = 2,
-};
-
-#ifdef WATCH_TEMP_PERIOD
-int watch_start_temp[EXTRUDERS] = ARRAY_BY_EXTRUDERS(0,0,0);
-unsigned long watchmillis[EXTRUDERS] = ARRAY_BY_EXTRUDERS(0,0,0);
-#endif //WATCH_TEMP_PERIOD
-
-#ifndef SOFT_PWM_SCALE
-#define SOFT_PWM_SCALE 0
-#endif
-
-#ifdef FILAMENT_SENSOR
-  static int meas_shift_index;  //used to point to a delayed sample in buffer for filament width sensor
-#endif
-//===========================================================================
-//=============================   functions      ============================
-//===========================================================================
-
-  void PID_autotune(float temp, int extruder, int ncycles)
-  {
-  pid_number_of_cycles = ncycles;
-  pid_tuning_finished = false;
-  float input = 0.0;
-  pid_cycle=0;
-  bool heating = true;
-
-  unsigned long temp_millis = millis();
-  unsigned long t1=temp_millis;
-  unsigned long t2=temp_millis;
-  long t_high = 0;
-  long t_low = 0;
-
-  long bias, d;
-  float Ku, Tu;
-  float max = 0, min = 10000;
-
-#if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1) || \
-    (defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1) || \
-    (defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1)
-  unsigned long extruder_autofan_last_check = millis();
-#endif
-
-  if ((extruder >= EXTRUDERS)
-  #if (TEMP_BED_PIN <= -1)
-       ||(extruder < 0)
-  #endif
-       ){
-          SERIAL_ECHOLN("PID Autotune failed. Bad extruder number.");
-		  pid_tuning_finished = true;
-		  pid_cycle = 0;
-          return;
-        }
-	
-  SERIAL_ECHOLN("PID Autotune start");
-  
-  disable_heater(); // switch off all heaters.
-
-  if (extruder<0)
-  {
-     soft_pwm_bed = (MAX_BED_POWER)/2;
-     bias = d = (MAX_BED_POWER)/2;
-   }
-   else
-   {
-     soft_pwm[extruder] = (PID_MAX)/2;
-     bias = d = (PID_MAX)/2;
-  }
-
-
-
-
- for(;;) {
-
-    if(temp_meas_ready == true) { // temp sample ready
-      updateTemperaturesFromRawValues();
-
-      input = (extruder<0)?current_temperature_bed:current_temperature[extruder];
-
-      max=max(max,input);
-      min=min(min,input);
-
-      #if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1) || \
-          (defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1) || \
-          (defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1)
-      if(millis() - extruder_autofan_last_check > 2500) {
-        checkExtruderAutoFans();
-        extruder_autofan_last_check = millis();
-      }
-      #endif
-
-      if(heating == true && input > temp) {
-        if(millis() - t2 > 5000) { 
-          heating=false;
-          if (extruder<0)
-            soft_pwm_bed = (bias - d) >> 1;
-          else
-            soft_pwm[extruder] = (bias - d) >> 1;
-          t1=millis();
-          t_high=t1 - t2;
-          max=temp;
-        }
-      }
-      if(heating == false && input < temp) {
-        if(millis() - t1 > 5000) {
-          heating=true;
-          t2=millis();
-          t_low=t2 - t1;
-          if(pid_cycle > 0) {
-            bias += (d*(t_high - t_low))/(t_low + t_high);
-            bias = constrain(bias, 20 ,(extruder<0?(MAX_BED_POWER):(PID_MAX))-20);
-            if(bias > (extruder<0?(MAX_BED_POWER):(PID_MAX))/2) d = (extruder<0?(MAX_BED_POWER):(PID_MAX)) - 1 - bias;
-            else d = bias;
-
-            SERIAL_PROTOCOLPGM(" bias: "); SERIAL_PROTOCOL(bias);
-            SERIAL_PROTOCOLPGM(" d: "); SERIAL_PROTOCOL(d);
-            SERIAL_PROTOCOLPGM(" min: "); SERIAL_PROTOCOL(min);
-            SERIAL_PROTOCOLPGM(" max: "); SERIAL_PROTOCOLLN(max);
-            if(pid_cycle > 2) {
-              Ku = (4.0*d)/(3.14159*(max-min)/2.0);
-              Tu = ((float)(t_low + t_high)/1000.0);
-              SERIAL_PROTOCOLPGM(" Ku: "); SERIAL_PROTOCOL(Ku);
-              SERIAL_PROTOCOLPGM(" Tu: "); SERIAL_PROTOCOLLN(Tu);
-              _Kp = 0.6*Ku;
-              _Ki = 2*_Kp/Tu;
-              _Kd = _Kp*Tu/8;
-              SERIAL_PROTOCOLLNPGM(" Classic PID ");
-              SERIAL_PROTOCOLPGM(" Kp: "); SERIAL_PROTOCOLLN(_Kp);
-              SERIAL_PROTOCOLPGM(" Ki: "); SERIAL_PROTOCOLLN(_Ki);
-              SERIAL_PROTOCOLPGM(" Kd: "); SERIAL_PROTOCOLLN(_Kd);
-              /*
-              _Kp = 0.33*Ku;
-              _Ki = _Kp/Tu;
-              _Kd = _Kp*Tu/3;
-              SERIAL_PROTOCOLLNPGM(" Some overshoot ");
-              SERIAL_PROTOCOLPGM(" Kp: "); SERIAL_PROTOCOLLN(_Kp);
-              SERIAL_PROTOCOLPGM(" Ki: "); SERIAL_PROTOCOLLN(_Ki);
-              SERIAL_PROTOCOLPGM(" Kd: "); SERIAL_PROTOCOLLN(_Kd);
-              _Kp = 0.2*Ku;
-              _Ki = 2*_Kp/Tu;
-              _Kd = _Kp*Tu/3;
-              SERIAL_PROTOCOLLNPGM(" No overshoot ");
-              SERIAL_PROTOCOLPGM(" Kp: "); SERIAL_PROTOCOLLN(_Kp);
-              SERIAL_PROTOCOLPGM(" Ki: "); SERIAL_PROTOCOLLN(_Ki);
-              SERIAL_PROTOCOLPGM(" Kd: "); SERIAL_PROTOCOLLN(_Kd);
-              */
-            }
-          }
-          if (extruder<0)
-            soft_pwm_bed = (bias + d) >> 1;
-          else
-            soft_pwm[extruder] = (bias + d) >> 1;
-          pid_cycle++;
-          min=temp;
-        }
-      } 
-    }
-    if(input > (temp + 20)) {
-      SERIAL_PROTOCOLLNPGM("PID Autotune failed! Temperature too high");
-	  pid_tuning_finished = true;
-	  pid_cycle = 0;
-      return;
-    }
-    if(millis() - temp_millis > 2000) {
-      int p;
-      if (extruder<0){
-        p=soft_pwm_bed;       
-        SERIAL_PROTOCOLPGM("ok B:");
-      }else{
-        p=soft_pwm[extruder];       
-        SERIAL_PROTOCOLPGM("ok T:");
-      }
-			
-      SERIAL_PROTOCOL(input);   
-      SERIAL_PROTOCOLPGM(" @:");
-      SERIAL_PROTOCOLLN(p);       
-
-      temp_millis = millis();
-    }
-    if(((millis() - t1) + (millis() - t2)) > (10L*60L*1000L*2L)) {
-      SERIAL_PROTOCOLLNPGM("PID Autotune failed! timeout");
-	  pid_tuning_finished = true;
-	  pid_cycle = 0;
-      return;
-    }
-    if(pid_cycle > ncycles) {
-      SERIAL_PROTOCOLLNPGM("PID Autotune finished! Put the last Kp, Ki and Kd constants from above into Configuration.h");
-	  pid_tuning_finished = true;
-	  pid_cycle = 0;
-      return;
-    }
-    lcd_update();
-  }
-}
-
-void updatePID()
-{
-#ifdef PIDTEMP
-  for(int e = 0; e < EXTRUDERS; e++) { 
-     temp_iState_max[e] = PID_INTEGRAL_DRIVE_MAX / Ki;  
-  }
-#endif
-#ifdef PIDTEMPBED
-  temp_iState_max_bed = PID_INTEGRAL_DRIVE_MAX / bedKi;  
-#endif
-}
-  
-int getHeaterPower(int heater) {
-	if (heater<0)
-		return soft_pwm_bed;
-  return soft_pwm[heater];
-}
-
-#if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1) || \
-    (defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1) || \
-    (defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1)
-
-  #if defined(FAN_PIN) && FAN_PIN > -1
-    #if EXTRUDER_0_AUTO_FAN_PIN == FAN_PIN 
-       #error "You cannot set EXTRUDER_0_AUTO_FAN_PIN equal to FAN_PIN"
-    #endif
-    #if EXTRUDER_1_AUTO_FAN_PIN == FAN_PIN 
-       #error "You cannot set EXTRUDER_1_AUTO_FAN_PIN equal to FAN_PIN"
-    #endif
-    #if EXTRUDER_2_AUTO_FAN_PIN == FAN_PIN 
-       #error "You cannot set EXTRUDER_2_AUTO_FAN_PIN equal to FAN_PIN"
-    #endif
-  #endif 
-
-void setExtruderAutoFanState(int pin, bool state)
-{
-  unsigned char newFanSpeed = (state != 0) ? EXTRUDER_AUTO_FAN_SPEED : 0;
-  // this idiom allows both digital and PWM fan outputs (see M42 handling).
-  pinMode(pin, OUTPUT);
-  digitalWrite(pin, newFanSpeed);
-  analogWrite(pin, newFanSpeed);
-}
-
-void checkExtruderAutoFans()
-{
-  uint8_t fanState = 0;
-
-  // which fan pins need to be turned on?      
-  #if defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1
-    if (current_temperature[0] > EXTRUDER_AUTO_FAN_TEMPERATURE) 
-      fanState |= 1;
-  #endif
-  #if defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1
-    if (current_temperature[1] > EXTRUDER_AUTO_FAN_TEMPERATURE) 
-    {
-      if (EXTRUDER_1_AUTO_FAN_PIN == EXTRUDER_0_AUTO_FAN_PIN) 
-        fanState |= 1;
-      else
-        fanState |= 2;
-    }
-  #endif
-  #if defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1
-    if (current_temperature[2] > EXTRUDER_AUTO_FAN_TEMPERATURE) 
-    {
-      if (EXTRUDER_2_AUTO_FAN_PIN == EXTRUDER_0_AUTO_FAN_PIN) 
-        fanState |= 1;
-      else if (EXTRUDER_2_AUTO_FAN_PIN == EXTRUDER_1_AUTO_FAN_PIN) 
-        fanState |= 2;
-      else
-        fanState |= 4;
-    }
-  #endif
-  
-  // update extruder auto fan states
-  #if defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1
-    setExtruderAutoFanState(EXTRUDER_0_AUTO_FAN_PIN, (fanState & 1) != 0);
-  #endif 
-  #if defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1
-    if (EXTRUDER_1_AUTO_FAN_PIN != EXTRUDER_0_AUTO_FAN_PIN) 
-      setExtruderAutoFanState(EXTRUDER_1_AUTO_FAN_PIN, (fanState & 2) != 0);
-  #endif 
-  #if defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1
-    if (EXTRUDER_2_AUTO_FAN_PIN != EXTRUDER_0_AUTO_FAN_PIN 
-        && EXTRUDER_2_AUTO_FAN_PIN != EXTRUDER_1_AUTO_FAN_PIN)
-      setExtruderAutoFanState(EXTRUDER_2_AUTO_FAN_PIN, (fanState & 4) != 0);
-  #endif 
-}
-
-#endif // any extruder auto fan pins set
-
-void manage_heater()
-{
-  float pid_input;
-  float pid_output;
-
-  if(temp_meas_ready != true)   //better readability
-    return; 
-
-  updateTemperaturesFromRawValues();
-
-#ifdef TEMP_RUNAWAY_BED_HYSTERESIS
-  temp_runaway_check(0, target_temperature_bed, current_temperature_bed, (int)soft_pwm_bed, true);
-#endif
-
-  for(int e = 0; e < EXTRUDERS; e++) 
-  {
-
-#ifdef TEMP_RUNAWAY_EXTRUDER_HYSTERESIS
-	  temp_runaway_check(e+1, target_temperature[e], current_temperature[e], (int)soft_pwm[e], false);
-#endif
-
-  #ifdef PIDTEMP
-    pid_input = current_temperature[e];
-
-    #ifndef PID_OPENLOOP
-        pid_error[e] = target_temperature[e] - pid_input;
-        if(pid_error[e] > PID_FUNCTIONAL_RANGE) {
-          pid_output = BANG_MAX;
-          pid_reset[e] = true;
-        }
-        else if(pid_error[e] < -PID_FUNCTIONAL_RANGE || target_temperature[e] == 0) {
-          pid_output = 0;
-          pid_reset[e] = true;
-        }
-        else {
-          if(pid_reset[e] == true) {
-            temp_iState[e] = 0.0;
-            pid_reset[e] = false;
-          }
-          pTerm[e] = Kp * pid_error[e];
-          temp_iState[e] += pid_error[e];
-          temp_iState[e] = constrain(temp_iState[e], temp_iState_min[e], temp_iState_max[e]);
-          iTerm[e] = Ki * temp_iState[e];
-
-          //K1 defined in Configuration.h in the PID settings
-          #define K2 (1.0-K1)
-          dTerm[e] = (Kd * (pid_input - temp_dState[e]))*K2 + (K1 * dTerm[e]);
-          pid_output = pTerm[e] + iTerm[e] - dTerm[e];
-          if (pid_output > PID_MAX) {
-            if (pid_error[e] > 0 )  temp_iState[e] -= pid_error[e]; // conditional un-integration
-            pid_output=PID_MAX;
-          } else if (pid_output < 0){
-            if (pid_error[e] < 0 )  temp_iState[e] -= pid_error[e]; // conditional un-integration
-            pid_output=0;
-          }
-        }
-        temp_dState[e] = pid_input;
-    #else 
-          pid_output = constrain(target_temperature[e], 0, PID_MAX);
-    #endif //PID_OPENLOOP
-    #ifdef PID_DEBUG
-    SERIAL_ECHO_START;
-    SERIAL_ECHO(" PID_DEBUG ");
-    SERIAL_ECHO(e);
-    SERIAL_ECHO(": Input ");
-    SERIAL_ECHO(pid_input);
-    SERIAL_ECHO(" Output ");
-    SERIAL_ECHO(pid_output);
-    SERIAL_ECHO(" pTerm ");
-    SERIAL_ECHO(pTerm[e]);
-    SERIAL_ECHO(" iTerm ");
-    SERIAL_ECHO(iTerm[e]);
-    SERIAL_ECHO(" dTerm ");
-    SERIAL_ECHOLN(dTerm[e]);
-    #endif //PID_DEBUG
-  #else /* PID off */
-    pid_output = 0;
-    if(current_temperature[e] < target_temperature[e]) {
-      pid_output = PID_MAX;
-    }
-  #endif
-
-    // Check if temperature is within the correct range
-    if((current_temperature[e] > minttemp[e]) && (current_temperature[e] < maxttemp[e])) 
-    {
-      soft_pwm[e] = (int)pid_output >> 1;
-    }
-    else {
-      soft_pwm[e] = 0;
-    }
-
-    #ifdef WATCH_TEMP_PERIOD
-    if(watchmillis[e] && millis() - watchmillis[e] > WATCH_TEMP_PERIOD)
-    {
-        if(degHotend(e) < watch_start_temp[e] + WATCH_TEMP_INCREASE)
-        {
-            setTargetHotend(0, e);
-            LCD_MESSAGEPGM("Heating failed");
-            SERIAL_ECHO_START;
-            SERIAL_ECHOLN("Heating failed");
-        }else{
-            watchmillis[e] = 0;
-        }
-    }
-    #endif
-    #ifdef TEMP_SENSOR_1_AS_REDUNDANT
-      if(fabs(current_temperature[0] - redundant_temperature) > MAX_REDUNDANT_TEMP_SENSOR_DIFF) {
-        disable_heater();
-        if(IsStopped() == false) {
-          SERIAL_ERROR_START;
-          SERIAL_ERRORLNPGM("Extruder switched off. Temperature difference between temp sensors is too high !");
-          LCD_ALERTMESSAGEPGM("Err: REDUNDANT TEMP ERROR");
-        }
-        #ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE
-          Stop();
-        #endif
-      }
-    #endif
-  } // End extruder for loop
-
-  #if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1) || \
-      (defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1) || \
-      (defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1)
-  if(millis() - extruder_autofan_last_check > 2500)  // only need to check fan state very infrequently
-  {
-    checkExtruderAutoFans();
-    extruder_autofan_last_check = millis();
-  }  
-  #endif       
-  
-  #ifndef PIDTEMPBED
-  if(millis() - previous_millis_bed_heater < BED_CHECK_INTERVAL)
-    return;
-  previous_millis_bed_heater = millis();
-  #endif
-
-  #if TEMP_SENSOR_BED != 0
-
-  #ifdef PIDTEMPBED
-    pid_input = current_temperature_bed;
-
-    #ifndef PID_OPENLOOP
-		  pid_error_bed = target_temperature_bed - pid_input;
-		  pTerm_bed = bedKp * pid_error_bed;
-		  temp_iState_bed += pid_error_bed;
-		  temp_iState_bed = constrain(temp_iState_bed, temp_iState_min_bed, temp_iState_max_bed);
-		  iTerm_bed = bedKi * temp_iState_bed;
-
-		  //K1 defined in Configuration.h in the PID settings
-		  #define K2 (1.0-K1)
-		  dTerm_bed= (bedKd * (pid_input - temp_dState_bed))*K2 + (K1 * dTerm_bed);
-		  temp_dState_bed = pid_input;
-
-		  pid_output = pTerm_bed + iTerm_bed - dTerm_bed;
-          	  if (pid_output > MAX_BED_POWER) {
-            	    if (pid_error_bed > 0 )  temp_iState_bed -= pid_error_bed; // conditional un-integration
-                    pid_output=MAX_BED_POWER;
-          	  } else if (pid_output < 0){
-            	    if (pid_error_bed < 0 )  temp_iState_bed -= pid_error_bed; // conditional un-integration
-                    pid_output=0;
-                  }
-
-    #else 
-      pid_output = constrain(target_temperature_bed, 0, MAX_BED_POWER);
-    #endif //PID_OPENLOOP
-
-	  if((current_temperature_bed > BED_MINTEMP) && (current_temperature_bed < BED_MAXTEMP)) 
-	  {
-	    soft_pwm_bed = (int)pid_output >> 1;
-	  }
-	  else {
-	    soft_pwm_bed = 0;
-	  }
-
-    #elif !defined(BED_LIMIT_SWITCHING)
-      // Check if temperature is within the correct range
-      if((current_temperature_bed > BED_MINTEMP) && (current_temperature_bed < BED_MAXTEMP))
-      {
-        if(current_temperature_bed >= target_temperature_bed)
-        {
-          soft_pwm_bed = 0;
-        }
-        else 
-        {
-          soft_pwm_bed = MAX_BED_POWER>>1;
-        }
-      }
-      else
-      {
-        soft_pwm_bed = 0;
-        WRITE(HEATER_BED_PIN,LOW);
-      }
-    #else //#ifdef BED_LIMIT_SWITCHING
-      // Check if temperature is within the correct band
-      if((current_temperature_bed > BED_MINTEMP) && (current_temperature_bed < BED_MAXTEMP))
-      {
-        if(current_temperature_bed > target_temperature_bed + BED_HYSTERESIS)
-        {
-          soft_pwm_bed = 0;
-        }
-        else if(current_temperature_bed <= target_temperature_bed - BED_HYSTERESIS)
-        {
-          soft_pwm_bed = MAX_BED_POWER>>1;
-        }
-      }
-      else
-      {
-        soft_pwm_bed = 0;
-        WRITE(HEATER_BED_PIN,LOW);
-      }
-    #endif
-  #endif
-  
-//code for controlling the extruder rate based on the width sensor 
-#ifdef FILAMENT_SENSOR
-  if(filament_sensor) 
-	{
-	meas_shift_index=delay_index1-meas_delay_cm;
-		  if(meas_shift_index<0)
-			  meas_shift_index = meas_shift_index + (MAX_MEASUREMENT_DELAY+1);  //loop around buffer if needed
-		  
-		  //get the delayed info and add 100 to reconstitute to a percent of the nominal filament diameter
-		  //then square it to get an area
-		  
-		  if(meas_shift_index<0)
-			  meas_shift_index=0;
-		  else if (meas_shift_index>MAX_MEASUREMENT_DELAY)
-			  meas_shift_index=MAX_MEASUREMENT_DELAY;
-		  
-		     volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] = pow((float)(100+measurement_delay[meas_shift_index])/100.0,2);
-		     if (volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] <0.01)
-		    	 volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM]=0.01;
-	}
-#endif
-}
-
-#define PGM_RD_W(x)   (short)pgm_read_word(&x)
-// Derived from RepRap FiveD extruder::getTemperature()
-// For hot end temperature measurement.
-static float analog2temp(int raw, uint8_t e) {
-#ifdef TEMP_SENSOR_1_AS_REDUNDANT
-  if(e > EXTRUDERS)
-#else
-  if(e >= EXTRUDERS)
-#endif
-  {
-      SERIAL_ERROR_START;
-      SERIAL_ERROR((int)e);
-      SERIAL_ERRORLNPGM(" - Invalid extruder number !");
-      kill();
-      return 0.0;
-  } 
-  #ifdef HEATER_0_USES_MAX6675
-    if (e == 0)
-    {
-      return 0.25 * raw;
-    }
-  #endif
-
-  if(heater_ttbl_map[e] != NULL)
-  {
-    float celsius = 0;
-    uint8_t i;
-    short (*tt)[][2] = (short (*)[][2])(heater_ttbl_map[e]);
-
-    for (i=1; i<heater_ttbllen_map[e]; i++)
-    {
-      if (PGM_RD_W((*tt)[i][0]) > raw)
-      {
-        celsius = PGM_RD_W((*tt)[i-1][1]) + 
-          (raw - PGM_RD_W((*tt)[i-1][0])) * 
-          (float)(PGM_RD_W((*tt)[i][1]) - PGM_RD_W((*tt)[i-1][1])) /
-          (float)(PGM_RD_W((*tt)[i][0]) - PGM_RD_W((*tt)[i-1][0]));
-        break;
-      }
-    }
-
-    // Overflow: Set to last value in the table
-    if (i == heater_ttbllen_map[e]) celsius = PGM_RD_W((*tt)[i-1][1]);
-
-    return celsius;
-  }
-  return ((raw * ((5.0 * 100.0) / 1024.0) / OVERSAMPLENR) * TEMP_SENSOR_AD595_GAIN) + TEMP_SENSOR_AD595_OFFSET;
-}
-
-// Derived from RepRap FiveD extruder::getTemperature()
-// For bed temperature measurement.
-static float analog2tempBed(int raw) {
-  #ifdef BED_USES_THERMISTOR
-    float celsius = 0;
-    byte i;
-
-    for (i=1; i<BEDTEMPTABLE_LEN; i++)
-    {
-      if (PGM_RD_W(BEDTEMPTABLE[i][0]) > raw)
-      {
-        celsius  = PGM_RD_W(BEDTEMPTABLE[i-1][1]) + 
-          (raw - PGM_RD_W(BEDTEMPTABLE[i-1][0])) * 
-          (float)(PGM_RD_W(BEDTEMPTABLE[i][1]) - PGM_RD_W(BEDTEMPTABLE[i-1][1])) /
-          (float)(PGM_RD_W(BEDTEMPTABLE[i][0]) - PGM_RD_W(BEDTEMPTABLE[i-1][0]));
-        break;
-      }
-    }
-
-    // Overflow: Set to last value in the table
-    if (i == BEDTEMPTABLE_LEN) celsius = PGM_RD_W(BEDTEMPTABLE[i-1][1]);
-
-
-	// temperature offset adjustment
-#ifdef BED_OFFSET
-	float _offset = BED_OFFSET;
-	float _offset_center = BED_OFFSET_CENTER;
-	float _offset_start = BED_OFFSET_START;
-	float _first_koef = (_offset / 2) / (_offset_center - _offset_start);
-	float _second_koef = (_offset / 2) / (100 - _offset_center);
-
-
-	if (celsius >= _offset_start && celsius <= _offset_center)
-	{
-		celsius = celsius + (_first_koef * (celsius - _offset_start));
-	}
-	else if (celsius > _offset_center && celsius <= 100)
-	{
-		celsius = celsius + (_first_koef * (_offset_center - _offset_start)) + ( _second_koef * ( celsius - ( 100 - _offset_center ) )) ;
-	}
-	else if (celsius > 100)
-	{
-		celsius = celsius + _offset;
-	}
-#endif
-
-
-    return celsius;
-  #elif defined BED_USES_AD595
-    return ((raw * ((5.0 * 100.0) / 1024.0) / OVERSAMPLENR) * TEMP_SENSOR_AD595_GAIN) + TEMP_SENSOR_AD595_OFFSET;
-  #else
-    return 0;
-  #endif
-}
-
-/* Called to get the raw values into the the actual temperatures. The raw values are created in interrupt context,
-    and this function is called from normal context as it is too slow to run in interrupts and will block the stepper routine otherwise */
-static void updateTemperaturesFromRawValues()
-{
-    for(uint8_t e=0;e<EXTRUDERS;e++)
-    {
-        current_temperature[e] = analog2temp(current_temperature_raw[e], e);
-    }
-    
-	current_temperature_bed = analog2tempBed(current_temperature_bed_raw);
-
-    #ifdef TEMP_SENSOR_1_AS_REDUNDANT
-      redundant_temperature = analog2temp(redundant_temperature_raw, 1);
-    #endif
-    #if defined (FILAMENT_SENSOR) && (FILWIDTH_PIN > -1)    //check if a sensor is supported 
-      filament_width_meas = analog2widthFil();
-    #endif  
-    //Reset the watchdog after we know we have a temperature measurement.
-    watchdog_reset();
-
-    CRITICAL_SECTION_START;
-    temp_meas_ready = false;
-    CRITICAL_SECTION_END;
-}
-
-
-// For converting raw Filament Width to milimeters 
-#ifdef FILAMENT_SENSOR
-float analog2widthFil() { 
-return current_raw_filwidth/16383.0*5.0; 
-//return current_raw_filwidth; 
-} 
- 
-// For converting raw Filament Width to a ratio 
-int widthFil_to_size_ratio() { 
- 
-float temp; 
-      
-temp=filament_width_meas;
-if(filament_width_meas<MEASURED_LOWER_LIMIT)
-	temp=filament_width_nominal;  //assume sensor cut out
-else if (filament_width_meas>MEASURED_UPPER_LIMIT)
-	temp= MEASURED_UPPER_LIMIT;
-
-
-return(filament_width_nominal/temp*100); 
-
-
-} 
-#endif
-
-
-
-
-
-void tp_init()
-{
-#if MB(RUMBA) && ((TEMP_SENSOR_0==-1)||(TEMP_SENSOR_1==-1)||(TEMP_SENSOR_2==-1)||(TEMP_SENSOR_BED==-1))
-  //disable RUMBA JTAG in case the thermocouple extension is plugged on top of JTAG connector
-  MCUCR=(1<<JTD); 
-  MCUCR=(1<<JTD);
-#endif
-  
-  // Finish init of mult extruder arrays 
-  for(int e = 0; e < EXTRUDERS; e++) {
-    // populate with the first value 
-    maxttemp[e] = maxttemp[0];
-#ifdef PIDTEMP
-    temp_iState_min[e] = 0.0;
-    temp_iState_max[e] = PID_INTEGRAL_DRIVE_MAX / Ki;
-#endif //PIDTEMP
-#ifdef PIDTEMPBED
-    temp_iState_min_bed = 0.0;
-    temp_iState_max_bed = PID_INTEGRAL_DRIVE_MAX / bedKi;
-#endif //PIDTEMPBED
-  }
-
-  #if defined(HEATER_0_PIN) && (HEATER_0_PIN > -1) 
-    SET_OUTPUT(HEATER_0_PIN);
-  #endif  
-  #if defined(HEATER_1_PIN) && (HEATER_1_PIN > -1) 
-    SET_OUTPUT(HEATER_1_PIN);
-  #endif  
-  #if defined(HEATER_2_PIN) && (HEATER_2_PIN > -1) 
-    SET_OUTPUT(HEATER_2_PIN);
-  #endif  
-  #if defined(HEATER_BED_PIN) && (HEATER_BED_PIN > -1) 
-    SET_OUTPUT(HEATER_BED_PIN);
-  #endif  
-  #if defined(FAN_PIN) && (FAN_PIN > -1) 
-    SET_OUTPUT(FAN_PIN);
-    #ifdef FAST_PWM_FAN
-    setPwmFrequency(FAN_PIN, 1); // No prescaling. Pwm frequency = F_CPU/256/8
-    #endif
-    #ifdef FAN_SOFT_PWM
-    soft_pwm_fan = fanSpeedSoftPwm / 2;
-    #endif
-  #endif  
-
-  #ifdef HEATER_0_USES_MAX6675
-    #ifndef SDSUPPORT
-      SET_OUTPUT(SCK_PIN);
-      WRITE(SCK_PIN,0);
-    
-      SET_OUTPUT(MOSI_PIN);
-      WRITE(MOSI_PIN,1);
-    
-      SET_INPUT(MISO_PIN);
-      WRITE(MISO_PIN,1);
-    #endif
-    /* Using pinMode and digitalWrite, as that was the only way I could get it to compile */
-    
-    //Have to toggle SD card CS pin to low first, to enable firmware to talk with SD card
-	pinMode(SS_PIN, OUTPUT);
-	digitalWrite(SS_PIN,0);  
-	pinMode(MAX6675_SS, OUTPUT);
-	digitalWrite(MAX6675_SS,1);
-  #endif
-
-  // Set analog inputs
-  ADCSRA = 1<<ADEN | 1<<ADSC | 1<<ADIF | 0x07;
-  DIDR0 = 0;
-  #ifdef DIDR2
-    DIDR2 = 0;
-  #endif
-  #if defined(TEMP_0_PIN) && (TEMP_0_PIN > -1)
-    #if TEMP_0_PIN < 8
-       DIDR0 |= 1 << TEMP_0_PIN; 
-    #else
-       DIDR2 |= 1<<(TEMP_0_PIN - 8); 
-    #endif
-  #endif
-  #if defined(TEMP_1_PIN) && (TEMP_1_PIN > -1)
-    #if TEMP_1_PIN < 8
-       DIDR0 |= 1<<TEMP_1_PIN; 
-    #else
-       DIDR2 |= 1<<(TEMP_1_PIN - 8); 
-    #endif
-  #endif
-  #if defined(TEMP_2_PIN) && (TEMP_2_PIN > -1)
-    #if TEMP_2_PIN < 8
-       DIDR0 |= 1 << TEMP_2_PIN; 
-    #else
-       DIDR2 |= 1<<(TEMP_2_PIN - 8); 
-    #endif
-  #endif
-  #if defined(TEMP_BED_PIN) && (TEMP_BED_PIN > -1)
-    #if TEMP_BED_PIN < 8
-       DIDR0 |= 1<<TEMP_BED_PIN; 
-    #else
-       DIDR2 |= 1<<(TEMP_BED_PIN - 8); 
-    #endif
-  #endif
-  
-  //Added for Filament Sensor 
-  #ifdef FILAMENT_SENSOR
-   #if defined(FILWIDTH_PIN) && (FILWIDTH_PIN > -1) 
-	#if FILWIDTH_PIN < 8 
-       	   DIDR0 |= 1<<FILWIDTH_PIN;  
-	#else 
-       	   DIDR2 |= 1<<(FILWIDTH_PIN - 8);  
-	#endif 
-   #endif
-  #endif
-  
-  // Use timer0 for temperature measurement
-  // Interleave temperature interrupt with millies interrupt
-  OCR0B = 128;
-  TIMSK0 |= (1<<OCIE0B);  
-  
-  // Wait for temperature measurement to settle
-  delay(250);
-
-#ifdef HEATER_0_MINTEMP
-  minttemp[0] = HEATER_0_MINTEMP;
-  while(analog2temp(minttemp_raw[0], 0) < HEATER_0_MINTEMP) {
-#if HEATER_0_RAW_LO_TEMP < HEATER_0_RAW_HI_TEMP
-    minttemp_raw[0] += OVERSAMPLENR;
-#else
-    minttemp_raw[0] -= OVERSAMPLENR;
-#endif
-  }
-#endif //MINTEMP
-#ifdef HEATER_0_MAXTEMP
-  maxttemp[0] = HEATER_0_MAXTEMP;
-  while(analog2temp(maxttemp_raw[0], 0) > HEATER_0_MAXTEMP) {
-#if HEATER_0_RAW_LO_TEMP < HEATER_0_RAW_HI_TEMP
-    maxttemp_raw[0] -= OVERSAMPLENR;
-#else
-    maxttemp_raw[0] += OVERSAMPLENR;
-#endif
-  }
-#endif //MAXTEMP
-
-#if (EXTRUDERS > 1) && defined(HEATER_1_MINTEMP)
-  minttemp[1] = HEATER_1_MINTEMP;
-  while(analog2temp(minttemp_raw[1], 1) < HEATER_1_MINTEMP) {
-#if HEATER_1_RAW_LO_TEMP < HEATER_1_RAW_HI_TEMP
-    minttemp_raw[1] += OVERSAMPLENR;
-#else
-    minttemp_raw[1] -= OVERSAMPLENR;
-#endif
-  }
-#endif // MINTEMP 1
-#if (EXTRUDERS > 1) && defined(HEATER_1_MAXTEMP)
-  maxttemp[1] = HEATER_1_MAXTEMP;
-  while(analog2temp(maxttemp_raw[1], 1) > HEATER_1_MAXTEMP) {
-#if HEATER_1_RAW_LO_TEMP < HEATER_1_RAW_HI_TEMP
-    maxttemp_raw[1] -= OVERSAMPLENR;
-#else
-    maxttemp_raw[1] += OVERSAMPLENR;
-#endif
-  }
-#endif //MAXTEMP 1
-
-#if (EXTRUDERS > 2) && defined(HEATER_2_MINTEMP)
-  minttemp[2] = HEATER_2_MINTEMP;
-  while(analog2temp(minttemp_raw[2], 2) < HEATER_2_MINTEMP) {
-#if HEATER_2_RAW_LO_TEMP < HEATER_2_RAW_HI_TEMP
-    minttemp_raw[2] += OVERSAMPLENR;
-#else
-    minttemp_raw[2] -= OVERSAMPLENR;
-#endif
-  }
-#endif //MINTEMP 2
-#if (EXTRUDERS > 2) && defined(HEATER_2_MAXTEMP)
-  maxttemp[2] = HEATER_2_MAXTEMP;
-  while(analog2temp(maxttemp_raw[2], 2) > HEATER_2_MAXTEMP) {
-#if HEATER_2_RAW_LO_TEMP < HEATER_2_RAW_HI_TEMP
-    maxttemp_raw[2] -= OVERSAMPLENR;
-#else
-    maxttemp_raw[2] += OVERSAMPLENR;
-#endif
-  }
-#endif //MAXTEMP 2
-
-#ifdef BED_MINTEMP
-  /* No bed MINTEMP error implemented?!? */
-  while(analog2tempBed(bed_minttemp_raw) < BED_MINTEMP) {
-#if HEATER_BED_RAW_LO_TEMP < HEATER_BED_RAW_HI_TEMP
-    bed_minttemp_raw += OVERSAMPLENR;
-#else
-    bed_minttemp_raw -= OVERSAMPLENR;
-#endif
-  }
-  
-#endif //BED_MINTEMP
-#ifdef BED_MAXTEMP
-  while(analog2tempBed(bed_maxttemp_raw) > BED_MAXTEMP) {
-#if HEATER_BED_RAW_LO_TEMP < HEATER_BED_RAW_HI_TEMP
-    bed_maxttemp_raw -= OVERSAMPLENR;
-#else
-    bed_maxttemp_raw += OVERSAMPLENR;
-#endif
-  }
-#endif //BED_MAXTEMP
-}
-
-void setWatch() 
-{  
-#ifdef WATCH_TEMP_PERIOD
-  for (int e = 0; e < EXTRUDERS; e++)
-  {
-    if(degHotend(e) < degTargetHotend(e) - (WATCH_TEMP_INCREASE * 2))
-    {
-      watch_start_temp[e] = degHotend(e);
-      watchmillis[e] = millis();
-    } 
-  }
-#endif 
-}
-
-#if (defined (TEMP_RUNAWAY_BED_HYSTERESIS) && TEMP_RUNAWAY_BED_TIMEOUT > 0) || (defined (TEMP_RUNAWAY_EXTRUDER_HYSTERESIS) && TEMP_RUNAWAY_EXTRUDER_TIMEOUT > 0)
-void temp_runaway_check(int _heater_id, float _target_temperature, float _current_temperature, float _output, bool _isbed)
-{
-	float __hysteresis = 0;
-	int __timeout = 0;
-	bool temp_runaway_check_active = false;
-	static float __preheat_start[2] = { 0,0}; //currently just bed and one extruder
-	static int __preheat_counter[2] = { 0,0};
-	static int __preheat_errors[2] = { 0,0};
-		
-
-#ifdef 	TEMP_RUNAWAY_BED_TIMEOUT
-	if (_isbed)
-	{
-		__hysteresis = TEMP_RUNAWAY_BED_HYSTERESIS;
-		__timeout = TEMP_RUNAWAY_BED_TIMEOUT;
-	}
-#endif
-#ifdef 	TEMP_RUNAWAY_EXTRUDER_TIMEOUT
-	if (!_isbed)
-	{
-		__hysteresis = TEMP_RUNAWAY_EXTRUDER_HYSTERESIS;
-		__timeout = TEMP_RUNAWAY_EXTRUDER_TIMEOUT;
-	}
-#endif
-
-	if (millis() - temp_runaway_timer[_heater_id] > 2000)
-	{
-
-		temp_runaway_timer[_heater_id] = millis();
-		if (_output == 0)
-		{
-			temp_runaway_check_active = false;
-			temp_runaway_error_counter[_heater_id] = 0;
-		}
-
-		if (temp_runaway_target[_heater_id] != _target_temperature)
-		{
-			if (_target_temperature > 0)
-			{
-				temp_runaway_status[_heater_id] = TempRunaway_PREHEAT;
-				temp_runaway_target[_heater_id] = _target_temperature;
-				__preheat_start[_heater_id] = _current_temperature;
-				__preheat_counter[_heater_id] = 0;
-			}
-			else
-			{
-				temp_runaway_status[_heater_id] = TempRunaway_INACTIVE;
-				temp_runaway_target[_heater_id] = _target_temperature;
-			}
-		}
-
-		if (temp_runaway_status[_heater_id] == TempRunaway_PREHEAT)
-		{
-			if (_current_temperature < ((_isbed) ? (0.8 * _target_temperature) : 150)) //check only in area where temperature is changing fastly for heater, check to 0.8 x target temperature for bed
-			{
-				__preheat_counter[_heater_id]++;
-				if (__preheat_counter[_heater_id] > ((_isbed) ? 16 : 8)) // periodicaly check if current temperature changes
-				{
-					/*SERIAL_ECHOPGM("Heater:");
-					MYSERIAL.print(_heater_id);
-					SERIAL_ECHOPGM(" T:");
-					MYSERIAL.print(_current_temperature);
-					SERIAL_ECHOPGM(" Tstart:");
-					MYSERIAL.print(__preheat_start[_heater_id]);*/
-					
-					if (_current_temperature - __preheat_start[_heater_id] < 2) {
-						__preheat_errors[_heater_id]++;
-						/*SERIAL_ECHOPGM(" Preheat errors:");
-						MYSERIAL.println(__preheat_errors[_heater_id]);*/
-					}
-					else {
-						//SERIAL_ECHOLNPGM("");
-						__preheat_errors[_heater_id] = 0;
-					}
-
-					if (__preheat_errors[_heater_id] > ((_isbed) ? 2 : 5)) 
-					{
-						if (farm_mode) { prusa_statistics(0); }
-						temp_runaway_stop(true, _isbed);
-						if (farm_mode) { prusa_statistics(91); }
-					}
-					__preheat_start[_heater_id] = _current_temperature;
-					__preheat_counter[_heater_id] = 0;
-				}
-			}
-		}
-
-		if (_current_temperature >= _target_temperature  && temp_runaway_status[_heater_id] == TempRunaway_PREHEAT)
-		{
-			temp_runaway_status[_heater_id] = TempRunaway_ACTIVE;
-			temp_runaway_check_active = false;
-		}
-
-		if (!temp_runaway_check_active && _output > 0)
-		{
-			temp_runaway_check_active = true;
-		}
-
-
-		if (temp_runaway_check_active)
-		{			
-			//	we are in range
-			if (_target_temperature - __hysteresis < _current_temperature && _current_temperature < _target_temperature + __hysteresis)
-			{
-				temp_runaway_check_active = false;
-				temp_runaway_error_counter[_heater_id] = 0;
-			}
-			else
-			{
-				if (temp_runaway_status[_heater_id] > TempRunaway_PREHEAT)
-				{
-					temp_runaway_error_counter[_heater_id]++;
-					if (temp_runaway_error_counter[_heater_id] * 2 > __timeout)
-					{
-						if (farm_mode) { prusa_statistics(0); }
-						temp_runaway_stop(false, _isbed);
-						if (farm_mode) { prusa_statistics(90); }
-					}
-				}
-			}
-		}
-
-	}
-}
-
-void temp_runaway_stop(bool isPreheat, bool isBed)
-{
-	cancel_heatup = true;
-	quickStop();
-	if (card.sdprinting)
-	{
-		card.sdprinting = false;
-		card.closefile();
-	}
-	
-	disable_heater();
-	disable_x();
-	disable_y();
-	disable_e0();
-	disable_e1();
-	disable_e2();
-	manage_heater();
-	lcd_update();
-	WRITE(BEEPER, HIGH);
-	delayMicroseconds(500);
-	WRITE(BEEPER, LOW);
-	delayMicroseconds(100);
-
-	if (isPreheat)
-	{
-		Stop();
-		isBed ? LCD_ALERTMESSAGEPGM("BED PREHEAT ERROR") : LCD_ALERTMESSAGEPGM("PREHEAT ERROR");
-		SERIAL_ERROR_START;
-		isBed ? SERIAL_ERRORLNPGM(" THERMAL RUNAWAY ( PREHEAT HEATBED)") : SERIAL_ERRORLNPGM(" THERMAL RUNAWAY ( PREHEAT HOTEND)");
-		SET_OUTPUT(EXTRUDER_0_AUTO_FAN_PIN);
-		SET_OUTPUT(FAN_PIN);
-		WRITE(EXTRUDER_0_AUTO_FAN_PIN, 1);
-		analogWrite(FAN_PIN, 255);
-		fanSpeed = 255;
-		delayMicroseconds(2000);
-	}
-	else
-	{
-		isBed ? LCD_ALERTMESSAGEPGM("BED THERMAL RUNAWAY") : LCD_ALERTMESSAGEPGM("THERMAL RUNAWAY");
-		SERIAL_ERROR_START;
-		isBed ? SERIAL_ERRORLNPGM(" HEATBED THERMAL RUNAWAY") : SERIAL_ERRORLNPGM(" HOTEND THERMAL RUNAWAY");
-	}
-}
-#endif
-
-
-void disable_heater()
-{
-  for(int i=0;i<EXTRUDERS;i++)
-    setTargetHotend(0,i);
-  setTargetBed(0);
-  #if defined(TEMP_0_PIN) && TEMP_0_PIN > -1
-  target_temperature[0]=0;
-  soft_pwm[0]=0;
-   #if defined(HEATER_0_PIN) && HEATER_0_PIN > -1  
-     WRITE(HEATER_0_PIN,LOW);
-   #endif
-  #endif
-     
-  #if defined(TEMP_1_PIN) && TEMP_1_PIN > -1 && EXTRUDERS > 1
-    target_temperature[1]=0;
-    soft_pwm[1]=0;
-    #if defined(HEATER_1_PIN) && HEATER_1_PIN > -1 
-      WRITE(HEATER_1_PIN,LOW);
-    #endif
-  #endif
-      
-  #if defined(TEMP_2_PIN) && TEMP_2_PIN > -1 && EXTRUDERS > 2
-    target_temperature[2]=0;
-    soft_pwm[2]=0;
-    #if defined(HEATER_2_PIN) && HEATER_2_PIN > -1  
-      WRITE(HEATER_2_PIN,LOW);
-    #endif
-  #endif 
-
-  #if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
-    target_temperature_bed=0;
-    soft_pwm_bed=0;
-    #if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1  
-      WRITE(HEATER_BED_PIN,LOW);
-    #endif
-  #endif 
-}
-
-void max_temp_error(uint8_t e) {
-  disable_heater();
-  if(IsStopped() == false) {
-    SERIAL_ERROR_START;
-    SERIAL_ERRORLN((int)e);
-    SERIAL_ERRORLNPGM(": Extruder switched off. MAXTEMP triggered !");
-    LCD_ALERTMESSAGEPGM("Err: MAXTEMP");
-  }
-  #ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE
-  Stop();
-    
-
-    
-  #endif
-    SET_OUTPUT(EXTRUDER_0_AUTO_FAN_PIN);
-    SET_OUTPUT(FAN_PIN);
-    SET_OUTPUT(BEEPER);
-    WRITE(FAN_PIN, 1);
-    WRITE(EXTRUDER_0_AUTO_FAN_PIN, 1);
-    WRITE(BEEPER, 1);
-    // fanSpeed will consumed by the check_axes_activity() routine.
-    fanSpeed=255;
-	if (farm_mode) { prusa_statistics(93); }
-}
-
-void min_temp_error(uint8_t e) {
-  disable_heater();
-  if(IsStopped() == false) {
-    SERIAL_ERROR_START;
-    SERIAL_ERRORLN((int)e);
-    SERIAL_ERRORLNPGM(": Extruder switched off. MINTEMP triggered !");
-    LCD_ALERTMESSAGEPGM("Err: MINTEMP");
-  }
-  #ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE
-  Stop();
-  #endif
-  if (farm_mode) { prusa_statistics(92); }
-
-}
-
-void bed_max_temp_error(void) {
-#if HEATER_BED_PIN > -1
-  WRITE(HEATER_BED_PIN, 0);
-#endif
-  if(IsStopped() == false) {
-    SERIAL_ERROR_START;
-    SERIAL_ERRORLNPGM("Temperature heated bed switched off. MAXTEMP triggered !");
-    LCD_ALERTMESSAGEPGM("Err: MAXTEMP BED");
-  }
-  #ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE
-  Stop();
-  #endif
-
-}
-
-void bed_min_temp_error(void) {
-#if HEATER_BED_PIN > -1
-    WRITE(HEATER_BED_PIN, 0);
-#endif
-    if(IsStopped() == false) {
-        SERIAL_ERROR_START;
-        SERIAL_ERRORLNPGM("Temperature heated bed switched off. MINTEMP triggered !");
-        LCD_ALERTMESSAGEPGM("Err: MINTEMP BED");
-    }
-#ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE
-    Stop();
-#endif
-}
-
-#ifdef HEATER_0_USES_MAX6675
-#define MAX6675_HEAT_INTERVAL 250
-long max6675_previous_millis = MAX6675_HEAT_INTERVAL;
-int max6675_temp = 2000;
-
-int read_max6675()
-{
-  if (millis() - max6675_previous_millis < MAX6675_HEAT_INTERVAL) 
-    return max6675_temp;
-  
-  max6675_previous_millis = millis();
-  max6675_temp = 0;
-    
-  #ifdef	PRR
-    PRR &= ~(1<<PRSPI);
-  #elif defined PRR0
-    PRR0 &= ~(1<<PRSPI);
-  #endif
-  
-  SPCR = (1<<MSTR) | (1<<SPE) | (1<<SPR0);
-  
-  // enable TT_MAX6675
-  WRITE(MAX6675_SS, 0);
-  
-  // ensure 100ns delay - a bit extra is fine
-  asm("nop");//50ns on 20Mhz, 62.5ns on 16Mhz
-  asm("nop");//50ns on 20Mhz, 62.5ns on 16Mhz
-  
-  // read MSB
-  SPDR = 0;
-  for (;(SPSR & (1<<SPIF)) == 0;);
-  max6675_temp = SPDR;
-  max6675_temp <<= 8;
-  
-  // read LSB
-  SPDR = 0;
-  for (;(SPSR & (1<<SPIF)) == 0;);
-  max6675_temp |= SPDR;
-  
-  // disable TT_MAX6675
-  WRITE(MAX6675_SS, 1);
-
-  if (max6675_temp & 4) 
-  {
-    // thermocouple open
-    max6675_temp = 2000;
-  }
-  else 
-  {
-    max6675_temp = max6675_temp >> 3;
-  }
-
-  return max6675_temp;
-}
-#endif
-
-
-// Timer 0 is shared with millies
-ISR(TIMER0_COMPB_vect)
-{
-  //these variables are only accesible from the ISR, but static, so they don't lose their value
-  static unsigned char temp_count = 0;
-  static unsigned long raw_temp_0_value = 0;
-  static unsigned long raw_temp_1_value = 0;
-  static unsigned long raw_temp_2_value = 0;
-  static unsigned long raw_temp_bed_value = 0;
-  static unsigned char temp_state = 10;
-  static unsigned char pwm_count = (1 << SOFT_PWM_SCALE);
-  static unsigned char soft_pwm_0;
-#ifdef SLOW_PWM_HEATERS
-  static unsigned char slow_pwm_count = 0;
-  static unsigned char state_heater_0 = 0;
-  static unsigned char state_timer_heater_0 = 0;
-#endif 
-#if (EXTRUDERS > 1) || defined(HEATERS_PARALLEL)
-  static unsigned char soft_pwm_1;
-#ifdef SLOW_PWM_HEATERS
-  static unsigned char state_heater_1 = 0;
-  static unsigned char state_timer_heater_1 = 0;
-#endif 
-#endif
-#if EXTRUDERS > 2
-  static unsigned char soft_pwm_2;
-#ifdef SLOW_PWM_HEATERS
-  static unsigned char state_heater_2 = 0;
-  static unsigned char state_timer_heater_2 = 0;
-#endif 
-#endif
-#if HEATER_BED_PIN > -1
-  static unsigned char soft_pwm_b;
-#ifdef SLOW_PWM_HEATERS
-  static unsigned char state_heater_b = 0;
-  static unsigned char state_timer_heater_b = 0;
-#endif 
-#endif
-  
-#if defined(FILWIDTH_PIN) &&(FILWIDTH_PIN > -1)
-  static unsigned long raw_filwidth_value = 0;  //added for filament width sensor
-#endif
-  
-#ifndef SLOW_PWM_HEATERS
-  /*
-   * standard PWM modulation
-   */
-  if(pwm_count == 0){
-    soft_pwm_0 = soft_pwm[0];
-    if(soft_pwm_0 > 0) { 
-      WRITE(HEATER_0_PIN,1);
-#ifdef HEATERS_PARALLEL
-      WRITE(HEATER_1_PIN,1);
-#endif
-    } else WRITE(HEATER_0_PIN,0);
-    
-#if EXTRUDERS > 1
-    soft_pwm_1 = soft_pwm[1];
-    if(soft_pwm_1 > 0) WRITE(HEATER_1_PIN,1); else WRITE(HEATER_1_PIN,0);
-#endif
-#if EXTRUDERS > 2
-    soft_pwm_2 = soft_pwm[2];
-    if(soft_pwm_2 > 0) WRITE(HEATER_2_PIN,1); else WRITE(HEATER_2_PIN,0);
-#endif
-#if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1
-    soft_pwm_b = soft_pwm_bed;
-    if(soft_pwm_b > 0) WRITE(HEATER_BED_PIN,1); else WRITE(HEATER_BED_PIN,0);
-#endif
-#ifdef FAN_SOFT_PWM
-    soft_pwm_fan = fanSpeedSoftPwm / 2;
-    if(soft_pwm_fan > 0) WRITE(FAN_PIN,1); else WRITE(FAN_PIN,0);
-#endif
-  }
-  if(soft_pwm_0 < pwm_count) { 
-    WRITE(HEATER_0_PIN,0);
-#ifdef HEATERS_PARALLEL
-    WRITE(HEATER_1_PIN,0);
-#endif
-  }
-#if EXTRUDERS > 1
-  if(soft_pwm_1 < pwm_count) WRITE(HEATER_1_PIN,0);
-#endif
-#if EXTRUDERS > 2
-  if(soft_pwm_2 < pwm_count) WRITE(HEATER_2_PIN,0);
-#endif
-#if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1
-  if(soft_pwm_b < pwm_count) WRITE(HEATER_BED_PIN,0);
-#endif
-#ifdef FAN_SOFT_PWM
-  if(soft_pwm_fan < pwm_count) WRITE(FAN_PIN,0);
-#endif
-  
-  pwm_count += (1 << SOFT_PWM_SCALE);
-  pwm_count &= 0x7f;
-  
-#else //ifndef SLOW_PWM_HEATERS
-  /*
-   * SLOW PWM HEATERS
-   *
-   * for heaters drived by relay
-   */
-#ifndef MIN_STATE_TIME
-#define MIN_STATE_TIME 16 // MIN_STATE_TIME * 65.5 = time in milliseconds
-#endif
-  if (slow_pwm_count == 0) {
-    // EXTRUDER 0 
-    soft_pwm_0 = soft_pwm[0];
-    if (soft_pwm_0 > 0) {
-      // turn ON heather only if the minimum time is up 
-      if (state_timer_heater_0 == 0) { 
-	// if change state set timer 
-	if (state_heater_0 == 0) {
-	  state_timer_heater_0 = MIN_STATE_TIME;
-	}
-	state_heater_0 = 1;
-	WRITE(HEATER_0_PIN, 1);
-#ifdef HEATERS_PARALLEL
-	WRITE(HEATER_1_PIN, 1);
-#endif
-      }
-    } else {
-      // turn OFF heather only if the minimum time is up 
-      if (state_timer_heater_0 == 0) {
-	// if change state set timer 
-	if (state_heater_0 == 1) {
-	  state_timer_heater_0 = MIN_STATE_TIME;
-	}
-	state_heater_0 = 0;
-	WRITE(HEATER_0_PIN, 0);
-#ifdef HEATERS_PARALLEL
-	WRITE(HEATER_1_PIN, 0);
-#endif
-      }
-    }
-    
-#if EXTRUDERS > 1
-    // EXTRUDER 1
-    soft_pwm_1 = soft_pwm[1];
-    if (soft_pwm_1 > 0) {
-      // turn ON heather only if the minimum time is up 
-      if (state_timer_heater_1 == 0) { 
-	// if change state set timer 
-	if (state_heater_1 == 0) {
-	  state_timer_heater_1 = MIN_STATE_TIME;
-	}
-	state_heater_1 = 1;
-	WRITE(HEATER_1_PIN, 1);
-      }
-    } else {
-      // turn OFF heather only if the minimum time is up 
-      if (state_timer_heater_1 == 0) {
-	// if change state set timer 
-	if (state_heater_1 == 1) {
-	  state_timer_heater_1 = MIN_STATE_TIME;
-	}
-	state_heater_1 = 0;
-	WRITE(HEATER_1_PIN, 0);
-      }
-    }
-#endif
-    
-#if EXTRUDERS > 2
-    // EXTRUDER 2
-    soft_pwm_2 = soft_pwm[2];
-    if (soft_pwm_2 > 0) {
-      // turn ON heather only if the minimum time is up 
-      if (state_timer_heater_2 == 0) { 
-	// if change state set timer 
-	if (state_heater_2 == 0) {
-	  state_timer_heater_2 = MIN_STATE_TIME;
-	}
-	state_heater_2 = 1;
-	WRITE(HEATER_2_PIN, 1);
-      }
-    } else {
-      // turn OFF heather only if the minimum time is up 
-      if (state_timer_heater_2 == 0) {
-	// if change state set timer 
-	if (state_heater_2 == 1) {
-	  state_timer_heater_2 = MIN_STATE_TIME;
-	}
-	state_heater_2 = 0;
-	WRITE(HEATER_2_PIN, 0);
-      }
-    }
-#endif
-    
-#if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1
-    // BED
-    soft_pwm_b = soft_pwm_bed;
-    if (soft_pwm_b > 0) {
-      // turn ON heather only if the minimum time is up 
-      if (state_timer_heater_b == 0) { 
-	// if change state set timer 
-	if (state_heater_b == 0) {
-	  state_timer_heater_b = MIN_STATE_TIME;
-	}
-	state_heater_b = 1;
-	WRITE(HEATER_BED_PIN, 1);
-      }
-    } else {
-      // turn OFF heather only if the minimum time is up 
-      if (state_timer_heater_b == 0) {
-	// if change state set timer 
-	if (state_heater_b == 1) {
-	  state_timer_heater_b = MIN_STATE_TIME;
-	}
-	state_heater_b = 0;
-	WRITE(HEATER_BED_PIN, 0);
-      }
-    }
-#endif
-  } // if (slow_pwm_count == 0)
-  
-  // EXTRUDER 0 
-  if (soft_pwm_0 < slow_pwm_count) {
-    // turn OFF heather only if the minimum time is up 
-    if (state_timer_heater_0 == 0) { 
-      // if change state set timer 
-      if (state_heater_0 == 1) {
-	state_timer_heater_0 = MIN_STATE_TIME;
-      }
-      state_heater_0 = 0;
-      WRITE(HEATER_0_PIN, 0);
-#ifdef HEATERS_PARALLEL
-      WRITE(HEATER_1_PIN, 0);
-#endif
-    }
-  }
-    
-#if EXTRUDERS > 1
-  // EXTRUDER 1 
-  if (soft_pwm_1 < slow_pwm_count) {
-    // turn OFF heather only if the minimum time is up 
-    if (state_timer_heater_1 == 0) { 
-      // if change state set timer 
-      if (state_heater_1 == 1) {
-	state_timer_heater_1 = MIN_STATE_TIME;
-      }
-      state_heater_1 = 0;
-      WRITE(HEATER_1_PIN, 0);
-    }
-  }
-#endif
-  
-#if EXTRUDERS > 2
-  // EXTRUDER 2
-  if (soft_pwm_2 < slow_pwm_count) {
-    // turn OFF heather only if the minimum time is up 
-    if (state_timer_heater_2 == 0) { 
-      // if change state set timer 
-      if (state_heater_2 == 1) {
-	state_timer_heater_2 = MIN_STATE_TIME;
-      }
-      state_heater_2 = 0;
-      WRITE(HEATER_2_PIN, 0);
-    }
-  }
-#endif
-  
-#if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1
-  // BED
-  if (soft_pwm_b < slow_pwm_count) {
-    // turn OFF heather only if the minimum time is up 
-    if (state_timer_heater_b == 0) { 
-      // if change state set timer 
-      if (state_heater_b == 1) {
-	state_timer_heater_b = MIN_STATE_TIME;
-      }
-      state_heater_b = 0;
-      WRITE(HEATER_BED_PIN, 0);
-    }
-  }
-#endif
-  
-#ifdef FAN_SOFT_PWM
-  if (pwm_count == 0){
-    soft_pwm_fan = fanSpeedSoftPwm / 2;
-    if (soft_pwm_fan > 0) WRITE(FAN_PIN,1); else WRITE(FAN_PIN,0);
-  }
-  if (soft_pwm_fan < pwm_count) WRITE(FAN_PIN,0);
-#endif
-  
-  pwm_count += (1 << SOFT_PWM_SCALE);
-  pwm_count &= 0x7f;
-  
-  // increment slow_pwm_count only every 64 pwm_count circa 65.5ms
-  if ((pwm_count % 64) == 0) {
-    slow_pwm_count++;
-    slow_pwm_count &= 0x7f;
-    
-    // Extruder 0
-    if (state_timer_heater_0 > 0) {
-      state_timer_heater_0--;
-    } 
-  
-#if EXTRUDERS > 1
-    // Extruder 1
-    if (state_timer_heater_1 > 0) 
-      state_timer_heater_1--;
-#endif
-    
-#if EXTRUDERS > 2
-    // Extruder 2
-    if (state_timer_heater_2 > 0) 
-      state_timer_heater_2--;
-#endif
-    
-#if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1
-    // Bed   
-    if (state_timer_heater_b > 0) 
-      state_timer_heater_b--;
-#endif
-  } //if ((pwm_count % 64) == 0) {
-  
-#endif //ifndef SLOW_PWM_HEATERS
-  
-  switch(temp_state) {
-    case 0: // Prepare TEMP_0
-      #if defined(TEMP_0_PIN) && (TEMP_0_PIN > -1)
-        #if TEMP_0_PIN > 7
-          ADCSRB = 1<<MUX5;
-        #else
-          ADCSRB = 0;
-        #endif
-        ADMUX = ((1 << REFS0) | (TEMP_0_PIN & 0x07));
-        ADCSRA |= 1<<ADSC; // Start conversion
-      #endif
-      lcd_buttons_update();
-      temp_state = 1;
-      break;
-    case 1: // Measure TEMP_0
-      #if defined(TEMP_0_PIN) && (TEMP_0_PIN > -1)
-        raw_temp_0_value += ADC;
-      #endif
-      #ifdef HEATER_0_USES_MAX6675 // TODO remove the blocking
-        raw_temp_0_value = read_max6675();
-      #endif
-      temp_state = 2;
-      break;
-    case 2: // Prepare TEMP_BED
-      #if defined(TEMP_BED_PIN) && (TEMP_BED_PIN > -1)
-        #if TEMP_BED_PIN > 7
-          ADCSRB = 1<<MUX5;
-        #else
-          ADCSRB = 0;
-        #endif
-        ADMUX = ((1 << REFS0) | (TEMP_BED_PIN & 0x07));
-        ADCSRA |= 1<<ADSC; // Start conversion
-      #endif
-      lcd_buttons_update();
-      temp_state = 3;
-      break;
-    case 3: // Measure TEMP_BED
-      #if defined(TEMP_BED_PIN) && (TEMP_BED_PIN > -1)
-        raw_temp_bed_value += ADC;
-      #endif
-      temp_state = 4;
-      break;
-    case 4: // Prepare TEMP_1
-      #if defined(TEMP_1_PIN) && (TEMP_1_PIN > -1)
-        #if TEMP_1_PIN > 7
-          ADCSRB = 1<<MUX5;
-        #else
-          ADCSRB = 0;
-        #endif
-        ADMUX = ((1 << REFS0) | (TEMP_1_PIN & 0x07));
-        ADCSRA |= 1<<ADSC; // Start conversion
-      #endif
-      lcd_buttons_update();
-      temp_state = 5;
-      break;
-    case 5: // Measure TEMP_1
-      #if defined(TEMP_1_PIN) && (TEMP_1_PIN > -1)
-        raw_temp_1_value += ADC;
-      #endif
-      temp_state = 6;
-      break;
-    case 6: // Prepare TEMP_2
-      #if defined(TEMP_2_PIN) && (TEMP_2_PIN > -1)
-        #if TEMP_2_PIN > 7
-          ADCSRB = 1<<MUX5;
-        #else
-          ADCSRB = 0;
-        #endif
-        ADMUX = ((1 << REFS0) | (TEMP_2_PIN & 0x07));
-        ADCSRA |= 1<<ADSC; // Start conversion
-      #endif
-      lcd_buttons_update();
-      temp_state = 7;
-      break;
-    case 7: // Measure TEMP_2
-      #if defined(TEMP_2_PIN) && (TEMP_2_PIN > -1)
-        raw_temp_2_value += ADC;
-      #endif
-      temp_state = 8;//change so that Filament Width is also measured
-      
-      break;
-    case 8: //Prepare FILWIDTH 
-     #if defined(FILWIDTH_PIN) && (FILWIDTH_PIN> -1) 
-      #if FILWIDTH_PIN>7 
-         ADCSRB = 1<<MUX5;
-      #else
-         ADCSRB = 0; 
-      #endif 
-      ADMUX = ((1 << REFS0) | (FILWIDTH_PIN & 0x07)); 
-      ADCSRA |= 1<<ADSC; // Start conversion 
-     #endif 
-     lcd_buttons_update();       
-     temp_state = 9; 
-     break; 
-    case 9:   //Measure FILWIDTH 
-     #if defined(FILWIDTH_PIN) &&(FILWIDTH_PIN > -1) 
-     //raw_filwidth_value += ADC;  //remove to use an IIR filter approach 
-      if(ADC>102)  //check that ADC is reading a voltage > 0.5 volts, otherwise don't take in the data.
-        {
-    	raw_filwidth_value= raw_filwidth_value-(raw_filwidth_value>>7);  //multipliy raw_filwidth_value by 127/128
-        
-        raw_filwidth_value= raw_filwidth_value + ((unsigned long)ADC<<7);  //add new ADC reading 
-        }
-     #endif 
-     temp_state = 0;   
-      
-     temp_count++;
-     break;      
-      
-      
-    case 10: //Startup, delay initial temp reading a tiny bit so the hardware can settle.
-      temp_state = 0;
-      break;
-//    default:
-//      SERIAL_ERROR_START;
-//      SERIAL_ERRORLNPGM("Temp measurement error!");
-//      break;
-  }
-    
-  if(temp_count >= OVERSAMPLENR) // 10 * 16 * 1/(16000000/64/256)  = 164ms.
-  {
-    if (!temp_meas_ready) //Only update the raw values if they have been read. Else we could be updating them during reading.
-    {
-      current_temperature_raw[0] = raw_temp_0_value;
-#if EXTRUDERS > 1
-      current_temperature_raw[1] = raw_temp_1_value;
-#endif
-#ifdef TEMP_SENSOR_1_AS_REDUNDANT
-      redundant_temperature_raw = raw_temp_1_value;
-#endif
-#if EXTRUDERS > 2
-      current_temperature_raw[2] = raw_temp_2_value;
-#endif
-      current_temperature_bed_raw = raw_temp_bed_value;
-    }
-
-//Add similar code for Filament Sensor - can be read any time since IIR filtering is used 
-#if defined(FILWIDTH_PIN) &&(FILWIDTH_PIN > -1)
-  current_raw_filwidth = raw_filwidth_value>>10;  //need to divide to get to 0-16384 range since we used 1/128 IIR filter approach 
-#endif
-    
-    
-    temp_meas_ready = true;
-    temp_count = 0;
-    raw_temp_0_value = 0;
-    raw_temp_1_value = 0;
-    raw_temp_2_value = 0;
-    raw_temp_bed_value = 0;
-
-#if HEATER_0_RAW_LO_TEMP > HEATER_0_RAW_HI_TEMP
-    if(current_temperature_raw[0] <= maxttemp_raw[0]) {
-#else
-    if(current_temperature_raw[0] >= maxttemp_raw[0]) {
-#endif
-        max_temp_error(0);
-    }
-#if HEATER_0_RAW_LO_TEMP > HEATER_0_RAW_HI_TEMP
-    if(current_temperature_raw[0] >= minttemp_raw[0]) {
-#else
-    if(current_temperature_raw[0] <= minttemp_raw[0]) {
-#endif
-        min_temp_error(0);
-    }
-#if EXTRUDERS > 1
-#if HEATER_1_RAW_LO_TEMP > HEATER_1_RAW_HI_TEMP
-    if(current_temperature_raw[1] <= maxttemp_raw[1]) {
-#else
-    if(current_temperature_raw[1] >= maxttemp_raw[1]) {
-#endif
-        max_temp_error(1);
-    }
-#if HEATER_1_RAW_LO_TEMP > HEATER_1_RAW_HI_TEMP
-    if(current_temperature_raw[1] >= minttemp_raw[1]) {
-#else
-    if(current_temperature_raw[1] <= minttemp_raw[1]) {
-#endif
-        min_temp_error(1);
-    }
-#endif
-#if EXTRUDERS > 2
-#if HEATER_2_RAW_LO_TEMP > HEATER_2_RAW_HI_TEMP
-    if(current_temperature_raw[2] <= maxttemp_raw[2]) {
-#else
-    if(current_temperature_raw[2] >= maxttemp_raw[2]) {
-#endif
-        max_temp_error(2);
-    }
-#if HEATER_2_RAW_LO_TEMP > HEATER_2_RAW_HI_TEMP
-    if(current_temperature_raw[2] >= minttemp_raw[2]) {
-#else
-    if(current_temperature_raw[2] <= minttemp_raw[2]) {
-#endif
-        min_temp_error(2);
-    }
-#endif
-  
-  /* No bed MINTEMP error? */
-        
-        
-#if defined(BED_MAXTEMP) && (TEMP_SENSOR_BED != 0)
-# if HEATER_BED_RAW_LO_TEMP > HEATER_BED_RAW_HI_TEMP
-    if(current_temperature_bed_raw <= bed_maxttemp_raw) {
-#else
-    if(current_temperature_bed_raw >= bed_maxttemp_raw) {
-#endif
-       target_temperature_bed = 0;
-       bed_max_temp_error();
-    }
-  }
-        
-# if HEATER_BED_RAW_LO_TEMP > HEATER_BED_RAW_HI_TEMP
-        if(current_temperature_bed_raw >= bed_minttemp_raw) {
-#else
-            if(current_temperature_bed_raw <= bed_minttemp_raw) {
-#endif
-                bed_min_temp_error();
-            }
-            
-#endif
-  
-#ifdef BABYSTEPPING
-  for(uint8_t axis=0;axis<3;axis++)
-  {
-    int curTodo=babystepsTodo[axis]; //get rid of volatile for performance
-   
-    if(curTodo>0)
-    {
-      babystep(axis,/*fwd*/true);
-      babystepsTodo[axis]--; //less to do next time
-    }
-    else
-    if(curTodo<0)
-    {
-      babystep(axis,/*fwd*/false);
-      babystepsTodo[axis]++; //less to do next time
-    }
-  }
-#endif //BABYSTEPPING
-}
-
-#ifdef PIDTEMP
-// Apply the scale factors to the PID values
-
-
-float scalePID_i(float i)
-{
-	return i*PID_dT;
-}
-
-float unscalePID_i(float i)
-{
-	return i/PID_dT;
-}
-
-float scalePID_d(float d)
-{
-    return d/PID_dT;
-}
-
-float unscalePID_d(float d)
-{
-	return d*PID_dT;
-}
-
-#endif //PIDTEMP
-
-
+/*
+  temperature.c - temperature control
+  Part of Marlin
+  
+ Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ 
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+ 
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+ 
+ You should have received a copy of the GNU General Public License
+ along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ This firmware is a mashup between Sprinter and grbl.
+  (https://github.com/kliment/Sprinter)
+  (https://github.com/simen/grbl/tree)
+ 
+ It has preliminary support for Matthew Roberts advance algorithm 
+    http://reprap.org/pipermail/reprap-dev/2011-May/003323.html
+
+ */
+
+
+#include "Marlin.h"
+#include "ultralcd.h"
+#include "temperature.h"
+#include "watchdog.h"
+#include "cardreader.h"
+
+#include "Sd2PinMap.h"
+
+
+//===========================================================================
+//=============================public variables============================
+//===========================================================================
+int target_temperature[EXTRUDERS] = { 0 };
+int target_temperature_bed = 0;
+int current_temperature_raw[EXTRUDERS] = { 0 };
+float current_temperature[EXTRUDERS] = { 0.0 };
+int current_temperature_bed_raw = 0;
+float current_temperature_bed = 0.0;
+#ifdef TEMP_SENSOR_1_AS_REDUNDANT
+  int redundant_temperature_raw = 0;
+  float redundant_temperature = 0.0;
+#endif
+  
+
+#ifdef PIDTEMP
+  float _Kp, _Ki, _Kd;
+  int pid_cycle, pid_number_of_cycles;
+  bool pid_tuning_finished = false;
+  float Kp=DEFAULT_Kp;
+  float Ki=(DEFAULT_Ki*PID_dT);
+  float Kd=(DEFAULT_Kd/PID_dT);
+  #ifdef PID_ADD_EXTRUSION_RATE
+    float Kc=DEFAULT_Kc;
+  #endif
+#endif //PIDTEMP
+
+#ifdef PIDTEMPBED
+  float bedKp=DEFAULT_bedKp;
+  float bedKi=(DEFAULT_bedKi*PID_dT);
+  float bedKd=(DEFAULT_bedKd/PID_dT);
+#endif //PIDTEMPBED
+  
+#ifdef FAN_SOFT_PWM
+  unsigned char fanSpeedSoftPwm;
+#endif
+
+unsigned char soft_pwm_bed;
+  
+#ifdef BABYSTEPPING
+  volatile int babystepsTodo[3]={0,0,0};
+#endif
+
+#ifdef FILAMENT_SENSOR
+  int current_raw_filwidth = 0;  //Holds measured filament diameter - one extruder only
+#endif  
+//===========================================================================
+//=============================private variables============================
+//===========================================================================
+static volatile bool temp_meas_ready = false;
+
+#ifdef PIDTEMP
+  //static cannot be external:
+  static float temp_iState[EXTRUDERS] = { 0 };
+  static float temp_dState[EXTRUDERS] = { 0 };
+  static float pTerm[EXTRUDERS];
+  static float iTerm[EXTRUDERS];
+  static float dTerm[EXTRUDERS];
+  //int output;
+  static float pid_error[EXTRUDERS];
+  static float temp_iState_min[EXTRUDERS];
+  static float temp_iState_max[EXTRUDERS];
+  // static float pid_input[EXTRUDERS];
+  // static float pid_output[EXTRUDERS];
+  static bool pid_reset[EXTRUDERS];
+#endif //PIDTEMP
+#ifdef PIDTEMPBED
+  //static cannot be external:
+  static float temp_iState_bed = { 0 };
+  static float temp_dState_bed = { 0 };
+  static float pTerm_bed;
+  static float iTerm_bed;
+  static float dTerm_bed;
+  //int output;
+  static float pid_error_bed;
+  static float temp_iState_min_bed;
+  static float temp_iState_max_bed;
+#else //PIDTEMPBED
+	static unsigned long  previous_millis_bed_heater;
+#endif //PIDTEMPBED
+  static unsigned char soft_pwm[EXTRUDERS];
+
+#ifdef FAN_SOFT_PWM
+  static unsigned char soft_pwm_fan;
+#endif
+#if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1) || \
+    (defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1) || \
+    (defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1)
+  static unsigned long extruder_autofan_last_check;
+#endif  
+
+#if EXTRUDERS > 3
+  # error Unsupported number of extruders
+#elif EXTRUDERS > 2
+  # define ARRAY_BY_EXTRUDERS(v1, v2, v3) { v1, v2, v3 }
+#elif EXTRUDERS > 1
+  # define ARRAY_BY_EXTRUDERS(v1, v2, v3) { v1, v2 }
+#else
+  # define ARRAY_BY_EXTRUDERS(v1, v2, v3) { v1 }
+#endif
+
+// Init min and max temp with extreme values to prevent false errors during startup
+static int minttemp_raw[EXTRUDERS] = ARRAY_BY_EXTRUDERS( HEATER_0_RAW_LO_TEMP , HEATER_1_RAW_LO_TEMP , HEATER_2_RAW_LO_TEMP );
+static int maxttemp_raw[EXTRUDERS] = ARRAY_BY_EXTRUDERS( HEATER_0_RAW_HI_TEMP , HEATER_1_RAW_HI_TEMP , HEATER_2_RAW_HI_TEMP );
+static int minttemp[EXTRUDERS] = ARRAY_BY_EXTRUDERS( 0, 0, 0 );
+static int maxttemp[EXTRUDERS] = ARRAY_BY_EXTRUDERS( 16383, 16383, 16383 );
+#ifdef BED_MINTEMP
+static int bed_minttemp_raw = HEATER_BED_RAW_LO_TEMP;
+#endif
+#ifdef BED_MAXTEMP
+static int bed_maxttemp_raw = HEATER_BED_RAW_HI_TEMP;
+#endif
+
+#ifdef TEMP_SENSOR_1_AS_REDUNDANT
+  static void *heater_ttbl_map[2] = {(void *)HEATER_0_TEMPTABLE, (void *)HEATER_1_TEMPTABLE };
+  static uint8_t heater_ttbllen_map[2] = { HEATER_0_TEMPTABLE_LEN, HEATER_1_TEMPTABLE_LEN };
+#else
+  static void *heater_ttbl_map[EXTRUDERS] = ARRAY_BY_EXTRUDERS( (void *)HEATER_0_TEMPTABLE, (void *)HEATER_1_TEMPTABLE, (void *)HEATER_2_TEMPTABLE );
+  static uint8_t heater_ttbllen_map[EXTRUDERS] = ARRAY_BY_EXTRUDERS( HEATER_0_TEMPTABLE_LEN, HEATER_1_TEMPTABLE_LEN, HEATER_2_TEMPTABLE_LEN );
+#endif
+
+static float analog2temp(int raw, uint8_t e);
+static float analog2tempBed(int raw);
+static void updateTemperaturesFromRawValues();
+
+enum TempRunawayStates
+{
+	TempRunaway_INACTIVE = 0,
+	TempRunaway_PREHEAT = 1,
+	TempRunaway_ACTIVE = 2,
+};
+
+#ifdef WATCH_TEMP_PERIOD
+int watch_start_temp[EXTRUDERS] = ARRAY_BY_EXTRUDERS(0,0,0);
+unsigned long watchmillis[EXTRUDERS] = ARRAY_BY_EXTRUDERS(0,0,0);
+#endif //WATCH_TEMP_PERIOD
+
+#ifndef SOFT_PWM_SCALE
+#define SOFT_PWM_SCALE 0
+#endif
+
+#ifdef FILAMENT_SENSOR
+  static int meas_shift_index;  //used to point to a delayed sample in buffer for filament width sensor
+#endif
+//===========================================================================
+//=============================   functions      ============================
+//===========================================================================
+
+  void PID_autotune(float temp, int extruder, int ncycles)
+  {
+  pid_number_of_cycles = ncycles;
+  pid_tuning_finished = false;
+  float input = 0.0;
+  pid_cycle=0;
+  bool heating = true;
+
+  unsigned long temp_millis = millis();
+  unsigned long t1=temp_millis;
+  unsigned long t2=temp_millis;
+  long t_high = 0;
+  long t_low = 0;
+
+  long bias, d;
+  float Ku, Tu;
+  float max = 0, min = 10000;
+
+#if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1) || \
+    (defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1) || \
+    (defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1)
+  unsigned long extruder_autofan_last_check = millis();
+#endif
+
+  if ((extruder >= EXTRUDERS)
+  #if (TEMP_BED_PIN <= -1)
+       ||(extruder < 0)
+  #endif
+       ){
+          SERIAL_ECHOLN("PID Autotune failed. Bad extruder number.");
+		  pid_tuning_finished = true;
+		  pid_cycle = 0;
+          return;
+        }
+	
+  SERIAL_ECHOLN("PID Autotune start");
+  
+  disable_heater(); // switch off all heaters.
+
+  if (extruder<0)
+  {
+     soft_pwm_bed = (MAX_BED_POWER)/2;
+     bias = d = (MAX_BED_POWER)/2;
+   }
+   else
+   {
+     soft_pwm[extruder] = (PID_MAX)/2;
+     bias = d = (PID_MAX)/2;
+  }
+
+
+
+
+ for(;;) {
+
+    if(temp_meas_ready == true) { // temp sample ready
+      updateTemperaturesFromRawValues();
+
+      input = (extruder<0)?current_temperature_bed:current_temperature[extruder];
+
+      max=max(max,input);
+      min=min(min,input);
+
+      #if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1) || \
+          (defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1) || \
+          (defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1)
+      if(millis() - extruder_autofan_last_check > 2500) {
+        checkExtruderAutoFans();
+        extruder_autofan_last_check = millis();
+      }
+      #endif
+
+      if(heating == true && input > temp) {
+        if(millis() - t2 > 5000) { 
+          heating=false;
+          if (extruder<0)
+            soft_pwm_bed = (bias - d) >> 1;
+          else
+            soft_pwm[extruder] = (bias - d) >> 1;
+          t1=millis();
+          t_high=t1 - t2;
+          max=temp;
+        }
+      }
+      if(heating == false && input < temp) {
+        if(millis() - t1 > 5000) {
+          heating=true;
+          t2=millis();
+          t_low=t2 - t1;
+          if(pid_cycle > 0) {
+            bias += (d*(t_high - t_low))/(t_low + t_high);
+            bias = constrain(bias, 20 ,(extruder<0?(MAX_BED_POWER):(PID_MAX))-20);
+            if(bias > (extruder<0?(MAX_BED_POWER):(PID_MAX))/2) d = (extruder<0?(MAX_BED_POWER):(PID_MAX)) - 1 - bias;
+            else d = bias;
+
+            SERIAL_PROTOCOLPGM(" bias: "); SERIAL_PROTOCOL(bias);
+            SERIAL_PROTOCOLPGM(" d: "); SERIAL_PROTOCOL(d);
+            SERIAL_PROTOCOLPGM(" min: "); SERIAL_PROTOCOL(min);
+            SERIAL_PROTOCOLPGM(" max: "); SERIAL_PROTOCOLLN(max);
+            if(pid_cycle > 2) {
+              Ku = (4.0*d)/(3.14159*(max-min)/2.0);
+              Tu = ((float)(t_low + t_high)/1000.0);
+              SERIAL_PROTOCOLPGM(" Ku: "); SERIAL_PROTOCOL(Ku);
+              SERIAL_PROTOCOLPGM(" Tu: "); SERIAL_PROTOCOLLN(Tu);
+              _Kp = 0.6*Ku;
+              _Ki = 2*_Kp/Tu;
+              _Kd = _Kp*Tu/8;
+              SERIAL_PROTOCOLLNPGM(" Classic PID ");
+              SERIAL_PROTOCOLPGM(" Kp: "); SERIAL_PROTOCOLLN(_Kp);
+              SERIAL_PROTOCOLPGM(" Ki: "); SERIAL_PROTOCOLLN(_Ki);
+              SERIAL_PROTOCOLPGM(" Kd: "); SERIAL_PROTOCOLLN(_Kd);
+              /*
+              _Kp = 0.33*Ku;
+              _Ki = _Kp/Tu;
+              _Kd = _Kp*Tu/3;
+              SERIAL_PROTOCOLLNPGM(" Some overshoot ");
+              SERIAL_PROTOCOLPGM(" Kp: "); SERIAL_PROTOCOLLN(_Kp);
+              SERIAL_PROTOCOLPGM(" Ki: "); SERIAL_PROTOCOLLN(_Ki);
+              SERIAL_PROTOCOLPGM(" Kd: "); SERIAL_PROTOCOLLN(_Kd);
+              _Kp = 0.2*Ku;
+              _Ki = 2*_Kp/Tu;
+              _Kd = _Kp*Tu/3;
+              SERIAL_PROTOCOLLNPGM(" No overshoot ");
+              SERIAL_PROTOCOLPGM(" Kp: "); SERIAL_PROTOCOLLN(_Kp);
+              SERIAL_PROTOCOLPGM(" Ki: "); SERIAL_PROTOCOLLN(_Ki);
+              SERIAL_PROTOCOLPGM(" Kd: "); SERIAL_PROTOCOLLN(_Kd);
+              */
+            }
+          }
+          if (extruder<0)
+            soft_pwm_bed = (bias + d) >> 1;
+          else
+            soft_pwm[extruder] = (bias + d) >> 1;
+          pid_cycle++;
+          min=temp;
+        }
+      } 
+    }
+    if(input > (temp + 20)) {
+      SERIAL_PROTOCOLLNPGM("PID Autotune failed! Temperature too high");
+	  pid_tuning_finished = true;
+	  pid_cycle = 0;
+      return;
+    }
+    if(millis() - temp_millis > 2000) {
+      int p;
+      if (extruder<0){
+        p=soft_pwm_bed;       
+        SERIAL_PROTOCOLPGM("ok B:");
+      }else{
+        p=soft_pwm[extruder];       
+        SERIAL_PROTOCOLPGM("ok T:");
+      }
+			
+      SERIAL_PROTOCOL(input);   
+      SERIAL_PROTOCOLPGM(" @:");
+      SERIAL_PROTOCOLLN(p);       
+
+      temp_millis = millis();
+    }
+    if(((millis() - t1) + (millis() - t2)) > (10L*60L*1000L*2L)) {
+      SERIAL_PROTOCOLLNPGM("PID Autotune failed! timeout");
+	  pid_tuning_finished = true;
+	  pid_cycle = 0;
+      return;
+    }
+    if(pid_cycle > ncycles) {
+      SERIAL_PROTOCOLLNPGM("PID Autotune finished! Put the last Kp, Ki and Kd constants from above into Configuration.h");
+	  pid_tuning_finished = true;
+	  pid_cycle = 0;
+      return;
+    }
+    lcd_update();
+  }
+}
+
+void updatePID()
+{
+#ifdef PIDTEMP
+  for(int e = 0; e < EXTRUDERS; e++) { 
+     temp_iState_max[e] = PID_INTEGRAL_DRIVE_MAX / Ki;  
+  }
+#endif
+#ifdef PIDTEMPBED
+  temp_iState_max_bed = PID_INTEGRAL_DRIVE_MAX / bedKi;  
+#endif
+}
+  
+int getHeaterPower(int heater) {
+	if (heater<0)
+		return soft_pwm_bed;
+  return soft_pwm[heater];
+}
+
+#if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1) || \
+    (defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1) || \
+    (defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1)
+
+  #if defined(FAN_PIN) && FAN_PIN > -1
+    #if EXTRUDER_0_AUTO_FAN_PIN == FAN_PIN 
+       #error "You cannot set EXTRUDER_0_AUTO_FAN_PIN equal to FAN_PIN"
+    #endif
+    #if EXTRUDER_1_AUTO_FAN_PIN == FAN_PIN 
+       #error "You cannot set EXTRUDER_1_AUTO_FAN_PIN equal to FAN_PIN"
+    #endif
+    #if EXTRUDER_2_AUTO_FAN_PIN == FAN_PIN 
+       #error "You cannot set EXTRUDER_2_AUTO_FAN_PIN equal to FAN_PIN"
+    #endif
+  #endif 
+
+void setExtruderAutoFanState(int pin, bool state)
+{
+  unsigned char newFanSpeed = (state != 0) ? EXTRUDER_AUTO_FAN_SPEED : 0;
+  // this idiom allows both digital and PWM fan outputs (see M42 handling).
+  pinMode(pin, OUTPUT);
+  digitalWrite(pin, newFanSpeed);
+  analogWrite(pin, newFanSpeed);
+}
+
+void checkExtruderAutoFans()
+{
+  uint8_t fanState = 0;
+
+  // which fan pins need to be turned on?      
+  #if defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1
+    if (current_temperature[0] > EXTRUDER_AUTO_FAN_TEMPERATURE) 
+      fanState |= 1;
+  #endif
+  #if defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1
+    if (current_temperature[1] > EXTRUDER_AUTO_FAN_TEMPERATURE) 
+    {
+      if (EXTRUDER_1_AUTO_FAN_PIN == EXTRUDER_0_AUTO_FAN_PIN) 
+        fanState |= 1;
+      else
+        fanState |= 2;
+    }
+  #endif
+  #if defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1
+    if (current_temperature[2] > EXTRUDER_AUTO_FAN_TEMPERATURE) 
+    {
+      if (EXTRUDER_2_AUTO_FAN_PIN == EXTRUDER_0_AUTO_FAN_PIN) 
+        fanState |= 1;
+      else if (EXTRUDER_2_AUTO_FAN_PIN == EXTRUDER_1_AUTO_FAN_PIN) 
+        fanState |= 2;
+      else
+        fanState |= 4;
+    }
+  #endif
+  
+  // update extruder auto fan states
+  #if defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1
+    setExtruderAutoFanState(EXTRUDER_0_AUTO_FAN_PIN, (fanState & 1) != 0);
+  #endif 
+  #if defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1
+    if (EXTRUDER_1_AUTO_FAN_PIN != EXTRUDER_0_AUTO_FAN_PIN) 
+      setExtruderAutoFanState(EXTRUDER_1_AUTO_FAN_PIN, (fanState & 2) != 0);
+  #endif 
+  #if defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1
+    if (EXTRUDER_2_AUTO_FAN_PIN != EXTRUDER_0_AUTO_FAN_PIN 
+        && EXTRUDER_2_AUTO_FAN_PIN != EXTRUDER_1_AUTO_FAN_PIN)
+      setExtruderAutoFanState(EXTRUDER_2_AUTO_FAN_PIN, (fanState & 4) != 0);
+  #endif 
+}
+
+#endif // any extruder auto fan pins set
+
+void manage_heater()
+{
+  float pid_input;
+  float pid_output;
+
+  if(temp_meas_ready != true)   //better readability
+    return; 
+
+  updateTemperaturesFromRawValues();
+
+#ifdef TEMP_RUNAWAY_BED_HYSTERESIS
+  temp_runaway_check(0, target_temperature_bed, current_temperature_bed, (int)soft_pwm_bed, true);
+#endif
+
+  for(int e = 0; e < EXTRUDERS; e++) 
+  {
+
+#ifdef TEMP_RUNAWAY_EXTRUDER_HYSTERESIS
+	  temp_runaway_check(e+1, target_temperature[e], current_temperature[e], (int)soft_pwm[e], false);
+#endif
+
+  #ifdef PIDTEMP
+    pid_input = current_temperature[e];
+
+    #ifndef PID_OPENLOOP
+        pid_error[e] = target_temperature[e] - pid_input;
+        if(pid_error[e] > PID_FUNCTIONAL_RANGE) {
+          pid_output = BANG_MAX;
+          pid_reset[e] = true;
+        }
+        else if(pid_error[e] < -PID_FUNCTIONAL_RANGE || target_temperature[e] == 0) {
+          pid_output = 0;
+          pid_reset[e] = true;
+        }
+        else {
+          if(pid_reset[e] == true) {
+            temp_iState[e] = 0.0;
+            pid_reset[e] = false;
+          }
+          pTerm[e] = Kp * pid_error[e];
+          temp_iState[e] += pid_error[e];
+          temp_iState[e] = constrain(temp_iState[e], temp_iState_min[e], temp_iState_max[e]);
+          iTerm[e] = Ki * temp_iState[e];
+
+          //K1 defined in Configuration.h in the PID settings
+          #define K2 (1.0-K1)
+          dTerm[e] = (Kd * (pid_input - temp_dState[e]))*K2 + (K1 * dTerm[e]);
+          pid_output = pTerm[e] + iTerm[e] - dTerm[e];
+          if (pid_output > PID_MAX) {
+            if (pid_error[e] > 0 )  temp_iState[e] -= pid_error[e]; // conditional un-integration
+            pid_output=PID_MAX;
+          } else if (pid_output < 0){
+            if (pid_error[e] < 0 )  temp_iState[e] -= pid_error[e]; // conditional un-integration
+            pid_output=0;
+          }
+        }
+        temp_dState[e] = pid_input;
+    #else 
+          pid_output = constrain(target_temperature[e], 0, PID_MAX);
+    #endif //PID_OPENLOOP
+    #ifdef PID_DEBUG
+    SERIAL_ECHO_START;
+    SERIAL_ECHO(" PID_DEBUG ");
+    SERIAL_ECHO(e);
+    SERIAL_ECHO(": Input ");
+    SERIAL_ECHO(pid_input);
+    SERIAL_ECHO(" Output ");
+    SERIAL_ECHO(pid_output);
+    SERIAL_ECHO(" pTerm ");
+    SERIAL_ECHO(pTerm[e]);
+    SERIAL_ECHO(" iTerm ");
+    SERIAL_ECHO(iTerm[e]);
+    SERIAL_ECHO(" dTerm ");
+    SERIAL_ECHOLN(dTerm[e]);
+    #endif //PID_DEBUG
+  #else /* PID off */
+    pid_output = 0;
+    if(current_temperature[e] < target_temperature[e]) {
+      pid_output = PID_MAX;
+    }
+  #endif
+
+    // Check if temperature is within the correct range
+    if((current_temperature[e] > minttemp[e]) && (current_temperature[e] < maxttemp[e])) 
+    {
+      soft_pwm[e] = (int)pid_output >> 1;
+    }
+    else {
+      soft_pwm[e] = 0;
+    }
+
+    #ifdef WATCH_TEMP_PERIOD
+    if(watchmillis[e] && millis() - watchmillis[e] > WATCH_TEMP_PERIOD)
+    {
+        if(degHotend(e) < watch_start_temp[e] + WATCH_TEMP_INCREASE)
+        {
+            setTargetHotend(0, e);
+            LCD_MESSAGEPGM("Heating failed");
+            SERIAL_ECHO_START;
+            SERIAL_ECHOLN("Heating failed");
+        }else{
+            watchmillis[e] = 0;
+        }
+    }
+    #endif
+    #ifdef TEMP_SENSOR_1_AS_REDUNDANT
+      if(fabs(current_temperature[0] - redundant_temperature) > MAX_REDUNDANT_TEMP_SENSOR_DIFF) {
+        disable_heater();
+        if(IsStopped() == false) {
+          SERIAL_ERROR_START;
+          SERIAL_ERRORLNPGM("Extruder switched off. Temperature difference between temp sensors is too high !");
+          LCD_ALERTMESSAGEPGM("Err: REDUNDANT TEMP ERROR");
+        }
+        #ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE
+          Stop();
+        #endif
+      }
+    #endif
+  } // End extruder for loop
+
+  #if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1) || \
+      (defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1) || \
+      (defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1)
+  if(millis() - extruder_autofan_last_check > 2500)  // only need to check fan state very infrequently
+  {
+    checkExtruderAutoFans();
+    extruder_autofan_last_check = millis();
+  }  
+  #endif       
+  
+  #ifndef PIDTEMPBED
+  if(millis() - previous_millis_bed_heater < BED_CHECK_INTERVAL)
+    return;
+  previous_millis_bed_heater = millis();
+  #endif
+
+  #if TEMP_SENSOR_BED != 0
+
+  #ifdef PIDTEMPBED
+    pid_input = current_temperature_bed;
+
+    #ifndef PID_OPENLOOP
+		  pid_error_bed = target_temperature_bed - pid_input;
+		  pTerm_bed = bedKp * pid_error_bed;
+		  temp_iState_bed += pid_error_bed;
+		  temp_iState_bed = constrain(temp_iState_bed, temp_iState_min_bed, temp_iState_max_bed);
+		  iTerm_bed = bedKi * temp_iState_bed;
+
+		  //K1 defined in Configuration.h in the PID settings
+		  #define K2 (1.0-K1)
+		  dTerm_bed= (bedKd * (pid_input - temp_dState_bed))*K2 + (K1 * dTerm_bed);
+		  temp_dState_bed = pid_input;
+
+		  pid_output = pTerm_bed + iTerm_bed - dTerm_bed;
+          	  if (pid_output > MAX_BED_POWER) {
+            	    if (pid_error_bed > 0 )  temp_iState_bed -= pid_error_bed; // conditional un-integration
+                    pid_output=MAX_BED_POWER;
+          	  } else if (pid_output < 0){
+            	    if (pid_error_bed < 0 )  temp_iState_bed -= pid_error_bed; // conditional un-integration
+                    pid_output=0;
+                  }
+
+    #else 
+      pid_output = constrain(target_temperature_bed, 0, MAX_BED_POWER);
+    #endif //PID_OPENLOOP
+
+	  if((current_temperature_bed > BED_MINTEMP) && (current_temperature_bed < BED_MAXTEMP)) 
+	  {
+	    soft_pwm_bed = (int)pid_output >> 1;
+	  }
+	  else {
+	    soft_pwm_bed = 0;
+	  }
+
+    #elif !defined(BED_LIMIT_SWITCHING)
+      // Check if temperature is within the correct range
+      if((current_temperature_bed > BED_MINTEMP) && (current_temperature_bed < BED_MAXTEMP))
+      {
+        if(current_temperature_bed >= target_temperature_bed)
+        {
+          soft_pwm_bed = 0;
+        }
+        else 
+        {
+          soft_pwm_bed = MAX_BED_POWER>>1;
+        }
+      }
+      else
+      {
+        soft_pwm_bed = 0;
+        WRITE(HEATER_BED_PIN,LOW);
+      }
+    #else //#ifdef BED_LIMIT_SWITCHING
+      // Check if temperature is within the correct band
+      if((current_temperature_bed > BED_MINTEMP) && (current_temperature_bed < BED_MAXTEMP))
+      {
+        if(current_temperature_bed > target_temperature_bed + BED_HYSTERESIS)
+        {
+          soft_pwm_bed = 0;
+        }
+        else if(current_temperature_bed <= target_temperature_bed - BED_HYSTERESIS)
+        {
+          soft_pwm_bed = MAX_BED_POWER>>1;
+        }
+      }
+      else
+      {
+        soft_pwm_bed = 0;
+        WRITE(HEATER_BED_PIN,LOW);
+      }
+    #endif
+  #endif
+  
+//code for controlling the extruder rate based on the width sensor 
+#ifdef FILAMENT_SENSOR
+  if(filament_sensor) 
+	{
+	meas_shift_index=delay_index1-meas_delay_cm;
+		  if(meas_shift_index<0)
+			  meas_shift_index = meas_shift_index + (MAX_MEASUREMENT_DELAY+1);  //loop around buffer if needed
+		  
+		  //get the delayed info and add 100 to reconstitute to a percent of the nominal filament diameter
+		  //then square it to get an area
+		  
+		  if(meas_shift_index<0)
+			  meas_shift_index=0;
+		  else if (meas_shift_index>MAX_MEASUREMENT_DELAY)
+			  meas_shift_index=MAX_MEASUREMENT_DELAY;
+		  
+		     volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] = pow((float)(100+measurement_delay[meas_shift_index])/100.0,2);
+		     if (volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] <0.01)
+		    	 volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM]=0.01;
+	}
+#endif
+}
+
+#define PGM_RD_W(x)   (short)pgm_read_word(&x)
+// Derived from RepRap FiveD extruder::getTemperature()
+// For hot end temperature measurement.
+static float analog2temp(int raw, uint8_t e) {
+#ifdef TEMP_SENSOR_1_AS_REDUNDANT
+  if(e > EXTRUDERS)
+#else
+  if(e >= EXTRUDERS)
+#endif
+  {
+      SERIAL_ERROR_START;
+      SERIAL_ERROR((int)e);
+      SERIAL_ERRORLNPGM(" - Invalid extruder number !");
+      kill();
+      return 0.0;
+  } 
+  #ifdef HEATER_0_USES_MAX6675
+    if (e == 0)
+    {
+      return 0.25 * raw;
+    }
+  #endif
+
+  if(heater_ttbl_map[e] != NULL)
+  {
+    float celsius = 0;
+    uint8_t i;
+    short (*tt)[][2] = (short (*)[][2])(heater_ttbl_map[e]);
+
+    for (i=1; i<heater_ttbllen_map[e]; i++)
+    {
+      if (PGM_RD_W((*tt)[i][0]) > raw)
+      {
+        celsius = PGM_RD_W((*tt)[i-1][1]) + 
+          (raw - PGM_RD_W((*tt)[i-1][0])) * 
+          (float)(PGM_RD_W((*tt)[i][1]) - PGM_RD_W((*tt)[i-1][1])) /
+          (float)(PGM_RD_W((*tt)[i][0]) - PGM_RD_W((*tt)[i-1][0]));
+        break;
+      }
+    }
+
+    // Overflow: Set to last value in the table
+    if (i == heater_ttbllen_map[e]) celsius = PGM_RD_W((*tt)[i-1][1]);
+
+    return celsius;
+  }
+  return ((raw * ((5.0 * 100.0) / 1024.0) / OVERSAMPLENR) * TEMP_SENSOR_AD595_GAIN) + TEMP_SENSOR_AD595_OFFSET;
+}
+
+// Derived from RepRap FiveD extruder::getTemperature()
+// For bed temperature measurement.
+static float analog2tempBed(int raw) {
+  #ifdef BED_USES_THERMISTOR
+    float celsius = 0;
+    byte i;
+
+    for (i=1; i<BEDTEMPTABLE_LEN; i++)
+    {
+      if (PGM_RD_W(BEDTEMPTABLE[i][0]) > raw)
+      {
+        celsius  = PGM_RD_W(BEDTEMPTABLE[i-1][1]) + 
+          (raw - PGM_RD_W(BEDTEMPTABLE[i-1][0])) * 
+          (float)(PGM_RD_W(BEDTEMPTABLE[i][1]) - PGM_RD_W(BEDTEMPTABLE[i-1][1])) /
+          (float)(PGM_RD_W(BEDTEMPTABLE[i][0]) - PGM_RD_W(BEDTEMPTABLE[i-1][0]));
+        break;
+      }
+    }
+
+    // Overflow: Set to last value in the table
+    if (i == BEDTEMPTABLE_LEN) celsius = PGM_RD_W(BEDTEMPTABLE[i-1][1]);
+
+
+	// temperature offset adjustment
+#ifdef BED_OFFSET
+	float _offset = BED_OFFSET;
+	float _offset_center = BED_OFFSET_CENTER;
+	float _offset_start = BED_OFFSET_START;
+	float _first_koef = (_offset / 2) / (_offset_center - _offset_start);
+	float _second_koef = (_offset / 2) / (100 - _offset_center);
+
+
+	if (celsius >= _offset_start && celsius <= _offset_center)
+	{
+		celsius = celsius + (_first_koef * (celsius - _offset_start));
+	}
+	else if (celsius > _offset_center && celsius <= 100)
+	{
+		celsius = celsius + (_first_koef * (_offset_center - _offset_start)) + ( _second_koef * ( celsius - ( 100 - _offset_center ) )) ;
+	}
+	else if (celsius > 100)
+	{
+		celsius = celsius + _offset;
+	}
+#endif
+
+
+    return celsius;
+  #elif defined BED_USES_AD595
+    return ((raw * ((5.0 * 100.0) / 1024.0) / OVERSAMPLENR) * TEMP_SENSOR_AD595_GAIN) + TEMP_SENSOR_AD595_OFFSET;
+  #else
+    return 0;
+  #endif
+}
+
+/* Called to get the raw values into the the actual temperatures. The raw values are created in interrupt context,
+    and this function is called from normal context as it is too slow to run in interrupts and will block the stepper routine otherwise */
+static void updateTemperaturesFromRawValues()
+{
+    for(uint8_t e=0;e<EXTRUDERS;e++)
+    {
+        current_temperature[e] = analog2temp(current_temperature_raw[e], e);
+    }
+    
+	current_temperature_bed = analog2tempBed(current_temperature_bed_raw);
+
+    #ifdef TEMP_SENSOR_1_AS_REDUNDANT
+      redundant_temperature = analog2temp(redundant_temperature_raw, 1);
+    #endif
+    #if defined (FILAMENT_SENSOR) && (FILWIDTH_PIN > -1)    //check if a sensor is supported 
+      filament_width_meas = analog2widthFil();
+    #endif  
+    //Reset the watchdog after we know we have a temperature measurement.
+    watchdog_reset();
+
+    CRITICAL_SECTION_START;
+    temp_meas_ready = false;
+    CRITICAL_SECTION_END;
+}
+
+
+// For converting raw Filament Width to milimeters 
+#ifdef FILAMENT_SENSOR
+float analog2widthFil() { 
+return current_raw_filwidth/16383.0*5.0; 
+//return current_raw_filwidth; 
+} 
+ 
+// For converting raw Filament Width to a ratio 
+int widthFil_to_size_ratio() { 
+ 
+float temp; 
+      
+temp=filament_width_meas;
+if(filament_width_meas<MEASURED_LOWER_LIMIT)
+	temp=filament_width_nominal;  //assume sensor cut out
+else if (filament_width_meas>MEASURED_UPPER_LIMIT)
+	temp= MEASURED_UPPER_LIMIT;
+
+
+return(filament_width_nominal/temp*100); 
+
+
+} 
+#endif
+
+
+
+
+
+void tp_init()
+{
+#if MB(RUMBA) && ((TEMP_SENSOR_0==-1)||(TEMP_SENSOR_1==-1)||(TEMP_SENSOR_2==-1)||(TEMP_SENSOR_BED==-1))
+  //disable RUMBA JTAG in case the thermocouple extension is plugged on top of JTAG connector
+  MCUCR=(1<<JTD); 
+  MCUCR=(1<<JTD);
+#endif
+  
+  // Finish init of mult extruder arrays 
+  for(int e = 0; e < EXTRUDERS; e++) {
+    // populate with the first value 
+    maxttemp[e] = maxttemp[0];
+#ifdef PIDTEMP
+    temp_iState_min[e] = 0.0;
+    temp_iState_max[e] = PID_INTEGRAL_DRIVE_MAX / Ki;
+#endif //PIDTEMP
+#ifdef PIDTEMPBED
+    temp_iState_min_bed = 0.0;
+    temp_iState_max_bed = PID_INTEGRAL_DRIVE_MAX / bedKi;
+#endif //PIDTEMPBED
+  }
+
+  #if defined(HEATER_0_PIN) && (HEATER_0_PIN > -1) 
+    SET_OUTPUT(HEATER_0_PIN);
+  #endif  
+  #if defined(HEATER_1_PIN) && (HEATER_1_PIN > -1) 
+    SET_OUTPUT(HEATER_1_PIN);
+  #endif  
+  #if defined(HEATER_2_PIN) && (HEATER_2_PIN > -1) 
+    SET_OUTPUT(HEATER_2_PIN);
+  #endif  
+  #if defined(HEATER_BED_PIN) && (HEATER_BED_PIN > -1) 
+    SET_OUTPUT(HEATER_BED_PIN);
+  #endif  
+  #if defined(FAN_PIN) && (FAN_PIN > -1) 
+    SET_OUTPUT(FAN_PIN);
+    #ifdef FAST_PWM_FAN
+    setPwmFrequency(FAN_PIN, 1); // No prescaling. Pwm frequency = F_CPU/256/8
+    #endif
+    #ifdef FAN_SOFT_PWM
+    soft_pwm_fan = fanSpeedSoftPwm / 2;
+    #endif
+  #endif  
+
+  #ifdef HEATER_0_USES_MAX6675
+    #ifndef SDSUPPORT
+      SET_OUTPUT(SCK_PIN);
+      WRITE(SCK_PIN,0);
+    
+      SET_OUTPUT(MOSI_PIN);
+      WRITE(MOSI_PIN,1);
+    
+      SET_INPUT(MISO_PIN);
+      WRITE(MISO_PIN,1);
+    #endif
+    /* Using pinMode and digitalWrite, as that was the only way I could get it to compile */
+    
+    //Have to toggle SD card CS pin to low first, to enable firmware to talk with SD card
+	pinMode(SS_PIN, OUTPUT);
+	digitalWrite(SS_PIN,0);  
+	pinMode(MAX6675_SS, OUTPUT);
+	digitalWrite(MAX6675_SS,1);
+  #endif
+
+  // Set analog inputs
+  ADCSRA = 1<<ADEN | 1<<ADSC | 1<<ADIF | 0x07;
+  DIDR0 = 0;
+  #ifdef DIDR2
+    DIDR2 = 0;
+  #endif
+  #if defined(TEMP_0_PIN) && (TEMP_0_PIN > -1)
+    #if TEMP_0_PIN < 8
+       DIDR0 |= 1 << TEMP_0_PIN; 
+    #else
+       DIDR2 |= 1<<(TEMP_0_PIN - 8); 
+    #endif
+  #endif
+  #if defined(TEMP_1_PIN) && (TEMP_1_PIN > -1)
+    #if TEMP_1_PIN < 8
+       DIDR0 |= 1<<TEMP_1_PIN; 
+    #else
+       DIDR2 |= 1<<(TEMP_1_PIN - 8); 
+    #endif
+  #endif
+  #if defined(TEMP_2_PIN) && (TEMP_2_PIN > -1)
+    #if TEMP_2_PIN < 8
+       DIDR0 |= 1 << TEMP_2_PIN; 
+    #else
+       DIDR2 |= 1<<(TEMP_2_PIN - 8); 
+    #endif
+  #endif
+  #if defined(TEMP_BED_PIN) && (TEMP_BED_PIN > -1)
+    #if TEMP_BED_PIN < 8
+       DIDR0 |= 1<<TEMP_BED_PIN; 
+    #else
+       DIDR2 |= 1<<(TEMP_BED_PIN - 8); 
+    #endif
+  #endif
+  
+  //Added for Filament Sensor 
+  #ifdef FILAMENT_SENSOR
+   #if defined(FILWIDTH_PIN) && (FILWIDTH_PIN > -1) 
+	#if FILWIDTH_PIN < 8 
+       	   DIDR0 |= 1<<FILWIDTH_PIN;  
+	#else 
+       	   DIDR2 |= 1<<(FILWIDTH_PIN - 8);  
+	#endif 
+   #endif
+  #endif
+  
+  // Use timer0 for temperature measurement
+  // Interleave temperature interrupt with millies interrupt
+  OCR0B = 128;
+  TIMSK0 |= (1<<OCIE0B);  
+  
+  // Wait for temperature measurement to settle
+  delay(250);
+
+#ifdef HEATER_0_MINTEMP
+  minttemp[0] = HEATER_0_MINTEMP;
+  while(analog2temp(minttemp_raw[0], 0) < HEATER_0_MINTEMP) {
+#if HEATER_0_RAW_LO_TEMP < HEATER_0_RAW_HI_TEMP
+    minttemp_raw[0] += OVERSAMPLENR;
+#else
+    minttemp_raw[0] -= OVERSAMPLENR;
+#endif
+  }
+#endif //MINTEMP
+#ifdef HEATER_0_MAXTEMP
+  maxttemp[0] = HEATER_0_MAXTEMP;
+  while(analog2temp(maxttemp_raw[0], 0) > HEATER_0_MAXTEMP) {
+#if HEATER_0_RAW_LO_TEMP < HEATER_0_RAW_HI_TEMP
+    maxttemp_raw[0] -= OVERSAMPLENR;
+#else
+    maxttemp_raw[0] += OVERSAMPLENR;
+#endif
+  }
+#endif //MAXTEMP
+
+#if (EXTRUDERS > 1) && defined(HEATER_1_MINTEMP)
+  minttemp[1] = HEATER_1_MINTEMP;
+  while(analog2temp(minttemp_raw[1], 1) < HEATER_1_MINTEMP) {
+#if HEATER_1_RAW_LO_TEMP < HEATER_1_RAW_HI_TEMP
+    minttemp_raw[1] += OVERSAMPLENR;
+#else
+    minttemp_raw[1] -= OVERSAMPLENR;
+#endif
+  }
+#endif // MINTEMP 1
+#if (EXTRUDERS > 1) && defined(HEATER_1_MAXTEMP)
+  maxttemp[1] = HEATER_1_MAXTEMP;
+  while(analog2temp(maxttemp_raw[1], 1) > HEATER_1_MAXTEMP) {
+#if HEATER_1_RAW_LO_TEMP < HEATER_1_RAW_HI_TEMP
+    maxttemp_raw[1] -= OVERSAMPLENR;
+#else
+    maxttemp_raw[1] += OVERSAMPLENR;
+#endif
+  }
+#endif //MAXTEMP 1
+
+#if (EXTRUDERS > 2) && defined(HEATER_2_MINTEMP)
+  minttemp[2] = HEATER_2_MINTEMP;
+  while(analog2temp(minttemp_raw[2], 2) < HEATER_2_MINTEMP) {
+#if HEATER_2_RAW_LO_TEMP < HEATER_2_RAW_HI_TEMP
+    minttemp_raw[2] += OVERSAMPLENR;
+#else
+    minttemp_raw[2] -= OVERSAMPLENR;
+#endif
+  }
+#endif //MINTEMP 2
+#if (EXTRUDERS > 2) && defined(HEATER_2_MAXTEMP)
+  maxttemp[2] = HEATER_2_MAXTEMP;
+  while(analog2temp(maxttemp_raw[2], 2) > HEATER_2_MAXTEMP) {
+#if HEATER_2_RAW_LO_TEMP < HEATER_2_RAW_HI_TEMP
+    maxttemp_raw[2] -= OVERSAMPLENR;
+#else
+    maxttemp_raw[2] += OVERSAMPLENR;
+#endif
+  }
+#endif //MAXTEMP 2
+
+#ifdef BED_MINTEMP
+  /* No bed MINTEMP error implemented?!? */
+  while(analog2tempBed(bed_minttemp_raw) < BED_MINTEMP) {
+#if HEATER_BED_RAW_LO_TEMP < HEATER_BED_RAW_HI_TEMP
+    bed_minttemp_raw += OVERSAMPLENR;
+#else
+    bed_minttemp_raw -= OVERSAMPLENR;
+#endif
+  }
+  
+#endif //BED_MINTEMP
+#ifdef BED_MAXTEMP
+  while(analog2tempBed(bed_maxttemp_raw) > BED_MAXTEMP) {
+#if HEATER_BED_RAW_LO_TEMP < HEATER_BED_RAW_HI_TEMP
+    bed_maxttemp_raw -= OVERSAMPLENR;
+#else
+    bed_maxttemp_raw += OVERSAMPLENR;
+#endif
+  }
+#endif //BED_MAXTEMP
+}
+
+void setWatch() 
+{  
+#ifdef WATCH_TEMP_PERIOD
+  for (int e = 0; e < EXTRUDERS; e++)
+  {
+    if(degHotend(e) < degTargetHotend(e) - (WATCH_TEMP_INCREASE * 2))
+    {
+      watch_start_temp[e] = degHotend(e);
+      watchmillis[e] = millis();
+    } 
+  }
+#endif 
+}
+
+#if (defined (TEMP_RUNAWAY_BED_HYSTERESIS) && TEMP_RUNAWAY_BED_TIMEOUT > 0) || (defined (TEMP_RUNAWAY_EXTRUDER_HYSTERESIS) && TEMP_RUNAWAY_EXTRUDER_TIMEOUT > 0)
+void temp_runaway_check(int _heater_id, float _target_temperature, float _current_temperature, float _output, bool _isbed)
+{
+	float __hysteresis = 0;
+	int __timeout = 0;
+	bool temp_runaway_check_active = false;
+	static float __preheat_start[2] = { 0,0}; //currently just bed and one extruder
+	static int __preheat_counter[2] = { 0,0};
+	static int __preheat_errors[2] = { 0,0};
+		
+
+#ifdef 	TEMP_RUNAWAY_BED_TIMEOUT
+	if (_isbed)
+	{
+		__hysteresis = TEMP_RUNAWAY_BED_HYSTERESIS;
+		__timeout = TEMP_RUNAWAY_BED_TIMEOUT;
+	}
+#endif
+#ifdef 	TEMP_RUNAWAY_EXTRUDER_TIMEOUT
+	if (!_isbed)
+	{
+		__hysteresis = TEMP_RUNAWAY_EXTRUDER_HYSTERESIS;
+		__timeout = TEMP_RUNAWAY_EXTRUDER_TIMEOUT;
+	}
+#endif
+
+	if (millis() - temp_runaway_timer[_heater_id] > 2000)
+	{
+
+		temp_runaway_timer[_heater_id] = millis();
+		if (_output == 0)
+		{
+			temp_runaway_check_active = false;
+			temp_runaway_error_counter[_heater_id] = 0;
+		}
+
+		if (temp_runaway_target[_heater_id] != _target_temperature)
+		{
+			if (_target_temperature > 0)
+			{
+				temp_runaway_status[_heater_id] = TempRunaway_PREHEAT;
+				temp_runaway_target[_heater_id] = _target_temperature;
+				__preheat_start[_heater_id] = _current_temperature;
+				__preheat_counter[_heater_id] = 0;
+			}
+			else
+			{
+				temp_runaway_status[_heater_id] = TempRunaway_INACTIVE;
+				temp_runaway_target[_heater_id] = _target_temperature;
+			}
+		}
+
+		if (temp_runaway_status[_heater_id] == TempRunaway_PREHEAT)
+		{
+			if (_current_temperature < ((_isbed) ? (0.8 * _target_temperature) : 150)) //check only in area where temperature is changing fastly for heater, check to 0.8 x target temperature for bed
+			{
+				__preheat_counter[_heater_id]++;
+				if (__preheat_counter[_heater_id] > ((_isbed) ? 16 : 8)) // periodicaly check if current temperature changes
+				{
+					/*SERIAL_ECHOPGM("Heater:");
+					MYSERIAL.print(_heater_id);
+					SERIAL_ECHOPGM(" T:");
+					MYSERIAL.print(_current_temperature);
+					SERIAL_ECHOPGM(" Tstart:");
+					MYSERIAL.print(__preheat_start[_heater_id]);*/
+					
+					if (_current_temperature - __preheat_start[_heater_id] < 2) {
+						__preheat_errors[_heater_id]++;
+						/*SERIAL_ECHOPGM(" Preheat errors:");
+						MYSERIAL.println(__preheat_errors[_heater_id]);*/
+					}
+					else {
+						//SERIAL_ECHOLNPGM("");
+						__preheat_errors[_heater_id] = 0;
+					}
+
+					if (__preheat_errors[_heater_id] > ((_isbed) ? 2 : 5)) 
+					{
+						if (farm_mode) { prusa_statistics(0); }
+						temp_runaway_stop(true, _isbed);
+						if (farm_mode) { prusa_statistics(91); }
+					}
+					__preheat_start[_heater_id] = _current_temperature;
+					__preheat_counter[_heater_id] = 0;
+				}
+			}
+		}
+
+		if (_current_temperature >= _target_temperature  && temp_runaway_status[_heater_id] == TempRunaway_PREHEAT)
+		{
+			temp_runaway_status[_heater_id] = TempRunaway_ACTIVE;
+			temp_runaway_check_active = false;
+		}
+
+		if (!temp_runaway_check_active && _output > 0)
+		{
+			temp_runaway_check_active = true;
+		}
+
+
+		if (temp_runaway_check_active)
+		{			
+			//	we are in range
+			if (_target_temperature - __hysteresis < _current_temperature && _current_temperature < _target_temperature + __hysteresis)
+			{
+				temp_runaway_check_active = false;
+				temp_runaway_error_counter[_heater_id] = 0;
+			}
+			else
+			{
+				if (temp_runaway_status[_heater_id] > TempRunaway_PREHEAT)
+				{
+					temp_runaway_error_counter[_heater_id]++;
+					if (temp_runaway_error_counter[_heater_id] * 2 > __timeout)
+					{
+						if (farm_mode) { prusa_statistics(0); }
+						temp_runaway_stop(false, _isbed);
+						if (farm_mode) { prusa_statistics(90); }
+					}
+				}
+			}
+		}
+
+	}
+}
+
+void temp_runaway_stop(bool isPreheat, bool isBed)
+{
+	cancel_heatup = true;
+	quickStop();
+	if (card.sdprinting)
+	{
+		card.sdprinting = false;
+		card.closefile();
+	}
+	
+	disable_heater();
+	disable_x();
+	disable_y();
+	disable_e0();
+	disable_e1();
+	disable_e2();
+	manage_heater();
+	lcd_update();
+	WRITE(BEEPER, HIGH);
+	delayMicroseconds(500);
+	WRITE(BEEPER, LOW);
+	delayMicroseconds(100);
+
+	if (isPreheat)
+	{
+		Stop();
+		isBed ? LCD_ALERTMESSAGEPGM("BED PREHEAT ERROR") : LCD_ALERTMESSAGEPGM("PREHEAT ERROR");
+		SERIAL_ERROR_START;
+		isBed ? SERIAL_ERRORLNPGM(" THERMAL RUNAWAY ( PREHEAT HEATBED)") : SERIAL_ERRORLNPGM(" THERMAL RUNAWAY ( PREHEAT HOTEND)");
+		SET_OUTPUT(EXTRUDER_0_AUTO_FAN_PIN);
+		SET_OUTPUT(FAN_PIN);
+		WRITE(EXTRUDER_0_AUTO_FAN_PIN, 1);
+		analogWrite(FAN_PIN, 255);
+		fanSpeed = 255;
+		delayMicroseconds(2000);
+	}
+	else
+	{
+		isBed ? LCD_ALERTMESSAGEPGM("BED THERMAL RUNAWAY") : LCD_ALERTMESSAGEPGM("THERMAL RUNAWAY");
+		SERIAL_ERROR_START;
+		isBed ? SERIAL_ERRORLNPGM(" HEATBED THERMAL RUNAWAY") : SERIAL_ERRORLNPGM(" HOTEND THERMAL RUNAWAY");
+	}
+}
+#endif
+
+
+void disable_heater()
+{
+  for(int i=0;i<EXTRUDERS;i++)
+    setTargetHotend(0,i);
+  setTargetBed(0);
+  #if defined(TEMP_0_PIN) && TEMP_0_PIN > -1
+  target_temperature[0]=0;
+  soft_pwm[0]=0;
+   #if defined(HEATER_0_PIN) && HEATER_0_PIN > -1  
+     WRITE(HEATER_0_PIN,LOW);
+   #endif
+  #endif
+     
+  #if defined(TEMP_1_PIN) && TEMP_1_PIN > -1 && EXTRUDERS > 1
+    target_temperature[1]=0;
+    soft_pwm[1]=0;
+    #if defined(HEATER_1_PIN) && HEATER_1_PIN > -1 
+      WRITE(HEATER_1_PIN,LOW);
+    #endif
+  #endif
+      
+  #if defined(TEMP_2_PIN) && TEMP_2_PIN > -1 && EXTRUDERS > 2
+    target_temperature[2]=0;
+    soft_pwm[2]=0;
+    #if defined(HEATER_2_PIN) && HEATER_2_PIN > -1  
+      WRITE(HEATER_2_PIN,LOW);
+    #endif
+  #endif 
+
+  #if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
+    target_temperature_bed=0;
+    soft_pwm_bed=0;
+    #if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1  
+      WRITE(HEATER_BED_PIN,LOW);
+    #endif
+  #endif 
+}
+
+void max_temp_error(uint8_t e) {
+  disable_heater();
+  if(IsStopped() == false) {
+    SERIAL_ERROR_START;
+    SERIAL_ERRORLN((int)e);
+    SERIAL_ERRORLNPGM(": Extruder switched off. MAXTEMP triggered !");
+    LCD_ALERTMESSAGEPGM("Err: MAXTEMP");
+  }
+  #ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE
+  Stop();
+    
+
+    
+  #endif
+    SET_OUTPUT(EXTRUDER_0_AUTO_FAN_PIN);
+    SET_OUTPUT(FAN_PIN);
+    SET_OUTPUT(BEEPER);
+    WRITE(FAN_PIN, 1);
+    WRITE(EXTRUDER_0_AUTO_FAN_PIN, 1);
+    WRITE(BEEPER, 1);
+    // fanSpeed will consumed by the check_axes_activity() routine.
+    fanSpeed=255;
+	if (farm_mode) { prusa_statistics(93); }
+}
+
+void min_temp_error(uint8_t e) {
+  disable_heater();
+  if(IsStopped() == false) {
+    SERIAL_ERROR_START;
+    SERIAL_ERRORLN((int)e);
+    SERIAL_ERRORLNPGM(": Extruder switched off. MINTEMP triggered !");
+    LCD_ALERTMESSAGEPGM("Err: MINTEMP");
+  }
+  #ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE
+  Stop();
+  #endif
+  if (farm_mode) { prusa_statistics(92); }
+
+}
+
+void bed_max_temp_error(void) {
+#if HEATER_BED_PIN > -1
+  WRITE(HEATER_BED_PIN, 0);
+#endif
+  if(IsStopped() == false) {
+    SERIAL_ERROR_START;
+    SERIAL_ERRORLNPGM("Temperature heated bed switched off. MAXTEMP triggered !");
+    LCD_ALERTMESSAGEPGM("Err: MAXTEMP BED");
+  }
+  #ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE
+  Stop();
+  #endif
+
+}
+
+void bed_min_temp_error(void) {
+#if HEATER_BED_PIN > -1
+    WRITE(HEATER_BED_PIN, 0);
+#endif
+    if(IsStopped() == false) {
+        SERIAL_ERROR_START;
+        SERIAL_ERRORLNPGM("Temperature heated bed switched off. MINTEMP triggered !");
+        LCD_ALERTMESSAGEPGM("Err: MINTEMP BED");
+    }
+#ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE
+    Stop();
+#endif
+}
+
+#ifdef HEATER_0_USES_MAX6675
+#define MAX6675_HEAT_INTERVAL 250
+long max6675_previous_millis = MAX6675_HEAT_INTERVAL;
+int max6675_temp = 2000;
+
+int read_max6675()
+{
+  if (millis() - max6675_previous_millis < MAX6675_HEAT_INTERVAL) 
+    return max6675_temp;
+  
+  max6675_previous_millis = millis();
+  max6675_temp = 0;
+    
+  #ifdef	PRR
+    PRR &= ~(1<<PRSPI);
+  #elif defined PRR0
+    PRR0 &= ~(1<<PRSPI);
+  #endif
+  
+  SPCR = (1<<MSTR) | (1<<SPE) | (1<<SPR0);
+  
+  // enable TT_MAX6675
+  WRITE(MAX6675_SS, 0);
+  
+  // ensure 100ns delay - a bit extra is fine
+  asm("nop");//50ns on 20Mhz, 62.5ns on 16Mhz
+  asm("nop");//50ns on 20Mhz, 62.5ns on 16Mhz
+  
+  // read MSB
+  SPDR = 0;
+  for (;(SPSR & (1<<SPIF)) == 0;);
+  max6675_temp = SPDR;
+  max6675_temp <<= 8;
+  
+  // read LSB
+  SPDR = 0;
+  for (;(SPSR & (1<<SPIF)) == 0;);
+  max6675_temp |= SPDR;
+  
+  // disable TT_MAX6675
+  WRITE(MAX6675_SS, 1);
+
+  if (max6675_temp & 4) 
+  {
+    // thermocouple open
+    max6675_temp = 2000;
+  }
+  else 
+  {
+    max6675_temp = max6675_temp >> 3;
+  }
+
+  return max6675_temp;
+}
+#endif
+
+
+// Timer 0 is shared with millies
+ISR(TIMER0_COMPB_vect)
+{
+  //these variables are only accesible from the ISR, but static, so they don't lose their value
+  static unsigned char temp_count = 0;
+  static unsigned long raw_temp_0_value = 0;
+  static unsigned long raw_temp_1_value = 0;
+  static unsigned long raw_temp_2_value = 0;
+  static unsigned long raw_temp_bed_value = 0;
+  static unsigned char temp_state = 10;
+  static unsigned char pwm_count = (1 << SOFT_PWM_SCALE);
+  static unsigned char soft_pwm_0;
+#ifdef SLOW_PWM_HEATERS
+  static unsigned char slow_pwm_count = 0;
+  static unsigned char state_heater_0 = 0;
+  static unsigned char state_timer_heater_0 = 0;
+#endif 
+#if (EXTRUDERS > 1) || defined(HEATERS_PARALLEL)
+  static unsigned char soft_pwm_1;
+#ifdef SLOW_PWM_HEATERS
+  static unsigned char state_heater_1 = 0;
+  static unsigned char state_timer_heater_1 = 0;
+#endif 
+#endif
+#if EXTRUDERS > 2
+  static unsigned char soft_pwm_2;
+#ifdef SLOW_PWM_HEATERS
+  static unsigned char state_heater_2 = 0;
+  static unsigned char state_timer_heater_2 = 0;
+#endif 
+#endif
+#if HEATER_BED_PIN > -1
+  static unsigned char soft_pwm_b;
+#ifdef SLOW_PWM_HEATERS
+  static unsigned char state_heater_b = 0;
+  static unsigned char state_timer_heater_b = 0;
+#endif 
+#endif
+  
+#if defined(FILWIDTH_PIN) &&(FILWIDTH_PIN > -1)
+  static unsigned long raw_filwidth_value = 0;  //added for filament width sensor
+#endif
+  
+#ifndef SLOW_PWM_HEATERS
+  /*
+   * standard PWM modulation
+   */
+  if(pwm_count == 0){
+    soft_pwm_0 = soft_pwm[0];
+    if(soft_pwm_0 > 0) { 
+      WRITE(HEATER_0_PIN,1);
+#ifdef HEATERS_PARALLEL
+      WRITE(HEATER_1_PIN,1);
+#endif
+    } else WRITE(HEATER_0_PIN,0);
+    
+#if EXTRUDERS > 1
+    soft_pwm_1 = soft_pwm[1];
+    if(soft_pwm_1 > 0) WRITE(HEATER_1_PIN,1); else WRITE(HEATER_1_PIN,0);
+#endif
+#if EXTRUDERS > 2
+    soft_pwm_2 = soft_pwm[2];
+    if(soft_pwm_2 > 0) WRITE(HEATER_2_PIN,1); else WRITE(HEATER_2_PIN,0);
+#endif
+#if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1
+    soft_pwm_b = soft_pwm_bed;
+    if(soft_pwm_b > 0) WRITE(HEATER_BED_PIN,1); else WRITE(HEATER_BED_PIN,0);
+#endif
+#ifdef FAN_SOFT_PWM
+    soft_pwm_fan = fanSpeedSoftPwm / 2;
+    if(soft_pwm_fan > 0) WRITE(FAN_PIN,1); else WRITE(FAN_PIN,0);
+#endif
+  }
+  if(soft_pwm_0 < pwm_count) { 
+    WRITE(HEATER_0_PIN,0);
+#ifdef HEATERS_PARALLEL
+    WRITE(HEATER_1_PIN,0);
+#endif
+  }
+#if EXTRUDERS > 1
+  if(soft_pwm_1 < pwm_count) WRITE(HEATER_1_PIN,0);
+#endif
+#if EXTRUDERS > 2
+  if(soft_pwm_2 < pwm_count) WRITE(HEATER_2_PIN,0);
+#endif
+#if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1
+  if(soft_pwm_b < pwm_count) WRITE(HEATER_BED_PIN,0);
+#endif
+#ifdef FAN_SOFT_PWM
+  if(soft_pwm_fan < pwm_count) WRITE(FAN_PIN,0);
+#endif
+  
+  pwm_count += (1 << SOFT_PWM_SCALE);
+  pwm_count &= 0x7f;
+  
+#else //ifndef SLOW_PWM_HEATERS
+  /*
+   * SLOW PWM HEATERS
+   *
+   * for heaters drived by relay
+   */
+#ifndef MIN_STATE_TIME
+#define MIN_STATE_TIME 16 // MIN_STATE_TIME * 65.5 = time in milliseconds
+#endif
+  if (slow_pwm_count == 0) {
+    // EXTRUDER 0 
+    soft_pwm_0 = soft_pwm[0];
+    if (soft_pwm_0 > 0) {
+      // turn ON heather only if the minimum time is up 
+      if (state_timer_heater_0 == 0) { 
+	// if change state set timer 
+	if (state_heater_0 == 0) {
+	  state_timer_heater_0 = MIN_STATE_TIME;
+	}
+	state_heater_0 = 1;
+	WRITE(HEATER_0_PIN, 1);
+#ifdef HEATERS_PARALLEL
+	WRITE(HEATER_1_PIN, 1);
+#endif
+      }
+    } else {
+      // turn OFF heather only if the minimum time is up 
+      if (state_timer_heater_0 == 0) {
+	// if change state set timer 
+	if (state_heater_0 == 1) {
+	  state_timer_heater_0 = MIN_STATE_TIME;
+	}
+	state_heater_0 = 0;
+	WRITE(HEATER_0_PIN, 0);
+#ifdef HEATERS_PARALLEL
+	WRITE(HEATER_1_PIN, 0);
+#endif
+      }
+    }
+    
+#if EXTRUDERS > 1
+    // EXTRUDER 1
+    soft_pwm_1 = soft_pwm[1];
+    if (soft_pwm_1 > 0) {
+      // turn ON heather only if the minimum time is up 
+      if (state_timer_heater_1 == 0) { 
+	// if change state set timer 
+	if (state_heater_1 == 0) {
+	  state_timer_heater_1 = MIN_STATE_TIME;
+	}
+	state_heater_1 = 1;
+	WRITE(HEATER_1_PIN, 1);
+      }
+    } else {
+      // turn OFF heather only if the minimum time is up 
+      if (state_timer_heater_1 == 0) {
+	// if change state set timer 
+	if (state_heater_1 == 1) {
+	  state_timer_heater_1 = MIN_STATE_TIME;
+	}
+	state_heater_1 = 0;
+	WRITE(HEATER_1_PIN, 0);
+      }
+    }
+#endif
+    
+#if EXTRUDERS > 2
+    // EXTRUDER 2
+    soft_pwm_2 = soft_pwm[2];
+    if (soft_pwm_2 > 0) {
+      // turn ON heather only if the minimum time is up 
+      if (state_timer_heater_2 == 0) { 
+	// if change state set timer 
+	if (state_heater_2 == 0) {
+	  state_timer_heater_2 = MIN_STATE_TIME;
+	}
+	state_heater_2 = 1;
+	WRITE(HEATER_2_PIN, 1);
+      }
+    } else {
+      // turn OFF heather only if the minimum time is up 
+      if (state_timer_heater_2 == 0) {
+	// if change state set timer 
+	if (state_heater_2 == 1) {
+	  state_timer_heater_2 = MIN_STATE_TIME;
+	}
+	state_heater_2 = 0;
+	WRITE(HEATER_2_PIN, 0);
+      }
+    }
+#endif
+    
+#if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1
+    // BED
+    soft_pwm_b = soft_pwm_bed;
+    if (soft_pwm_b > 0) {
+      // turn ON heather only if the minimum time is up 
+      if (state_timer_heater_b == 0) { 
+	// if change state set timer 
+	if (state_heater_b == 0) {
+	  state_timer_heater_b = MIN_STATE_TIME;
+	}
+	state_heater_b = 1;
+	WRITE(HEATER_BED_PIN, 1);
+      }
+    } else {
+      // turn OFF heather only if the minimum time is up 
+      if (state_timer_heater_b == 0) {
+	// if change state set timer 
+	if (state_heater_b == 1) {
+	  state_timer_heater_b = MIN_STATE_TIME;
+	}
+	state_heater_b = 0;
+	WRITE(HEATER_BED_PIN, 0);
+      }
+    }
+#endif
+  } // if (slow_pwm_count == 0)
+  
+  // EXTRUDER 0 
+  if (soft_pwm_0 < slow_pwm_count) {
+    // turn OFF heather only if the minimum time is up 
+    if (state_timer_heater_0 == 0) { 
+      // if change state set timer 
+      if (state_heater_0 == 1) {
+	state_timer_heater_0 = MIN_STATE_TIME;
+      }
+      state_heater_0 = 0;
+      WRITE(HEATER_0_PIN, 0);
+#ifdef HEATERS_PARALLEL
+      WRITE(HEATER_1_PIN, 0);
+#endif
+    }
+  }
+    
+#if EXTRUDERS > 1
+  // EXTRUDER 1 
+  if (soft_pwm_1 < slow_pwm_count) {
+    // turn OFF heather only if the minimum time is up 
+    if (state_timer_heater_1 == 0) { 
+      // if change state set timer 
+      if (state_heater_1 == 1) {
+	state_timer_heater_1 = MIN_STATE_TIME;
+      }
+      state_heater_1 = 0;
+      WRITE(HEATER_1_PIN, 0);
+    }
+  }
+#endif
+  
+#if EXTRUDERS > 2
+  // EXTRUDER 2
+  if (soft_pwm_2 < slow_pwm_count) {
+    // turn OFF heather only if the minimum time is up 
+    if (state_timer_heater_2 == 0) { 
+      // if change state set timer 
+      if (state_heater_2 == 1) {
+	state_timer_heater_2 = MIN_STATE_TIME;
+      }
+      state_heater_2 = 0;
+      WRITE(HEATER_2_PIN, 0);
+    }
+  }
+#endif
+  
+#if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1
+  // BED
+  if (soft_pwm_b < slow_pwm_count) {
+    // turn OFF heather only if the minimum time is up 
+    if (state_timer_heater_b == 0) { 
+      // if change state set timer 
+      if (state_heater_b == 1) {
+	state_timer_heater_b = MIN_STATE_TIME;
+      }
+      state_heater_b = 0;
+      WRITE(HEATER_BED_PIN, 0);
+    }
+  }
+#endif
+  
+#ifdef FAN_SOFT_PWM
+  if (pwm_count == 0){
+    soft_pwm_fan = fanSpeedSoftPwm / 2;
+    if (soft_pwm_fan > 0) WRITE(FAN_PIN,1); else WRITE(FAN_PIN,0);
+  }
+  if (soft_pwm_fan < pwm_count) WRITE(FAN_PIN,0);
+#endif
+  
+  pwm_count += (1 << SOFT_PWM_SCALE);
+  pwm_count &= 0x7f;
+  
+  // increment slow_pwm_count only every 64 pwm_count circa 65.5ms
+  if ((pwm_count % 64) == 0) {
+    slow_pwm_count++;
+    slow_pwm_count &= 0x7f;
+    
+    // Extruder 0
+    if (state_timer_heater_0 > 0) {
+      state_timer_heater_0--;
+    } 
+  
+#if EXTRUDERS > 1
+    // Extruder 1
+    if (state_timer_heater_1 > 0) 
+      state_timer_heater_1--;
+#endif
+    
+#if EXTRUDERS > 2
+    // Extruder 2
+    if (state_timer_heater_2 > 0) 
+      state_timer_heater_2--;
+#endif
+    
+#if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1
+    // Bed   
+    if (state_timer_heater_b > 0) 
+      state_timer_heater_b--;
+#endif
+  } //if ((pwm_count % 64) == 0) {
+  
+#endif //ifndef SLOW_PWM_HEATERS
+  
+  switch(temp_state) {
+    case 0: // Prepare TEMP_0
+      #if defined(TEMP_0_PIN) && (TEMP_0_PIN > -1)
+        #if TEMP_0_PIN > 7
+          ADCSRB = 1<<MUX5;
+        #else
+          ADCSRB = 0;
+        #endif
+        ADMUX = ((1 << REFS0) | (TEMP_0_PIN & 0x07));
+        ADCSRA |= 1<<ADSC; // Start conversion
+      #endif
+      lcd_buttons_update();
+      temp_state = 1;
+      break;
+    case 1: // Measure TEMP_0
+      #if defined(TEMP_0_PIN) && (TEMP_0_PIN > -1)
+        raw_temp_0_value += ADC;
+      #endif
+      #ifdef HEATER_0_USES_MAX6675 // TODO remove the blocking
+        raw_temp_0_value = read_max6675();
+      #endif
+      temp_state = 2;
+      break;
+    case 2: // Prepare TEMP_BED
+      #if defined(TEMP_BED_PIN) && (TEMP_BED_PIN > -1)
+        #if TEMP_BED_PIN > 7
+          ADCSRB = 1<<MUX5;
+        #else
+          ADCSRB = 0;
+        #endif
+        ADMUX = ((1 << REFS0) | (TEMP_BED_PIN & 0x07));
+        ADCSRA |= 1<<ADSC; // Start conversion
+      #endif
+      lcd_buttons_update();
+      temp_state = 3;
+      break;
+    case 3: // Measure TEMP_BED
+      #if defined(TEMP_BED_PIN) && (TEMP_BED_PIN > -1)
+        raw_temp_bed_value += ADC;
+      #endif
+      temp_state = 4;
+      break;
+    case 4: // Prepare TEMP_1
+      #if defined(TEMP_1_PIN) && (TEMP_1_PIN > -1)
+        #if TEMP_1_PIN > 7
+          ADCSRB = 1<<MUX5;
+        #else
+          ADCSRB = 0;
+        #endif
+        ADMUX = ((1 << REFS0) | (TEMP_1_PIN & 0x07));
+        ADCSRA |= 1<<ADSC; // Start conversion
+      #endif
+      lcd_buttons_update();
+      temp_state = 5;
+      break;
+    case 5: // Measure TEMP_1
+      #if defined(TEMP_1_PIN) && (TEMP_1_PIN > -1)
+        raw_temp_1_value += ADC;
+      #endif
+      temp_state = 6;
+      break;
+    case 6: // Prepare TEMP_2
+      #if defined(TEMP_2_PIN) && (TEMP_2_PIN > -1)
+        #if TEMP_2_PIN > 7
+          ADCSRB = 1<<MUX5;
+        #else
+          ADCSRB = 0;
+        #endif
+        ADMUX = ((1 << REFS0) | (TEMP_2_PIN & 0x07));
+        ADCSRA |= 1<<ADSC; // Start conversion
+      #endif
+      lcd_buttons_update();
+      temp_state = 7;
+      break;
+    case 7: // Measure TEMP_2
+      #if defined(TEMP_2_PIN) && (TEMP_2_PIN > -1)
+        raw_temp_2_value += ADC;
+      #endif
+      temp_state = 8;//change so that Filament Width is also measured
+      
+      break;
+    case 8: //Prepare FILWIDTH 
+     #if defined(FILWIDTH_PIN) && (FILWIDTH_PIN> -1) 
+      #if FILWIDTH_PIN>7 
+         ADCSRB = 1<<MUX5;
+      #else
+         ADCSRB = 0; 
+      #endif 
+      ADMUX = ((1 << REFS0) | (FILWIDTH_PIN & 0x07)); 
+      ADCSRA |= 1<<ADSC; // Start conversion 
+     #endif 
+     lcd_buttons_update();       
+     temp_state = 9; 
+     break; 
+    case 9:   //Measure FILWIDTH 
+     #if defined(FILWIDTH_PIN) &&(FILWIDTH_PIN > -1) 
+     //raw_filwidth_value += ADC;  //remove to use an IIR filter approach 
+      if(ADC>102)  //check that ADC is reading a voltage > 0.5 volts, otherwise don't take in the data.
+        {
+    	raw_filwidth_value= raw_filwidth_value-(raw_filwidth_value>>7);  //multipliy raw_filwidth_value by 127/128
+        
+        raw_filwidth_value= raw_filwidth_value + ((unsigned long)ADC<<7);  //add new ADC reading 
+        }
+     #endif 
+     temp_state = 0;   
+      
+     temp_count++;
+     break;      
+      
+      
+    case 10: //Startup, delay initial temp reading a tiny bit so the hardware can settle.
+      temp_state = 0;
+      break;
+//    default:
+//      SERIAL_ERROR_START;
+//      SERIAL_ERRORLNPGM("Temp measurement error!");
+//      break;
+  }
+    
+  if(temp_count >= OVERSAMPLENR) // 10 * 16 * 1/(16000000/64/256)  = 164ms.
+  {
+    if (!temp_meas_ready) //Only update the raw values if they have been read. Else we could be updating them during reading.
+    {
+      current_temperature_raw[0] = raw_temp_0_value;
+#if EXTRUDERS > 1
+      current_temperature_raw[1] = raw_temp_1_value;
+#endif
+#ifdef TEMP_SENSOR_1_AS_REDUNDANT
+      redundant_temperature_raw = raw_temp_1_value;
+#endif
+#if EXTRUDERS > 2
+      current_temperature_raw[2] = raw_temp_2_value;
+#endif
+      current_temperature_bed_raw = raw_temp_bed_value;
+    }
+
+//Add similar code for Filament Sensor - can be read any time since IIR filtering is used 
+#if defined(FILWIDTH_PIN) &&(FILWIDTH_PIN > -1)
+  current_raw_filwidth = raw_filwidth_value>>10;  //need to divide to get to 0-16384 range since we used 1/128 IIR filter approach 
+#endif
+    
+    
+    temp_meas_ready = true;
+    temp_count = 0;
+    raw_temp_0_value = 0;
+    raw_temp_1_value = 0;
+    raw_temp_2_value = 0;
+    raw_temp_bed_value = 0;
+
+#if HEATER_0_RAW_LO_TEMP > HEATER_0_RAW_HI_TEMP
+    if(current_temperature_raw[0] <= maxttemp_raw[0]) {
+#else
+    if(current_temperature_raw[0] >= maxttemp_raw[0]) {
+#endif
+        max_temp_error(0);
+    }
+#if HEATER_0_RAW_LO_TEMP > HEATER_0_RAW_HI_TEMP
+    if(current_temperature_raw[0] >= minttemp_raw[0]) {
+#else
+    if(current_temperature_raw[0] <= minttemp_raw[0]) {
+#endif
+        min_temp_error(0);
+    }
+#if EXTRUDERS > 1
+#if HEATER_1_RAW_LO_TEMP > HEATER_1_RAW_HI_TEMP
+    if(current_temperature_raw[1] <= maxttemp_raw[1]) {
+#else
+    if(current_temperature_raw[1] >= maxttemp_raw[1]) {
+#endif
+        max_temp_error(1);
+    }
+#if HEATER_1_RAW_LO_TEMP > HEATER_1_RAW_HI_TEMP
+    if(current_temperature_raw[1] >= minttemp_raw[1]) {
+#else
+    if(current_temperature_raw[1] <= minttemp_raw[1]) {
+#endif
+        min_temp_error(1);
+    }
+#endif
+#if EXTRUDERS > 2
+#if HEATER_2_RAW_LO_TEMP > HEATER_2_RAW_HI_TEMP
+    if(current_temperature_raw[2] <= maxttemp_raw[2]) {
+#else
+    if(current_temperature_raw[2] >= maxttemp_raw[2]) {
+#endif
+        max_temp_error(2);
+    }
+#if HEATER_2_RAW_LO_TEMP > HEATER_2_RAW_HI_TEMP
+    if(current_temperature_raw[2] >= minttemp_raw[2]) {
+#else
+    if(current_temperature_raw[2] <= minttemp_raw[2]) {
+#endif
+        min_temp_error(2);
+    }
+#endif
+  
+  /* No bed MINTEMP error? */
+        
+        
+#if defined(BED_MAXTEMP) && (TEMP_SENSOR_BED != 0)
+# if HEATER_BED_RAW_LO_TEMP > HEATER_BED_RAW_HI_TEMP
+    if(current_temperature_bed_raw <= bed_maxttemp_raw) {
+#else
+    if(current_temperature_bed_raw >= bed_maxttemp_raw) {
+#endif
+       target_temperature_bed = 0;
+       bed_max_temp_error();
+    }
+  }
+        
+# if HEATER_BED_RAW_LO_TEMP > HEATER_BED_RAW_HI_TEMP
+        if(current_temperature_bed_raw >= bed_minttemp_raw) {
+#else
+            if(current_temperature_bed_raw <= bed_minttemp_raw) {
+#endif
+                bed_min_temp_error();
+            }
+            
+#endif
+  
+#ifdef BABYSTEPPING
+  for(uint8_t axis=0;axis<3;axis++)
+  {
+    int curTodo=babystepsTodo[axis]; //get rid of volatile for performance
+   
+    if(curTodo>0)
+    {
+      babystep(axis,/*fwd*/true);
+      babystepsTodo[axis]--; //less to do next time
+    }
+    else
+    if(curTodo<0)
+    {
+      babystep(axis,/*fwd*/false);
+      babystepsTodo[axis]++; //less to do next time
+    }
+  }
+#endif //BABYSTEPPING
+}
+
+#ifdef PIDTEMP
+// Apply the scale factors to the PID values
+
+
+float scalePID_i(float i)
+{
+	return i*PID_dT;
+}
+
+float unscalePID_i(float i)
+{
+	return i/PID_dT;
+}
+
+float scalePID_d(float d)
+{
+    return d/PID_dT;
+}
+
+float unscalePID_d(float d)
+{
+	return d*PID_dT;
+}
+
+#endif //PIDTEMP
+
+

+ 211 - 211
Firmware/temperature.h

@@ -1,211 +1,211 @@
-/*
-  temperature.h - temperature controller
-  Part of Marlin
-
-  Copyright (c) 2011 Erik van der Zalm
-
-  Grbl is free software: you can redistribute it and/or modify
-  it under the terms of the GNU General Public License as published by
-  the Free Software Foundation, either version 3 of the License, or
-  (at your option) any later version.
-
-  Grbl is distributed in the hope that it will be useful,
-  but WITHOUT ANY WARRANTY; without even the implied warranty of
-  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-  GNU General Public License for more details.
-
-  You should have received a copy of the GNU General Public License
-  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
-*/
-
-#ifndef temperature_h
-#define temperature_h 
-
-#include "Marlin.h"
-#include "planner.h"
-#ifdef PID_ADD_EXTRUSION_RATE
-  #include "stepper.h"
-#endif
-
-// public functions
-void tp_init();  //initialize the heating
-void manage_heater(); //it is critical that this is called periodically.
-
-#ifdef FILAMENT_SENSOR
-// For converting raw Filament Width to milimeters 
- float analog2widthFil(); 
- 
-// For converting raw Filament Width to an extrusion ratio 
- int widthFil_to_size_ratio();
-#endif
-
-// low level conversion routines
-// do not use these routines and variables outside of temperature.cpp
-extern int target_temperature[EXTRUDERS];  
-extern float current_temperature[EXTRUDERS];
-#ifdef SHOW_TEMP_ADC_VALUES
-  extern int current_temperature_raw[EXTRUDERS];
-  extern int current_temperature_bed_raw;
-#endif
-extern int target_temperature_bed;
-extern float current_temperature_bed;
-#ifdef TEMP_SENSOR_1_AS_REDUNDANT
-  extern float redundant_temperature;
-#endif
-
-#if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1
-  extern unsigned char soft_pwm_bed;
-#endif
-
-#ifdef PIDTEMP
-  extern int pid_cycle, pid_number_of_cycles;
-  extern float Kp,Ki,Kd,Kc,_Kp,_Ki,_Kd;
-  extern bool pid_tuning_finished;
-  float scalePID_i(float i);
-  float scalePID_d(float d);
-  float unscalePID_i(float i);
-  float unscalePID_d(float d);
-
-#endif
-#ifdef PIDTEMPBED
-  extern float bedKp,bedKi,bedKd;
-#endif
-  
-  
-#ifdef BABYSTEPPING
-  extern volatile int babystepsTodo[3];
-#endif
-
-inline void babystepsTodoZadd(int n)
-{
-    if (n != 0) {
-        CRITICAL_SECTION_START
-        babystepsTodo[Z_AXIS] += n;
-        CRITICAL_SECTION_END
-    }
-}
-
-inline void babystepsTodoZsubtract(int n)
-{
-    if (n != 0) {
-        CRITICAL_SECTION_START
-        babystepsTodo[Z_AXIS] -= n;
-        CRITICAL_SECTION_END
-    }
-}
-
-//high level conversion routines, for use outside of temperature.cpp
-//inline so that there is no performance decrease.
-//deg=degreeCelsius
-
-FORCE_INLINE float degHotend(uint8_t extruder) {  
-  return current_temperature[extruder];
-};
-
-#ifdef SHOW_TEMP_ADC_VALUES
-  FORCE_INLINE float rawHotendTemp(uint8_t extruder) {  
-    return current_temperature_raw[extruder];
-  };
-
-  FORCE_INLINE float rawBedTemp() {  
-    return current_temperature_bed_raw;
-  };
-#endif
-
-FORCE_INLINE float degBed() {
-  return current_temperature_bed;
-};
-
-FORCE_INLINE float degTargetHotend(uint8_t extruder) {  
-  return target_temperature[extruder];
-};
-
-FORCE_INLINE float degTargetBed() {   
-  return target_temperature_bed;
-};
-
-FORCE_INLINE void setTargetHotend(const float &celsius, uint8_t extruder) {  
-  target_temperature[extruder] = celsius;
-};
-
-FORCE_INLINE void setTargetBed(const float &celsius) {  
-  target_temperature_bed = celsius;
-};
-
-FORCE_INLINE bool isHeatingHotend(uint8_t extruder){  
-  return target_temperature[extruder] > current_temperature[extruder];
-};
-
-FORCE_INLINE bool isHeatingBed() {
-  return target_temperature_bed > current_temperature_bed;
-};
-
-FORCE_INLINE bool isCoolingHotend(uint8_t extruder) {  
-  return target_temperature[extruder] < current_temperature[extruder];
-};
-
-FORCE_INLINE bool isCoolingBed() {
-  return target_temperature_bed < current_temperature_bed;
-};
-
-#define degHotend0() degHotend(0)
-#define degTargetHotend0() degTargetHotend(0)
-#define setTargetHotend0(_celsius) setTargetHotend((_celsius), 0)
-#define isHeatingHotend0() isHeatingHotend(0)
-#define isCoolingHotend0() isCoolingHotend(0)
-#if EXTRUDERS > 1
-#define degHotend1() degHotend(1)
-#define degTargetHotend1() degTargetHotend(1)
-#define setTargetHotend1(_celsius) setTargetHotend((_celsius), 1)
-#define isHeatingHotend1() isHeatingHotend(1)
-#define isCoolingHotend1() isCoolingHotend(1)
-#else
-#define setTargetHotend1(_celsius) do{}while(0)
-#endif
-#if EXTRUDERS > 2
-#define degHotend2() degHotend(2)
-#define degTargetHotend2() degTargetHotend(2)
-#define setTargetHotend2(_celsius) setTargetHotend((_celsius), 2)
-#define isHeatingHotend2() isHeatingHotend(2)
-#define isCoolingHotend2() isCoolingHotend(2)
-#else
-#define setTargetHotend2(_celsius) do{}while(0)
-#endif
-#if EXTRUDERS > 3
-#error Invalid number of extruders
-#endif
-
-#if (defined (TEMP_RUNAWAY_BED_HYSTERESIS) && TEMP_RUNAWAY_BED_TIMEOUT > 0) || (defined (TEMP_RUNAWAY_EXTRUDER_HYSTERESIS) && TEMP_RUNAWAY_EXTRUDER_TIMEOUT > 0)
-static float temp_runaway_status[4];
-static float temp_runaway_target[4];
-static float temp_runaway_timer[4];
-static int temp_runaway_error_counter[4];
-
-void temp_runaway_check(int _heater_id, float _target_temperature, float _current_temperature, float _output, bool _isbed);
-void temp_runaway_stop(bool isPreheat, bool isBed);
-#endif
-
-int getHeaterPower(int heater);
-void disable_heater();
-void setWatch();
-void updatePID();
-
-
-FORCE_INLINE void autotempShutdown(){
- #ifdef AUTOTEMP
- if(autotemp_enabled)
- {
-  autotemp_enabled=false;
-  if(degTargetHotend(active_extruder)>autotemp_min)
-    setTargetHotend(0,active_extruder);
- }
- #endif
-}
-
-void PID_autotune(float temp, int extruder, int ncycles);
-
-void setExtruderAutoFanState(int pin, bool state);
-void checkExtruderAutoFans();
-
-#endif
-
+/*
+  temperature.h - temperature controller
+  Part of Marlin
+
+  Copyright (c) 2011 Erik van der Zalm
+
+  Grbl is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  Grbl is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef temperature_h
+#define temperature_h 
+
+#include "Marlin.h"
+#include "planner.h"
+#ifdef PID_ADD_EXTRUSION_RATE
+  #include "stepper.h"
+#endif
+
+// public functions
+void tp_init();  //initialize the heating
+void manage_heater(); //it is critical that this is called periodically.
+
+#ifdef FILAMENT_SENSOR
+// For converting raw Filament Width to milimeters 
+ float analog2widthFil(); 
+ 
+// For converting raw Filament Width to an extrusion ratio 
+ int widthFil_to_size_ratio();
+#endif
+
+// low level conversion routines
+// do not use these routines and variables outside of temperature.cpp
+extern int target_temperature[EXTRUDERS];  
+extern float current_temperature[EXTRUDERS];
+#ifdef SHOW_TEMP_ADC_VALUES
+  extern int current_temperature_raw[EXTRUDERS];
+  extern int current_temperature_bed_raw;
+#endif
+extern int target_temperature_bed;
+extern float current_temperature_bed;
+#ifdef TEMP_SENSOR_1_AS_REDUNDANT
+  extern float redundant_temperature;
+#endif
+
+#if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1
+  extern unsigned char soft_pwm_bed;
+#endif
+
+#ifdef PIDTEMP
+  extern int pid_cycle, pid_number_of_cycles;
+  extern float Kp,Ki,Kd,Kc,_Kp,_Ki,_Kd;
+  extern bool pid_tuning_finished;
+  float scalePID_i(float i);
+  float scalePID_d(float d);
+  float unscalePID_i(float i);
+  float unscalePID_d(float d);
+
+#endif
+#ifdef PIDTEMPBED
+  extern float bedKp,bedKi,bedKd;
+#endif
+  
+  
+#ifdef BABYSTEPPING
+  extern volatile int babystepsTodo[3];
+#endif
+
+inline void babystepsTodoZadd(int n)
+{
+    if (n != 0) {
+        CRITICAL_SECTION_START
+        babystepsTodo[Z_AXIS] += n;
+        CRITICAL_SECTION_END
+    }
+}
+
+inline void babystepsTodoZsubtract(int n)
+{
+    if (n != 0) {
+        CRITICAL_SECTION_START
+        babystepsTodo[Z_AXIS] -= n;
+        CRITICAL_SECTION_END
+    }
+}
+
+//high level conversion routines, for use outside of temperature.cpp
+//inline so that there is no performance decrease.
+//deg=degreeCelsius
+
+FORCE_INLINE float degHotend(uint8_t extruder) {  
+  return current_temperature[extruder];
+};
+
+#ifdef SHOW_TEMP_ADC_VALUES
+  FORCE_INLINE float rawHotendTemp(uint8_t extruder) {  
+    return current_temperature_raw[extruder];
+  };
+
+  FORCE_INLINE float rawBedTemp() {  
+    return current_temperature_bed_raw;
+  };
+#endif
+
+FORCE_INLINE float degBed() {
+  return current_temperature_bed;
+};
+
+FORCE_INLINE float degTargetHotend(uint8_t extruder) {  
+  return target_temperature[extruder];
+};
+
+FORCE_INLINE float degTargetBed() {   
+  return target_temperature_bed;
+};
+
+FORCE_INLINE void setTargetHotend(const float &celsius, uint8_t extruder) {  
+  target_temperature[extruder] = celsius;
+};
+
+FORCE_INLINE void setTargetBed(const float &celsius) {  
+  target_temperature_bed = celsius;
+};
+
+FORCE_INLINE bool isHeatingHotend(uint8_t extruder){  
+  return target_temperature[extruder] > current_temperature[extruder];
+};
+
+FORCE_INLINE bool isHeatingBed() {
+  return target_temperature_bed > current_temperature_bed;
+};
+
+FORCE_INLINE bool isCoolingHotend(uint8_t extruder) {  
+  return target_temperature[extruder] < current_temperature[extruder];
+};
+
+FORCE_INLINE bool isCoolingBed() {
+  return target_temperature_bed < current_temperature_bed;
+};
+
+#define degHotend0() degHotend(0)
+#define degTargetHotend0() degTargetHotend(0)
+#define setTargetHotend0(_celsius) setTargetHotend((_celsius), 0)
+#define isHeatingHotend0() isHeatingHotend(0)
+#define isCoolingHotend0() isCoolingHotend(0)
+#if EXTRUDERS > 1
+#define degHotend1() degHotend(1)
+#define degTargetHotend1() degTargetHotend(1)
+#define setTargetHotend1(_celsius) setTargetHotend((_celsius), 1)
+#define isHeatingHotend1() isHeatingHotend(1)
+#define isCoolingHotend1() isCoolingHotend(1)
+#else
+#define setTargetHotend1(_celsius) do{}while(0)
+#endif
+#if EXTRUDERS > 2
+#define degHotend2() degHotend(2)
+#define degTargetHotend2() degTargetHotend(2)
+#define setTargetHotend2(_celsius) setTargetHotend((_celsius), 2)
+#define isHeatingHotend2() isHeatingHotend(2)
+#define isCoolingHotend2() isCoolingHotend(2)
+#else
+#define setTargetHotend2(_celsius) do{}while(0)
+#endif
+#if EXTRUDERS > 3
+#error Invalid number of extruders
+#endif
+
+#if (defined (TEMP_RUNAWAY_BED_HYSTERESIS) && TEMP_RUNAWAY_BED_TIMEOUT > 0) || (defined (TEMP_RUNAWAY_EXTRUDER_HYSTERESIS) && TEMP_RUNAWAY_EXTRUDER_TIMEOUT > 0)
+static float temp_runaway_status[4];
+static float temp_runaway_target[4];
+static float temp_runaway_timer[4];
+static int temp_runaway_error_counter[4];
+
+void temp_runaway_check(int _heater_id, float _target_temperature, float _current_temperature, float _output, bool _isbed);
+void temp_runaway_stop(bool isPreheat, bool isBed);
+#endif
+
+int getHeaterPower(int heater);
+void disable_heater();
+void setWatch();
+void updatePID();
+
+
+FORCE_INLINE void autotempShutdown(){
+ #ifdef AUTOTEMP
+ if(autotemp_enabled)
+ {
+  autotemp_enabled=false;
+  if(degTargetHotend(active_extruder)>autotemp_min)
+    setTargetHotend(0,active_extruder);
+ }
+ #endif
+}
+
+void PID_autotune(float temp, int extruder, int ncycles);
+
+void setExtruderAutoFanState(int pin, bool state);
+void checkExtruderAutoFans();
+
+#endif
+

+ 288 - 288
Firmware/util.cpp

@@ -1,288 +1,288 @@
-#include "Configuration.h"
-
-#include "ultralcd.h"
-#include "language.h"
-#include "util.h"
-
-// Allocate the version string in the program memory. Otherwise the string lands either on the stack or in the global RAM.
-const char FW_VERSION_STR[] PROGMEM = FW_version;
-
-const char* FW_VERSION_STR_P()
-{
-    return FW_VERSION_STR;
-}
-
-const char FW_PRUSA3D_MAGIC_STR[] PROGMEM = FW_PRUSA3D_MAGIC;
-
-const char* FW_PRUSA3D_MAGIC_STR_P()
-{
-    return FW_PRUSA3D_MAGIC_STR;
-}
-
-const char STR_REVISION_DEV  [] PROGMEM = "dev";
-const char STR_REVISION_ALPHA[] PROGMEM = "alpha";
-const char STR_REVISION_BETA [] PROGMEM = "beta";
-const char STR_REVISION_RC   [] PROGMEM = "rc";
-
-inline bool is_whitespace_or_nl(char c)
-{
-    return c == ' ' || c == '\t' || c == '\n' || c == 'r';
-}
-
-inline bool is_whitespace_or_nl_or_eol(char c)
-{
-    return c == 0 || c == ' ' || c == '\t' || c == '\n' || c == '\r';
-}
-
-inline bool is_digit(char c)
-{
-    return c >= '0' && c <= '9';
-}
-
-// Parse a major.minor.revision version number.
-// Return true if valid.
-inline bool parse_version(const char *str, uint16_t version[4])
-{   
-#if 0
-    SERIAL_ECHOPGM("Parsing version string ");
-    SERIAL_ECHO(str);
-    SERIAL_ECHOLNPGM("");
-#endif
-
-    const char *major = str;
-    const char *p = str;
-    while (is_digit(*p)) ++ p;
-    if (*p != '.')
-        return false;
-    const char *minor = ++ p;
-    while (is_digit(*p)) ++ p;
-    if (*p != '.')
-        return false;
-    const char *rev = ++ p;
-    while (is_digit(*p)) ++ p;
-    if (! is_whitespace_or_nl_or_eol(*p) && *p != '-')
-        return false;
-
-    char *endptr = NULL;
-    version[0] = strtol(major, &endptr, 10);
-    if (endptr != minor - 1)
-        return false;
-    version[1] = strtol(minor, &endptr, 10);
-    if (endptr != rev - 1)
-        return false;
-    version[2] = strtol(rev, &endptr, 10);
-    if (endptr != p)
-        return false;
-
-    version[3] = FIRMWARE_REVISION_RELEASED;
-    if (*p ++ == '-') {
-        const char *q = p;
-        while (! is_whitespace_or_nl_or_eol(*q))
-            ++ q;
-        uint8_t n = q - p;
-        if (n == strlen_P(STR_REVISION_DEV) && strncmp_P(p, STR_REVISION_DEV, n) == 0)
-            version[3] = FIRMWARE_REVISION_DEV;
-        else if (n == strlen_P(STR_REVISION_ALPHA) && strncmp_P(p, STR_REVISION_ALPHA, n) == 0)
-            version[3] = FIRMWARE_REVISION_ALPHA;
-        else if (n == strlen_P(STR_REVISION_BETA) && strncmp_P(p, STR_REVISION_BETA, n) == 0)
-            version[3] = FIRMWARE_REVISION_BETA;
-        else if ((n == 2 || n == 3) && p[0] == 'r' && p[1] == 'c') {
-            if (n == 2)
-                version[3] = FIRMWARE_REVISION_RC;
-            else {
-                if (is_digit(p[2]))
-                    version[3] = FIRMWARE_REVISION_RC + p[2] - '1';
-                else
-                    return false;
-            }
-        } else
-            return false;
-    }
-
-#if 0
-    SERIAL_ECHOPGM("Version parsed, major: ");
-    SERIAL_ECHO(version[0]);
-    SERIAL_ECHOPGM(", minor: ");
-    SERIAL_ECHO(version[1]);
-    SERIAL_ECHOPGM(", revision: ");
-    SERIAL_ECHO(version[2]);
-    SERIAL_ECHOPGM(", flavor: ");
-    SERIAL_ECHO(version[3]);
-    SERIAL_ECHOLNPGM("");
-#endif
-    return true;
-}
-
-inline bool strncmp_PP(const char *p1, const char *p2, uint8_t n)
-{
-    for (; n > 0; -- n, ++ p1, ++ p2) {
-        if (pgm_read_byte(p1) < pgm_read_byte(p2))
-            return -1;
-        if (pgm_read_byte(p1) > pgm_read_byte(p2))
-            return 1;
-        if (pgm_read_byte(p1) == 0)
-            return 0;
-    }
-    return 0;
-}
-
-// Parse a major.minor.revision version number.
-// Return true if valid.
-inline bool parse_version_P(const char *str, uint16_t version[4])
-{    
-#if 0
-    SERIAL_ECHOPGM("Parsing version string ");
-    SERIAL_ECHORPGM(str);
-    SERIAL_ECHOLNPGM("");
-#endif
-
-    const char *major = str;
-    const char *p = str;
-    while (is_digit(char(pgm_read_byte(p)))) ++ p;
-    if (pgm_read_byte(p) != '.')
-        return false;
-    const char *minor = ++ p;
-    while (is_digit(char(pgm_read_byte(p)))) ++ p;
-    if (pgm_read_byte(p) != '.')
-        return false;
-    const char *rev = ++ p;
-    while (is_digit(char(pgm_read_byte(p)))) ++ p;
-    if (! is_whitespace_or_nl_or_eol(char(pgm_read_byte(p))) && pgm_read_byte(p) != '-')
-        return false;
-
-    char buf[5];
-    uint8_t n = minor - major - 1;
-    if (n > 4)
-        return false;
-    memcpy_P(buf, major, n); buf[n] = 0;
-    char *endptr = NULL;
-    version[0] = strtol(buf, &endptr, 10);
-    if (*endptr != 0)
-        return false;
-    n = rev - minor - 1;
-    if (n > 4)
-        return false;
-    memcpy_P(buf, minor, n); buf[n] = 0;
-    version[1] = strtol(buf, &endptr, 10);
-    if (*endptr != 0)
-        return false;
-    n = p - rev;
-    if (n > 4)
-        return false;
-    memcpy_P(buf, rev, n);
-    buf[n] = 0;
-    version[2] = strtol(buf, &endptr, 10);
-    if (*endptr != 0)
-        return false;
-
-    version[3] = FIRMWARE_REVISION_RELEASED;
-    if (pgm_read_byte(p ++) == '-') {
-        const char *q = p;
-        while (! is_whitespace_or_nl_or_eol(char(pgm_read_byte(q))))
-            ++ q;
-        n = q - p;
-        if (n == strlen_P(STR_REVISION_DEV) && strncmp_PP(p, STR_REVISION_DEV, n) == 0)
-            version[3] = FIRMWARE_REVISION_DEV;
-        else if (n == strlen_P(STR_REVISION_ALPHA) && strncmp_PP(p, STR_REVISION_ALPHA, n) == 0)
-            version[3] = FIRMWARE_REVISION_ALPHA;
-        else if (n == strlen_P(STR_REVISION_BETA) && strncmp_PP(p, STR_REVISION_BETA, n) == 0)
-            version[3] = FIRMWARE_REVISION_BETA;
-        else if ((n == 2 || n == 3) && strncmp_PP(p, STR_REVISION_RC, 2) == 0) {
-            if (n == 2)
-                version[3] = FIRMWARE_REVISION_RC;
-            else {
-                p += 2;
-                if (is_digit(pgm_read_byte(p)))
-                    version[3] = FIRMWARE_REVISION_RC + pgm_read_byte(p) - '1';
-                else
-                    return false;
-            }
-        } else
-            return false;
-    }
-
-#if 0
-    SERIAL_ECHOPGM("Version parsed, major: ");
-    SERIAL_ECHO(version[0]);
-    SERIAL_ECHOPGM(", minor: ");
-    SERIAL_ECHO(version[1]);
-    SERIAL_ECHOPGM(", revision: ");
-    SERIAL_ECHO(version[2]);
-    SERIAL_ECHOPGM(", flavor: ");
-    SERIAL_ECHO(version[3]);
-    SERIAL_ECHOLNPGM("");
-#endif
-    return true;
-}
-
-// 1 - yes, 0 - false, -1 - error;
-inline int8_t is_provided_version_newer(const char *version_string)
-{
-    uint16_t ver_gcode[3], ver_current[3];
-    if (! parse_version(version_string, ver_gcode))
-        return -1;
-    if (! parse_version_P(FW_VERSION_STR, ver_current))
-        return 0; // this shall not happen
-    for (uint8_t i = 0; i < 3; ++ i)
-        if (ver_gcode[i] > ver_current[i])
-            return 1;
-    return 0;
-}
-
-bool show_upgrade_dialog_if_version_newer(const char *version_string)
-{
-    uint16_t ver_gcode[4], ver_current[4];
-    if (! parse_version(version_string, ver_gcode)) {
-//        SERIAL_PROTOCOLLNPGM("parse_version failed");
-        return false;
-    }
-    if (! parse_version_P(FW_VERSION_STR, ver_current)) {
-//        SERIAL_PROTOCOLLNPGM("parse_version_P failed");
-        return false; // this shall not happen
-    }
-//    SERIAL_PROTOCOLLNPGM("versions parsed");
-    bool upgrade = false;
-    for (uint8_t i = 0; i < 4; ++ i) {
-        if (ver_gcode[i] > ver_current[i]) {
-            upgrade = true;
-            break;
-        } else if (ver_gcode[i] < ver_current[i])
-            break;
-    }
-
-    if (upgrade) {
-        lcd_display_message_fullscreen_P(MSG_NEW_FIRMWARE_AVAILABLE);
-        lcd_print_at_PGM(0, 2, PSTR(""));
-        for (const char *c = version_string; ! is_whitespace_or_nl_or_eol(*c); ++ c)
-            lcd_implementation_write(*c);
-        lcd_print_at_PGM(0, 3, MSG_NEW_FIRMWARE_PLEASE_UPGRADE);
-        tone(BEEPER, 1000);
-        delay_keep_alive(50);
-        noTone(BEEPER);
-        delay_keep_alive(500);
-        tone(BEEPER, 1000);
-        delay_keep_alive(50);
-        noTone(BEEPER);
-        lcd_wait_for_click();
-        lcd_update_enable(true);
-        lcd_implementation_clear();
-        lcd_update();
-    }
-
-    // Succeeded.
-    return true;
-}
-
-void update_current_firmware_version_to_eeprom()
-{
-    for (int8_t i = 0; i < FW_PRUSA3D_MAGIC_LEN; ++ i)
-        eeprom_update_byte((uint8_t*)(EEPROM_FIRMWARE_PRUSA_MAGIC+i), pgm_read_byte(FW_PRUSA3D_MAGIC_STR+i));
-    uint16_t ver_current[4];
-    if (parse_version_P(FW_VERSION_STR, ver_current)) {
-        eeprom_update_word((uint16_t*)EEPROM_FIRMWARE_VERSION_MAJOR,    ver_current[0]);
-        eeprom_update_word((uint16_t*)EEPROM_FIRMWARE_VERSION_MINOR,    ver_current[1]);
-        eeprom_update_word((uint16_t*)EEPROM_FIRMWARE_VERSION_REVISION, ver_current[2]);
-        // See FirmwareRevisionFlavorType for the definition of firmware flavors.
-        eeprom_update_word((uint16_t*)EEPROM_FIRMWARE_VERSION_FLAVOR,   ver_current[3]);
-    }
-}
+#include "Configuration.h"
+
+#include "ultralcd.h"
+#include "language.h"
+#include "util.h"
+
+// Allocate the version string in the program memory. Otherwise the string lands either on the stack or in the global RAM.
+const char FW_VERSION_STR[] PROGMEM = FW_version;
+
+const char* FW_VERSION_STR_P()
+{
+    return FW_VERSION_STR;
+}
+
+const char FW_PRUSA3D_MAGIC_STR[] PROGMEM = FW_PRUSA3D_MAGIC;
+
+const char* FW_PRUSA3D_MAGIC_STR_P()
+{
+    return FW_PRUSA3D_MAGIC_STR;
+}
+
+const char STR_REVISION_DEV  [] PROGMEM = "dev";
+const char STR_REVISION_ALPHA[] PROGMEM = "alpha";
+const char STR_REVISION_BETA [] PROGMEM = "beta";
+const char STR_REVISION_RC   [] PROGMEM = "rc";
+
+inline bool is_whitespace_or_nl(char c)
+{
+    return c == ' ' || c == '\t' || c == '\n' || c == 'r';
+}
+
+inline bool is_whitespace_or_nl_or_eol(char c)
+{
+    return c == 0 || c == ' ' || c == '\t' || c == '\n' || c == '\r';
+}
+
+inline bool is_digit(char c)
+{
+    return c >= '0' && c <= '9';
+}
+
+// Parse a major.minor.revision version number.
+// Return true if valid.
+inline bool parse_version(const char *str, uint16_t version[4])
+{   
+#if 0
+    SERIAL_ECHOPGM("Parsing version string ");
+    SERIAL_ECHO(str);
+    SERIAL_ECHOLNPGM("");
+#endif
+
+    const char *major = str;
+    const char *p = str;
+    while (is_digit(*p)) ++ p;
+    if (*p != '.')
+        return false;
+    const char *minor = ++ p;
+    while (is_digit(*p)) ++ p;
+    if (*p != '.')
+        return false;
+    const char *rev = ++ p;
+    while (is_digit(*p)) ++ p;
+    if (! is_whitespace_or_nl_or_eol(*p) && *p != '-')
+        return false;
+
+    char *endptr = NULL;
+    version[0] = strtol(major, &endptr, 10);
+    if (endptr != minor - 1)
+        return false;
+    version[1] = strtol(minor, &endptr, 10);
+    if (endptr != rev - 1)
+        return false;
+    version[2] = strtol(rev, &endptr, 10);
+    if (endptr != p)
+        return false;
+
+    version[3] = FIRMWARE_REVISION_RELEASED;
+    if (*p ++ == '-') {
+        const char *q = p;
+        while (! is_whitespace_or_nl_or_eol(*q))
+            ++ q;
+        uint8_t n = q - p;
+        if (n == strlen_P(STR_REVISION_DEV) && strncmp_P(p, STR_REVISION_DEV, n) == 0)
+            version[3] = FIRMWARE_REVISION_DEV;
+        else if (n == strlen_P(STR_REVISION_ALPHA) && strncmp_P(p, STR_REVISION_ALPHA, n) == 0)
+            version[3] = FIRMWARE_REVISION_ALPHA;
+        else if (n == strlen_P(STR_REVISION_BETA) && strncmp_P(p, STR_REVISION_BETA, n) == 0)
+            version[3] = FIRMWARE_REVISION_BETA;
+        else if ((n == 2 || n == 3) && p[0] == 'r' && p[1] == 'c') {
+            if (n == 2)
+                version[3] = FIRMWARE_REVISION_RC;
+            else {
+                if (is_digit(p[2]))
+                    version[3] = FIRMWARE_REVISION_RC + p[2] - '1';
+                else
+                    return false;
+            }
+        } else
+            return false;
+    }
+
+#if 0
+    SERIAL_ECHOPGM("Version parsed, major: ");
+    SERIAL_ECHO(version[0]);
+    SERIAL_ECHOPGM(", minor: ");
+    SERIAL_ECHO(version[1]);
+    SERIAL_ECHOPGM(", revision: ");
+    SERIAL_ECHO(version[2]);
+    SERIAL_ECHOPGM(", flavor: ");
+    SERIAL_ECHO(version[3]);
+    SERIAL_ECHOLNPGM("");
+#endif
+    return true;
+}
+
+inline bool strncmp_PP(const char *p1, const char *p2, uint8_t n)
+{
+    for (; n > 0; -- n, ++ p1, ++ p2) {
+        if (pgm_read_byte(p1) < pgm_read_byte(p2))
+            return -1;
+        if (pgm_read_byte(p1) > pgm_read_byte(p2))
+            return 1;
+        if (pgm_read_byte(p1) == 0)
+            return 0;
+    }
+    return 0;
+}
+
+// Parse a major.minor.revision version number.
+// Return true if valid.
+inline bool parse_version_P(const char *str, uint16_t version[4])
+{    
+#if 0
+    SERIAL_ECHOPGM("Parsing version string ");
+    SERIAL_ECHORPGM(str);
+    SERIAL_ECHOLNPGM("");
+#endif
+
+    const char *major = str;
+    const char *p = str;
+    while (is_digit(char(pgm_read_byte(p)))) ++ p;
+    if (pgm_read_byte(p) != '.')
+        return false;
+    const char *minor = ++ p;
+    while (is_digit(char(pgm_read_byte(p)))) ++ p;
+    if (pgm_read_byte(p) != '.')
+        return false;
+    const char *rev = ++ p;
+    while (is_digit(char(pgm_read_byte(p)))) ++ p;
+    if (! is_whitespace_or_nl_or_eol(char(pgm_read_byte(p))) && pgm_read_byte(p) != '-')
+        return false;
+
+    char buf[5];
+    uint8_t n = minor - major - 1;
+    if (n > 4)
+        return false;
+    memcpy_P(buf, major, n); buf[n] = 0;
+    char *endptr = NULL;
+    version[0] = strtol(buf, &endptr, 10);
+    if (*endptr != 0)
+        return false;
+    n = rev - minor - 1;
+    if (n > 4)
+        return false;
+    memcpy_P(buf, minor, n); buf[n] = 0;
+    version[1] = strtol(buf, &endptr, 10);
+    if (*endptr != 0)
+        return false;
+    n = p - rev;
+    if (n > 4)
+        return false;
+    memcpy_P(buf, rev, n);
+    buf[n] = 0;
+    version[2] = strtol(buf, &endptr, 10);
+    if (*endptr != 0)
+        return false;
+
+    version[3] = FIRMWARE_REVISION_RELEASED;
+    if (pgm_read_byte(p ++) == '-') {
+        const char *q = p;
+        while (! is_whitespace_or_nl_or_eol(char(pgm_read_byte(q))))
+            ++ q;
+        n = q - p;
+        if (n == strlen_P(STR_REVISION_DEV) && strncmp_PP(p, STR_REVISION_DEV, n) == 0)
+            version[3] = FIRMWARE_REVISION_DEV;
+        else if (n == strlen_P(STR_REVISION_ALPHA) && strncmp_PP(p, STR_REVISION_ALPHA, n) == 0)
+            version[3] = FIRMWARE_REVISION_ALPHA;
+        else if (n == strlen_P(STR_REVISION_BETA) && strncmp_PP(p, STR_REVISION_BETA, n) == 0)
+            version[3] = FIRMWARE_REVISION_BETA;
+        else if ((n == 2 || n == 3) && strncmp_PP(p, STR_REVISION_RC, 2) == 0) {
+            if (n == 2)
+                version[3] = FIRMWARE_REVISION_RC;
+            else {
+                p += 2;
+                if (is_digit(pgm_read_byte(p)))
+                    version[3] = FIRMWARE_REVISION_RC + pgm_read_byte(p) - '1';
+                else
+                    return false;
+            }
+        } else
+            return false;
+    }
+
+#if 0
+    SERIAL_ECHOPGM("Version parsed, major: ");
+    SERIAL_ECHO(version[0]);
+    SERIAL_ECHOPGM(", minor: ");
+    SERIAL_ECHO(version[1]);
+    SERIAL_ECHOPGM(", revision: ");
+    SERIAL_ECHO(version[2]);
+    SERIAL_ECHOPGM(", flavor: ");
+    SERIAL_ECHO(version[3]);
+    SERIAL_ECHOLNPGM("");
+#endif
+    return true;
+}
+
+// 1 - yes, 0 - false, -1 - error;
+inline int8_t is_provided_version_newer(const char *version_string)
+{
+    uint16_t ver_gcode[3], ver_current[3];
+    if (! parse_version(version_string, ver_gcode))
+        return -1;
+    if (! parse_version_P(FW_VERSION_STR, ver_current))
+        return 0; // this shall not happen
+    for (uint8_t i = 0; i < 3; ++ i)
+        if (ver_gcode[i] > ver_current[i])
+            return 1;
+    return 0;
+}
+
+bool show_upgrade_dialog_if_version_newer(const char *version_string)
+{
+    uint16_t ver_gcode[4], ver_current[4];
+    if (! parse_version(version_string, ver_gcode)) {
+//        SERIAL_PROTOCOLLNPGM("parse_version failed");
+        return false;
+    }
+    if (! parse_version_P(FW_VERSION_STR, ver_current)) {
+//        SERIAL_PROTOCOLLNPGM("parse_version_P failed");
+        return false; // this shall not happen
+    }
+//    SERIAL_PROTOCOLLNPGM("versions parsed");
+    bool upgrade = false;
+    for (uint8_t i = 0; i < 4; ++ i) {
+        if (ver_gcode[i] > ver_current[i]) {
+            upgrade = true;
+            break;
+        } else if (ver_gcode[i] < ver_current[i])
+            break;
+    }
+
+    if (upgrade) {
+        lcd_display_message_fullscreen_P(MSG_NEW_FIRMWARE_AVAILABLE);
+        lcd_print_at_PGM(0, 2, PSTR(""));
+        for (const char *c = version_string; ! is_whitespace_or_nl_or_eol(*c); ++ c)
+            lcd_implementation_write(*c);
+        lcd_print_at_PGM(0, 3, MSG_NEW_FIRMWARE_PLEASE_UPGRADE);
+        tone(BEEPER, 1000);
+        delay_keep_alive(50);
+        noTone(BEEPER);
+        delay_keep_alive(500);
+        tone(BEEPER, 1000);
+        delay_keep_alive(50);
+        noTone(BEEPER);
+        lcd_wait_for_click();
+        lcd_update_enable(true);
+        lcd_implementation_clear();
+        lcd_update();
+    }
+
+    // Succeeded.
+    return true;
+}
+
+void update_current_firmware_version_to_eeprom()
+{
+    for (int8_t i = 0; i < FW_PRUSA3D_MAGIC_LEN; ++ i)
+        eeprom_update_byte((uint8_t*)(EEPROM_FIRMWARE_PRUSA_MAGIC+i), pgm_read_byte(FW_PRUSA3D_MAGIC_STR+i));
+    uint16_t ver_current[4];
+    if (parse_version_P(FW_VERSION_STR, ver_current)) {
+        eeprom_update_word((uint16_t*)EEPROM_FIRMWARE_VERSION_MAJOR,    ver_current[0]);
+        eeprom_update_word((uint16_t*)EEPROM_FIRMWARE_VERSION_MINOR,    ver_current[1]);
+        eeprom_update_word((uint16_t*)EEPROM_FIRMWARE_VERSION_REVISION, ver_current[2]);
+        // See FirmwareRevisionFlavorType for the definition of firmware flavors.
+        eeprom_update_word((uint16_t*)EEPROM_FIRMWARE_VERSION_FLAVOR,   ver_current[3]);
+    }
+}

+ 35 - 35
Firmware/util.h

@@ -1,35 +1,35 @@
-#ifndef UTIL_H
-#define UTIL_H
-
-extern const char* FW_VERSION_STR_P();
-
-// Definition of a firmware flavor numerical values.
-enum FirmwareRevisionFlavorType
-{
-    FIRMWARE_REVISION_DEV = 0,
-    FIRMWARE_REVISION_ALPHA = 1,
-    FIRMWARE_REVISION_BETA = 2,
-    FIRMWARE_REVISION_RC,
-    FIRMWARE_REVISION_RC2,
-    FIRMWARE_REVISION_RC3,
-    FIRMWARE_REVISION_RC4,
-    FIRMWARE_REVISION_RC5,
-    FIRMWARE_REVISION_RELEASED = 127
-};
-
-extern bool show_upgrade_dialog_if_version_newer(const char *version_string);
-
-extern void update_current_firmware_version_to_eeprom();
-
-
-
-inline int8_t eeprom_read_int8(unsigned char* addr) {
-	uint8_t v = eeprom_read_byte(addr);
-	return *reinterpret_cast<int8_t*>(&v);
-}
-
-inline void eeprom_update_int8(unsigned char* addr, int8_t v) {
-	eeprom_update_byte(addr, *reinterpret_cast<uint8_t*>(&v));
-}
-
-#endif /* UTIL_H */
+#ifndef UTIL_H
+#define UTIL_H
+
+extern const char* FW_VERSION_STR_P();
+
+// Definition of a firmware flavor numerical values.
+enum FirmwareRevisionFlavorType
+{
+    FIRMWARE_REVISION_DEV = 0,
+    FIRMWARE_REVISION_ALPHA = 1,
+    FIRMWARE_REVISION_BETA = 2,
+    FIRMWARE_REVISION_RC,
+    FIRMWARE_REVISION_RC2,
+    FIRMWARE_REVISION_RC3,
+    FIRMWARE_REVISION_RC4,
+    FIRMWARE_REVISION_RC5,
+    FIRMWARE_REVISION_RELEASED = 127
+};
+
+extern bool show_upgrade_dialog_if_version_newer(const char *version_string);
+
+extern void update_current_firmware_version_to_eeprom();
+
+
+
+inline int8_t eeprom_read_int8(unsigned char* addr) {
+	uint8_t v = eeprom_read_byte(addr);
+	return *reinterpret_cast<int8_t*>(&v);
+}
+
+inline void eeprom_update_int8(unsigned char* addr, int8_t v) {
+	eeprom_update_byte(addr, *reinterpret_cast<uint8_t*>(&v));
+}
+
+#endif /* UTIL_H */