Bläddra i källkod

Merge branch 'MK3' into MK3

MRprusa3d 6 år sedan
förälder
incheckning
7e462b3224

+ 4 - 6
Firmware/Configuration.h

@@ -7,8 +7,8 @@
 #define STR(x) STR_HELPER(x)
 
 // Firmware version
-#define FW_VERSION "3.2.0-alpha"
-#define FW_COMMIT_NR   370
+#define FW_VERSION "3.2.0-RC2"
+#define FW_COMMIT_NR   461
 // FW_VERSION_UNKNOWN means this is an unofficial build.
 // The firmware should only be checked into github with this symbol.
 #define FW_DEV_VERSION FW_VERSION_UNKNOWN
@@ -132,10 +132,6 @@
 // Power loss errors (total)
 #define EEPROM_POWER_COUNT_TOT       (EEPROM_FERROR_COUNT_TOT - 2)             // uint16
 
-#define EEPROM_PRINTER_TYPE          (EEPROM_POWER_COUNT_TOT - 2)              // uint16
-#define EEPROM_BOARD_TYPE            (EEPROM_PRINTER_TYPE - 2)                 // uint16
-
-
 ////////////////////////////////////////
 // TMC2130 Accurate sensorless homing 
 
@@ -174,6 +170,8 @@
 #define EEPROM_TMC2130_Z_MRES              (EEPROM_TMC2130_Y_MRES - 1)                         // uint8
 #define EEPROM_TMC2130_E_MRES              (EEPROM_TMC2130_Z_MRES - 1)                         // uint8
 
+#define EEPROM_PRINTER_TYPE          (EEPROM_TMC2130_E_MRES - 2)                               // uint16
+#define EEPROM_BOARD_TYPE            (EEPROM_PRINTER_TYPE - 2)                                 // uint16
 
 //TMC2130 configuration
 #define EEPROM_TMC_AXIS_SIZE  //axis configuration block size

+ 0 - 616
Firmware/Configuration_prusa.h

@@ -1,616 +0,0 @@
-#ifndef CONFIGURATION_PRUSA_H
-#define CONFIGURATION_PRUSA_H
-
-/*------------------------------------
- GENERAL SETTINGS
- *------------------------------------*/
-
-// Printer revision
-#define PRINTER_TYPE PRINTER_MK3
-#define FILAMENT_SIZE "1_75mm_MK3"
-#define NOZZLE_TYPE "E3Dv6full"
-
-// Developer flag
-#define DEVELOPER
-
-// Printer name
-#define CUSTOM_MENDEL_NAME "Prusa i3 MK3"
-
-// Electronics
-#define MOTHERBOARD BOARD_EINSY_1_0a
-#define STEEL_SHEET
-#define HAS_SECOND_SERIAL_PORT
-
-
-// Uncomment the below for the E3D PT100 temperature sensor (with or without PT100 Amplifier)
-//#define E3D_PT100_EXTRUDER_WITH_AMP
-//#define E3D_PT100_EXTRUDER_NO_AMP
-//#define E3D_PT100_BED_WITH_AMP
-//#define E3D_PT100_BED_NO_AMP
-
-
-/*------------------------------------
- AXIS SETTINGS
- *------------------------------------*/
-
-// Steps per unit {X,Y,Z,E}
-//#define DEFAULT_AXIS_STEPS_PER_UNIT   {100,100,3200/8,140}
-#define DEFAULT_AXIS_STEPS_PER_UNIT   {100,100,3200/8,280}
-//#define DEFAULT_AXIS_STEPS_PER_UNIT   {100,100,3200/8,560}
-
-// Endstop inverting
-const bool X_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
-const bool Y_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
-const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
-
-// Direction inverting
-#define INVERT_X_DIR true    // for Mendel set to false, for Orca set to true
-#define INVERT_Y_DIR false    // for Mendel set to true, for Orca set to false
-#define INVERT_Z_DIR true     // for Mendel set to false, for Orca set to true
-#define INVERT_E0_DIR false   // for direct drive extruder v9 set to true, for geared extruder set to false
-#define INVERT_E1_DIR false    // for direct drive extruder v9 set to true, for geared extruder set to false
-#define INVERT_E2_DIR false   // for direct drive extruder v9 set to true, for geared extruder set to false
-
-// Home position
-#define MANUAL_X_HOME_POS 0
-#define MANUAL_Y_HOME_POS -2.2
-#define MANUAL_Z_HOME_POS 0.2
-
-// Travel limits after homing
-#define X_MAX_POS 255
-#define X_MIN_POS 0
-#define Y_MAX_POS 210
-#define Y_MIN_POS -4 //orig -4
-#define Z_MAX_POS 210
-#define Z_MIN_POS 0.15
-
-// Canceled home position
-#define X_CANCEL_POS 50
-#define Y_CANCEL_POS 190
-
-//Pause print position
-#define X_PAUSE_POS 50
-#define Y_PAUSE_POS 190
-#define Z_PAUSE_LIFT 20
-
-#define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
-#define HOMING_FEEDRATE {3000, 3000, 800, 0}  // set the homing speeds (mm/min) // 3000 is also valid for stallGuard homing. Valid range: 2200 - 3000
-
-#define DEFAULT_MAX_FEEDRATE          {200, 200, 12, 120}      // (mm/sec)   max feedrate (M203)
-#define DEFAULT_MAX_ACCELERATION      {1000, 1000, 200, 5000}  // (mm/sec^2) max acceleration (M201)
-
-
-#define DEFAULT_ACCELERATION          1250   // X, Y, Z and E max acceleration in mm/s^2 for printing moves (M204S)
-#define DEFAULT_RETRACT_ACCELERATION  1250   // X, Y, Z and E max acceleration in mm/s^2 for retracts (M204T)
-
-#define MANUAL_FEEDRATE {2700, 2700, 1000, 100}   // set the speeds for manual moves (mm/min)
-
-//Silent mode limits
-#define SILENT_MAX_ACCEL  960 // max axxeleration in silent mode in mm/s^2
-#define SILENT_MAX_ACCEL_ST (100*SILENT_MAX_ACCEL) // max accel in steps/s^2
-#define SILENT_MAX_FEEDRATE 172  //max feedrate in mm/s, because mode switched to normal for homming , this value limits also homing, it should be greater (172mm/s=9600mm/min>2700mm/min)
-
-//Normal mode limits
-#define NORMAL_MAX_ACCEL 2500 // Y-axis max axxeleration in normal mode in mm/s^2
-#define NORMAL_MAX_ACCEL_ST (100*NORMAL_MAX_ACCEL) // max accel in steps/s^2
-#define NORMAL_MAX_FEEDRATE 200  //max feedrate in mm/s, because mode switched to normal for homming , this value limits also homing, it should be greater (172mm/s=9600mm/min>2700mm/min)
-
-//#define SIMPLE_ACCEL_LIMIT          //new limitation method for normal/silent
-
-//number of bytes from end of the file to start check
-#define END_FILE_SECTION 10000
-
-#define Z_AXIS_ALWAYS_ON 1
-
-// Automatic recovery after crash is detected
-#define AUTOMATIC_RECOVERY_AFTER_CRASH
-
-// New XYZ calibration
-#define NEW_XYZCAL
-
-// Do not use Arduino SPI 
-#define NEW_SPI
-
-// Watchdog support
-#define WATCHDOG
-
-// Power panic
-#define UVLO_SUPPORT
-
-// Fan check
-#define FANCHECK
-
-// Safety timer
-#define SAFETYTIMER
-
-// Filament sensor
-#define PAT9125
-
-
-// Disable some commands
-#define _DISABLE_M42_M226
-
-// Minimum ambient temperature limit to start triggering MINTEMP errors [C]
-// this value is litlebit higher that real limit, because ambient termistor is on the board and is temperated from it,
-// temperature inside the case is around 31C for ambient temperature 25C, when the printer is powered on long time and idle
-// the real limit is 15C (same as MINTEMP limit), this is because 15C is end of scale for both used thermistors (bed, heater)
-#define MINTEMP_MINAMBIENT      25
-#define MINTEMP_MINAMBIENT_RAW  978
-
-//#define DEBUG_BUILD
-#ifdef DEBUG_BUILD
-//#define _NO_ASM
-#define DEBUG_DCODES //D codes
-#define DEBUG_STACK_MONITOR        //Stack monitor in stepper ISR
-//#define DEBUG_FSENSOR_LOG          //Reports fsensor status to serial
-//#define DEBUG_CRASHDET_COUNTERS  //Display crash-detection counters on LCD
-//#define DEBUG_RESUME_PRINT       //Resume/save print debug enable 
-//#define DEBUG_UVLO_AUTOMATIC_RECOVER // Power panic automatic recovery debug output 
-//#define DEBUG_DISABLE_XMINLIMIT  //x min limit ignored
-//#define DEBUG_DISABLE_XMAXLIMIT  //x max limit ignored
-//#define DEBUG_DISABLE_YMINLIMIT  //y min limit ignored
-//#define DEBUG_DISABLE_YMAXLIMIT  //y max limit ignored
-//#define DEBUG_DISABLE_ZMINLIMIT  //z min limit ignored
-//#define DEBUG_DISABLE_ZMAXLIMIT  //z max limit ignored
-#define DEBUG_DISABLE_STARTMSGS //no startup messages 
-//#define DEBUG_DISABLE_MINTEMP   //mintemp error ignored
-//#define DEBUG_DISABLE_SWLIMITS  //sw limits ignored
-//#define DEBUG_DISABLE_LCD_STATUS_LINE  //empty four lcd line
-//#define DEBUG_DISABLE_PREVENT_EXTRUDER //cold extrusion and long extrusion allowed
-//#define DEBUG_DISABLE_PRUSA_STATISTICS //disable prusa_statistics() mesages
-//#define DEBUG_DISABLE_FORCE_SELFTEST //disable force selftest
-//#define DEBUG_XSTEP_DUP_PIN 21   //duplicate x-step output to pin 21 (SCL on P3)
-//#define DEBUG_YSTEP_DUP_PIN 21   //duplicate y-step output to pin 21 (SCL on P3)
-//#define DEBUG_BLINK_ACTIVE
-//#define DEBUG_DISABLE_FANCHECK     //disable fan check (no ISR INT7, check disabled)
-//#define DEBUG_DISABLE_FSENSORCHECK //disable fsensor check (no ISR INT7, check disabled)
-#define DEBUG_DUMP_TO_2ND_SERIAL   //dump received characters to 2nd serial line
-#define DEBUG_STEPPER_TIMER_MISSED // Stop on stepper timer overflow, beep and display a message.
-#define PLANNER_DIAGNOSTICS // Show the planner queue status on printer display.
-#endif /* DEBUG_BUILD */
-
-//#define EXPERIMENTAL_FEATURES
-#define TMC2130_LINEARITY_CORRECTION
-//#define TMC2130_VARIABLE_RESOLUTION
-
-
-
-/*------------------------------------
- TMC2130 default settings
- *------------------------------------*/
-
-#define TMC2130_FCLK 12000000       // fclk = 12MHz
-
-#define TMC2130_USTEPS_XY   16        // microstep resolution for XY axes
-#define TMC2130_USTEPS_Z    16        // microstep resolution for Z axis
-#define TMC2130_USTEPS_E    32        // microstep resolution for E axis
-#define TMC2130_INTPOL_XY   1         // extrapolate 256 for XY axes
-#define TMC2130_INTPOL_Z    1         // extrapolate 256 for Z axis
-#define TMC2130_INTPOL_E    1         // extrapolate 256 for E axis
-
-#define TMC2130_PWM_GRAD_X  2         // PWMCONF
-#define TMC2130_PWM_AMPL_X  230       // PWMCONF
-#define TMC2130_PWM_AUTO_X  1         // PWMCONF
-#define TMC2130_PWM_FREQ_X  2         // PWMCONF
-
-#define TMC2130_PWM_GRAD_Y  2         // PWMCONF
-#define TMC2130_PWM_AMPL_Y  235       // PWMCONF
-#define TMC2130_PWM_AUTO_Y  1         // PWMCONF
-#define TMC2130_PWM_FREQ_Y  2         // PWMCONF
-
-#define TMC2130_PWM_GRAD_E  2         // PWMCONF
-#define TMC2130_PWM_AMPL_E  235       // PWMCONF
-#define TMC2130_PWM_AUTO_E  1         // PWMCONF
-#define TMC2130_PWM_FREQ_E  2         // PWMCONF
-
-#define TMC2130_PWM_GRAD_Z  4         // PWMCONF
-#define TMC2130_PWM_AMPL_Z  200       // PWMCONF
-#define TMC2130_PWM_AUTO_Z  1         // PWMCONF
-#define TMC2130_PWM_FREQ_Z  2         // PWMCONF
-
-#define TMC2130_PWM_GRAD_E  4         // PWMCONF
-#define TMC2130_PWM_AMPL_E  240       // PWMCONF
-#define TMC2130_PWM_AUTO_E  1         // PWMCONF
-#define TMC2130_PWM_FREQ_E  2         // PWMCONF
-
-#define TMC2130_TOFF_XYZ    3         // CHOPCONF // fchop = 27.778kHz
-#define TMC2130_TOFF_E      3         // CHOPCONF // fchop = 27.778kHz
-//#define TMC2130_TOFF_E      4         // CHOPCONF // fchop = 21.429kHz
-//#define TMC2130_TOFF_E      5         // CHOPCONF // fchop = 17.442kHz
-
-//#define TMC2130_STEALTH_E // Extruder stealthChop mode
-//#define TMC2130_CNSTOFF_E // Extruder constant-off-time mode (similar to MK2)
-
-//#define TMC2130_PWM_DIV   683         // PWM frequency divider (1024, 683, 512, 410)
-#define TMC2130_PWM_DIV   512         // PWM frequency divider (1024, 683, 512, 410)
-#define TMC2130_PWM_CLK   (2 * TMC2130_FCLK / TMC2130_PWM_DIV) // PWM frequency (23.4kHz, 35.1kHz, 46.9kHz, 58.5kHz for 12MHz fclk)
-
-#define TMC2130_TPWMTHRS  0         // TPWMTHRS - Sets the switching speed threshold based on TSTEP from stealthChop to spreadCycle mode
-#define TMC2130_THIGH     0         // THIGH - unused
-
-//#define TMC2130_TCOOLTHRS_X 450       // TCOOLTHRS - coolstep treshold
-//#define TMC2130_TCOOLTHRS_Y 450       // TCOOLTHRS - coolstep treshold
-#define TMC2130_TCOOLTHRS_X 430       // TCOOLTHRS - coolstep treshold
-#define TMC2130_TCOOLTHRS_Y 430       // TCOOLTHRS - coolstep treshold
-#define TMC2130_TCOOLTHRS_Z 500       // TCOOLTHRS - coolstep treshold
-#define TMC2130_TCOOLTHRS_E 500       // TCOOLTHRS - coolstep treshold
-
-#define TMC2130_SG_HOMING       1     // stallguard homing
-#define TMC2130_SG_THRS_X       3     // stallguard sensitivity for X axis
-#define TMC2130_SG_THRS_Y       3     // stallguard sensitivity for Y axis
-#define TMC2130_SG_THRS_Z       3     // stallguard sensitivity for Z axis
-#define TMC2130_SG_THRS_E       3     // stallguard sensitivity for E axis
-
-//new settings is possible for vsense = 1, running current value > 31 set vsense to zero and shift both currents by 1 bit right (Z axis only)
-#define TMC2130_CURRENTS_H {16, 20, 28, 36}  // default holding currents for all axes
-#define TMC2130_CURRENTS_R {16, 20, 28, 36}  // default running currents for all axes
-#define TMC2130_UNLOAD_CURRENT_R 12			 // lowe current for M600 to protect filament sensor 
-
-#define TMC2130_STEALTH_Z
-
-//#define TMC2130_DEBUG
-//#define TMC2130_DEBUG_WR
-//#define TMC2130_DEBUG_RD
-
-
-/*------------------------------------
- EXTRUDER SETTINGS
- *------------------------------------*/
-
-// Mintemps
-#define HEATER_0_MINTEMP 15
-#define HEATER_1_MINTEMP 5
-#define HEATER_2_MINTEMP 5
-#define BED_MINTEMP 15
-
-// Maxtemps
-#if defined(E3D_PT100_EXTRUDER_WITH_AMP) || defined(E3D_PT100_EXTRUDER_NO_AMP)
-#define HEATER_0_MAXTEMP 410
-#else
-#define HEATER_0_MAXTEMP 305
-#endif
-#define HEATER_1_MAXTEMP 305
-#define HEATER_2_MAXTEMP 305
-#define BED_MAXTEMP 125
-
-#if defined(E3D_PT100_EXTRUDER_WITH_AMP) || defined(E3D_PT100_EXTRUDER_NO_AMP)
-// Define PID constants for extruder with PT100
-#define  DEFAULT_Kp 21.70
-#define  DEFAULT_Ki 1.60
-#define  DEFAULT_Kd 73.76
-#else
-// Define PID constants for extruder
-//#define  DEFAULT_Kp 40.925
-//#define  DEFAULT_Ki 4.875
-//#define  DEFAULT_Kd 86.085
-#define  DEFAULT_Kp 16.13
-#define  DEFAULT_Ki 1.1625
-#define  DEFAULT_Kd 56.23
-#endif
-
-// Extrude mintemp
-#define EXTRUDE_MINTEMP 190
-
-// Extruder cooling fans
-#define EXTRUDER_0_AUTO_FAN_PIN   8
-#define EXTRUDER_1_AUTO_FAN_PIN   -1
-#define EXTRUDER_2_AUTO_FAN_PIN   -1
-#define EXTRUDER_AUTO_FAN_TEMPERATURE 50
-#define EXTRUDER_AUTO_FAN_SPEED   255  // == full speed
-
-
-
-/*------------------------------------
- LOAD/UNLOAD FILAMENT SETTINGS
- *------------------------------------*/
-
-// Load filament commands
-#define LOAD_FILAMENT_0 "M83"
-#define LOAD_FILAMENT_1 "G1 E70 F400"
-#define LOAD_FILAMENT_2 "G1 E40 F100"
-
-// Unload filament commands
-#define UNLOAD_FILAMENT_0 "M83"
-#define UNLOAD_FILAMENT_1 "G1 E-80 F7000"
-
-/*------------------------------------
- CHANGE FILAMENT SETTINGS
- *------------------------------------*/
-
-// Filament change configuration
-#define FILAMENTCHANGEENABLE
-#ifdef FILAMENTCHANGEENABLE
-#define FILAMENTCHANGE_XPOS 211
-#define FILAMENTCHANGE_YPOS 0
-#define FILAMENTCHANGE_ZADD 2
-#define FILAMENTCHANGE_FIRSTRETRACT -2
-#define FILAMENTCHANGE_FINALRETRACT -80
-
-#define FILAMENTCHANGE_FIRSTFEED 70
-#define FILAMENTCHANGE_FINALFEED 50
-#define FILAMENTCHANGE_RECFEED 5
-
-#define FILAMENTCHANGE_XYFEED 50
-#define FILAMENTCHANGE_EFEED 20
-//#define FILAMENTCHANGE_RFEED 400
-#define FILAMENTCHANGE_RFEED 7000 / 60
-#define FILAMENTCHANGE_EXFEED 2
-#define FILAMENTCHANGE_ZFEED 15
-
-#endif
-
-/*------------------------------------
- ADDITIONAL FEATURES SETTINGS
- *------------------------------------*/
-
-// Define Prusa filament runout sensor
-//#define FILAMENT_RUNOUT_SUPPORT
-
-#ifdef FILAMENT_RUNOUT_SUPPORT
-#define FILAMENT_RUNOUT_SENSOR 1
-#endif
-
-// temperature runaway
-#define TEMP_RUNAWAY_BED_HYSTERESIS 5
-#define TEMP_RUNAWAY_BED_TIMEOUT 360
-
-#define TEMP_RUNAWAY_EXTRUDER_HYSTERESIS 15
-#define TEMP_RUNAWAY_EXTRUDER_TIMEOUT 45
-
-/*------------------------------------
- MOTOR CURRENT SETTINGS
- *------------------------------------*/
-
-// Motor Current setting for BIG RAMBo
-#define DIGIPOT_MOTOR_CURRENT {135,135,135,135,135} // Values 0-255 (RAMBO 135 = ~0.75A, 185 = ~1A)
-#define DIGIPOT_MOTOR_CURRENT_LOUD {135,135,135,135,135}
-
-// Motor Current settings for RAMBo mini PWM value = MotorCurrentSetting * 255 / range
-#if MOTHERBOARD == 200 || MOTHERBOARD == 203
-#define MOTOR_CURRENT_PWM_RANGE 2000
-#define DEFAULT_PWM_MOTOR_CURRENT  {400, 750, 750} // {XY,Z,E}
-#define DEFAULT_PWM_MOTOR_CURRENT_LOUD  {400, 750, 750} // {XY,Z,E}
-#endif
-
-/*------------------------------------
- PAT9125 SETTINGS
- *------------------------------------*/
-
-#define PAT9125_XRES			0
-#define PAT9125_YRES			255
-
-/*------------------------------------
- BED SETTINGS
- *------------------------------------*/
-
-// Define Mesh Bed Leveling system to enable it
-#define MESH_BED_LEVELING
-#ifdef MESH_BED_LEVELING
-
-#define MBL_Z_STEP 0.01
-
-// Mesh definitions
-#define MESH_MIN_X 35
-#define MESH_MAX_X 238
-#define MESH_MIN_Y 6
-#define MESH_MAX_Y 202
-
-// Mesh upsample definition
-#define MESH_NUM_X_POINTS 7
-#define MESH_NUM_Y_POINTS 7
-// Mesh measure definition
-#define MESH_MEAS_NUM_X_POINTS 3
-#define MESH_MEAS_NUM_Y_POINTS 3
-
-#define MESH_HOME_Z_CALIB 0.2
-#define MESH_HOME_Z_SEARCH 5 //Z lift for homing, mesh bed leveling etc.
-
-#define X_PROBE_OFFSET_FROM_EXTRUDER 23     // Z probe to nozzle X offset: -left  +right
-#define Y_PROBE_OFFSET_FROM_EXTRUDER 5     // Z probe to nozzle Y offset: -front +behind
-#define Z_PROBE_OFFSET_FROM_EXTRUDER -0.4  // Z probe to nozzle Z offset: -below (always!)
-#endif
-
-// Bed Temperature Control
-// Select PID or bang-bang with PIDTEMPBED. If bang-bang, BED_LIMIT_SWITCHING will enable hysteresis
-//
-// Uncomment this to enable PID on the bed. It uses the same frequency PWM as the extruder.
-// If your PID_dT above is the default, and correct for your hardware/configuration, that means 7.689Hz,
-// which is fine for driving a square wave into a resistive load and does not significantly impact you FET heating.
-// This also works fine on a Fotek SSR-10DA Solid State Relay into a 250W heater.
-// If your configuration is significantly different than this and you don't understand the issues involved, you probably
-// shouldn't use bed PID until someone else verifies your hardware works.
-// If this is enabled, find your own PID constants below.
-#define PIDTEMPBED
-//
-//#define BED_LIMIT_SWITCHING
-
-// This sets the max power delivered to the bed, and replaces the HEATER_BED_DUTY_CYCLE_DIVIDER option.
-// all forms of bed control obey this (PID, bang-bang, bang-bang with hysteresis)
-// setting this to anything other than 255 enables a form of PWM to the bed just like HEATER_BED_DUTY_CYCLE_DIVIDER did,
-// so you shouldn't use it unless you are OK with PWM on your bed.  (see the comment on enabling PIDTEMPBED)
-#define MAX_BED_POWER 255 // limits duty cycle to bed; 255=full current
-
-// Bed temperature compensation settings
-#define BED_OFFSET 10
-#define BED_OFFSET_START 40
-#define BED_OFFSET_CENTER 50
-
-
-#ifdef PIDTEMPBED
-//120v 250W silicone heater into 4mm borosilicate (MendelMax 1.5+)
-//from FOPDT model - kp=.39 Tp=405 Tdead=66, Tc set to 79.2, aggressive factor of .15 (vs .1, 1, 10)
-#if defined(E3D_PT100_BED_WITH_AMP) || defined(E3D_PT100_BED_NO_AMP)
-// Define PID constants for extruder with PT100
-#define  DEFAULT_bedKp 21.70
-#define  DEFAULT_bedKi 1.60
-#define  DEFAULT_bedKd 73.76
-#else
-#define  DEFAULT_bedKp 126.13
-#define  DEFAULT_bedKi 4.30
-#define  DEFAULT_bedKd 924.76
-#endif
-
-//120v 250W silicone heater into 4mm borosilicate (MendelMax 1.5+)
-//from pidautotune
-//    #define  DEFAULT_bedKp 97.1
-//    #define  DEFAULT_bedKi 1.41
-//    #define  DEFAULT_bedKd 1675.16
-
-// FIND YOUR OWN: "M303 E-1 C8 S90" to run autotune on the bed at 90 degreesC for 8 cycles.
-#endif // PIDTEMPBED
-
-
-/*-----------------------------------
- PREHEAT SETTINGS
- *------------------------------------*/
-
-#define FARM_PREHEAT_HOTEND_TEMP 250
-#define FARM_PREHEAT_HPB_TEMP 40
-#define FARM_PREHEAT_FAN_SPEED 0
-
-#define PLA_PREHEAT_HOTEND_TEMP 215
-#define PLA_PREHEAT_HPB_TEMP 60
-#define PLA_PREHEAT_FAN_SPEED 0
-
-#define ABS_PREHEAT_HOTEND_TEMP 255
-#define ABS_PREHEAT_HPB_TEMP 100
-#define ABS_PREHEAT_FAN_SPEED 0
-
-#define HIPS_PREHEAT_HOTEND_TEMP 220
-#define HIPS_PREHEAT_HPB_TEMP 100
-#define HIPS_PREHEAT_FAN_SPEED 0
-
-#define PP_PREHEAT_HOTEND_TEMP 254
-#define PP_PREHEAT_HPB_TEMP 100
-#define PP_PREHEAT_FAN_SPEED 0
-
-#define PET_PREHEAT_HOTEND_TEMP 230
-#define PET_PREHEAT_HPB_TEMP 85
-#define PET_PREHEAT_FAN_SPEED 0
-
-#define FLEX_PREHEAT_HOTEND_TEMP 240
-#define FLEX_PREHEAT_HPB_TEMP 50
-#define FLEX_PREHEAT_FAN_SPEED 0
-
-/*------------------------------------
- THERMISTORS SETTINGS
- *------------------------------------*/
-
-//
-//--NORMAL IS 4.7kohm PULLUP!-- 1kohm pullup can be used on hotend sensor, using correct resistor and table
-//
-//// Temperature sensor settings:
-// -2 is thermocouple with MAX6675 (only for sensor 0)
-// -1 is thermocouple with AD595
-// 0 is not used
-// 1 is 100k thermistor - best choice for EPCOS 100k (4.7k pullup)
-// 2 is 200k thermistor - ATC Semitec 204GT-2 (4.7k pullup)
-// 3 is Mendel-parts thermistor (4.7k pullup)
-// 4 is 10k thermistor !! do not use it for a hotend. It gives bad resolution at high temp. !!
-// 5 is 100K thermistor - ATC Semitec 104GT-2 (Used in ParCan & J-Head) (4.7k pullup)
-// 6 is 100k EPCOS - Not as accurate as table 1 (created using a fluke thermocouple) (4.7k pullup)
-// 7 is 100k Honeywell thermistor 135-104LAG-J01 (4.7k pullup)
-// 71 is 100k Honeywell thermistor 135-104LAF-J01 (4.7k pullup)
-// 8 is 100k 0603 SMD Vishay NTCS0603E3104FXT (4.7k pullup)
-// 9 is 100k GE Sensing AL03006-58.2K-97-G1 (4.7k pullup)
-// 10 is 100k RS thermistor 198-961 (4.7k pullup)
-// 11 is 100k beta 3950 1% thermistor (4.7k pullup)
-// 12 is 100k 0603 SMD Vishay NTCS0603E3104FXT (4.7k pullup) (calibrated for Makibox hot bed)
-// 13 is 100k Hisens 3950  1% up to 300°C for hotend "Simple ONE " & "Hotend "All In ONE"
-// 20 is the PT100 circuit found in the Ultimainboard V2.x
-// 60 is 100k Maker's Tool Works Kapton Bed Thermistor beta=3950
-//
-//    1k ohm pullup tables - This is not normal, you would have to have changed out your 4.7k for 1k
-//                          (but gives greater accuracy and more stable PID)
-// 51 is 100k thermistor - EPCOS (1k pullup)
-// 52 is 200k thermistor - ATC Semitec 204GT-2 (1k pullup)
-// 55 is 100k thermistor - ATC Semitec 104GT-2 (Used in ParCan & J-Head) (1k pullup)
-//
-// 1047 is Pt1000 with 4k7 pullup
-// 1010 is Pt1000 with 1k pullup (non standard)
-// 147 is Pt100 with 4k7 pullup
-// 148 is E3D Pt100 with 4k7 pullup and no PT100 Amplifier on a MiniRambo 1.3a
-// 247 is Pt100 with 4k7 pullup and PT100 Amplifier
-// 110 is Pt100 with 1k pullup (non standard)
-
-#if defined(E3D_PT100_EXTRUDER_WITH_AMP)
-#define TEMP_SENSOR_0 247
-#elif defined(E3D_PT100_EXTRUDER_NO_AMP)
-#define TEMP_SENSOR_0 148
-#else
-#define TEMP_SENSOR_0 5
-#endif
-#define TEMP_SENSOR_1 0
-#define TEMP_SENSOR_2 0
-#if defined(E3D_PT100_BED_WITH_AMP)
-#define TEMP_SENSOR_BED 247
-#elif defined(E3D_PT100_BED_NO_AMP)
-#define TEMP_SENSOR_BED 148
-#else
-#define TEMP_SENSOR_BED 1
-#endif
-#define TEMP_SENSOR_PINDA 1
-#define TEMP_SENSOR_AMBIENT 2000
-
-#define STACK_GUARD_TEST_VALUE 0xA2A2
-
-#define MAX_BED_TEMP_CALIBRATION 50
-#define MAX_HOTEND_TEMP_CALIBRATION 50
-
-#define MAX_E_STEPS_PER_UNIT 250
-#define MIN_E_STEPS_PER_UNIT 100
-
-#define Z_BABYSTEP_MIN -3999
-#define Z_BABYSTEP_MAX 0
-
-#define PINDA_PREHEAT_X 20
-#define PINDA_PREHEAT_Y 60
-#define PINDA_PREHEAT_Z 0.15
-/*
-#define PINDA_PREHEAT_X 70
-#define PINDA_PREHEAT_Y -3
-#define PINDA_PREHEAT_Z 1*/
-#define PINDA_HEAT_T 120 //time in s
-
-#define PINDA_MIN_T 50
-#define PINDA_STEP_T 10
-#define PINDA_MAX_T 100
-
-#define PING_TIME 60 //time in s
-#define PING_TIME_LONG 600 //10 min; used when length of commands buffer > 0 to avoid false triggering when dealing with long gcodes
-#define PING_ALLERT_PERIOD 60 //time in s
-
-#define NC_TIME 10 //time in s for periodic important status messages sending which needs reponse from monitoring
-#define NC_BUTTON_LONG_PRESS 15 //time in s
-
-#define LONG_PRESS_TIME 1000 //time in ms for button long press
-#define BUTTON_BLANKING_TIME 200 //time in ms for blanking after button release
-
-#define DEFAULT_PID_TEMP 210
-
-#define MIN_PRINT_FAN_SPEED 75
-
-#ifdef SNMM
-#define DEFAULT_RETRACTION 4 //used for PINDA temp calibration and pause print
-#else
-#define DEFAULT_RETRACTION 1 //used for PINDA temp calibration and pause print
-#endif
-
-// How much shall the print head be lifted on power panic?
-// Ideally the Z axis will reach a zero phase of the stepper driver on power outage. To simplify this,
-// UVLO_Z_AXIS_SHIFT shall be an integer multiply of the stepper driver cycle, that is 4x full step.
-// For example, the Prusa i3 MK2 with 16 microsteps per full step has Z stepping of 400 microsteps per mm.
-// At 400 microsteps per mm, a full step lifts the Z axis by 0.04mm, and a stepper driver cycle is 0.16mm.
-// The following example, 12 * (4 * 16 / 400) = 12 * 0.16mm = 1.92mm.
-//#define UVLO_Z_AXIS_SHIFT 1.92
-#define UVLO_Z_AXIS_SHIFT 0.64
-// If power panic occured, and the current temperature is higher then target temperature before interrupt minus this offset, print will be recovered automatically. 
-#define AUTOMATIC_UVLO_BED_TEMP_OFFSET 5 
-
-#define HEATBED_V2
-
-#define M600_TIMEOUT 600  //seconds
-
-//#define SUPPORT_VERBOSITY
-
-#endif //__CONFIGURATION_PRUSA_H

+ 59 - 26
Firmware/Marlin_main.cpp

@@ -1336,7 +1336,8 @@ void setup()
   }
   if (eeprom_read_byte((uint8_t*)EEPROM_WIZARD_ACTIVE) == 0) { //dont show calibration status messages if wizard is currently active
 	  if (calibration_status() == CALIBRATION_STATUS_ASSEMBLED ||
-		  calibration_status() == CALIBRATION_STATUS_UNKNOWN) {
+		  calibration_status() == CALIBRATION_STATUS_UNKNOWN || 
+		  calibration_status() == CALIBRATION_STATUS_XYZ_CALIBRATION) {
 		  // Reset the babystepping values, so the printer will not move the Z axis up when the babystepping is enabled.
 		  eeprom_update_word((uint16_t*)EEPROM_BABYSTEP_Z, 0);
 		  // Show the message.
@@ -1357,13 +1358,13 @@ void setup()
 	  }
   }
 
-#ifndef DEBUG_DISABLE_FORCE_SELFTEST
-  if (force_selftest_if_fw_version() && calibration_status() < CALIBRATION_STATUS_ASSEMBLED ) {
+#if !defined (DEBUG_DISABLE_FORCE_SELFTEST) && defined (TMC2130)
+  if (force_selftest_if_fw_version() && calibration_status() < CALIBRATION_STATUS_ASSEMBLED) {
 	  lcd_show_fullscreen_message_and_wait_P(MSG_FORCE_SELFTEST);
 	  update_current_firmware_version_to_eeprom();
 	  lcd_selftest();
   }
-#endif //DEBUG_DISABLE_FORCE_SELFTEST
+#endif //TMC2130 && !DEBUG_DISABLE_FORCE_SELFTEST
 
   KEEPALIVE_STATE(IN_PROCESS);
 #endif //DEBUG_DISABLE_STARTMSGS
@@ -2074,6 +2075,9 @@ void homeaxis(int axis, uint8_t cnt, uint8_t* pstep)
         feedrate = homing_feedrate[axis];
         plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
         st_synchronize();
+#ifdef TMC2130
+		if ((tmc2130_mode == TMC2130_MODE_NORMAL) && (READ(Z_TMC2130_DIAG) != 0)) return; //Z crash
+#endif //TMC2130
         current_position[axis] = 0;
         plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
         destination[axis] = -home_retract_mm(axis) * axis_home_dir;
@@ -2083,6 +2087,9 @@ void homeaxis(int axis, uint8_t cnt, uint8_t* pstep)
         feedrate = homing_feedrate[axis]/2 ;
         plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
         st_synchronize();
+#ifdef TMC2130
+		if ((tmc2130_mode == TMC2130_MODE_NORMAL) && (READ(Z_TMC2130_DIAG) != 0)) return; //Z crash
+#endif //TMC2130
         axis_is_at_home(axis);
         destination[axis] = current_position[axis];
         feedrate = 0.0;
@@ -3099,7 +3106,7 @@ void process_commands()
       			  } 
               // 1st mesh bed leveling measurement point, corrected.
               world2machine_initialize();
-              world2machine(pgm_read_float(bed_ref_points), pgm_read_float(bed_ref_points+1), destination[X_AXIS], destination[Y_AXIS]);
+              world2machine(pgm_read_float(bed_ref_points_4), pgm_read_float(bed_ref_points_4+1), destination[X_AXIS], destination[Y_AXIS]);
               world2machine_reset();
               if (destination[Y_AXIS] < Y_MIN_POS)
                   destination[Y_AXIS] = Y_MIN_POS;
@@ -3107,7 +3114,18 @@ void process_commands()
               feedrate = homing_feedrate[Z_AXIS]/10;
               current_position[Z_AXIS] = 0;
               enable_endstops(false);
+#ifdef DEBUG_BUILD
+              SERIAL_ECHOLNPGM("plan_set_position()");
+              MYSERIAL.println(current_position[X_AXIS]);MYSERIAL.println(current_position[Y_AXIS]);
+              MYSERIAL.println(current_position[Z_AXIS]);MYSERIAL.println(current_position[E_AXIS]);
+#endif
               plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+#ifdef DEBUG_BUILD
+              SERIAL_ECHOLNPGM("plan_buffer_line()");
+              MYSERIAL.println(destination[X_AXIS]);MYSERIAL.println(destination[Y_AXIS]);
+              MYSERIAL.println(destination[Z_AXIS]);MYSERIAL.println(destination[E_AXIS]);
+              MYSERIAL.println(feedrate);MYSERIAL.println(active_extruder);
+#endif
               plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
               st_synchronize();
               current_position[X_AXIS] = destination[X_AXIS];
@@ -3451,13 +3469,32 @@ void process_commands()
 			}
 			lcd_show_fullscreen_message_and_wait_P(MSG_TEMP_CAL_WARNING);
 			bool result = lcd_show_fullscreen_message_yes_no_and_wait_P(MSG_STEEL_SHEET_CHECK, false, false);
+			
 			if (result)
 			{
 				current_position[Z_AXIS] = 50;
-				current_position[Y_AXIS] = 190;
+				current_position[Y_AXIS] += 180;
 				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
 				st_synchronize();
 				lcd_show_fullscreen_message_and_wait_P(MSG_REMOVE_STEEL_SHEET);
+				current_position[Y_AXIS] -= 180;
+				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
+				st_synchronize();
+				feedrate = homing_feedrate[Z_AXIS] / 10;
+				enable_endstops(true);
+				endstops_hit_on_purpose();
+				homeaxis(Z_AXIS);
+				plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+				enable_endstops(false);
+			}
+			if ((current_temperature_pinda > 35) && (farm_mode == false)) {
+				//waiting for PIDNA probe to cool down in case that we are not in farm mode
+				current_position[Z_AXIS] = 100;
+				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
+				if (lcd_wait_for_pinda(35) == false) { //waiting for PINDA probe to cool, if this takes more then time expected, temp. cal. fails
+					lcd_temp_cal_show_result(false);
+					break;
+				}
 			}
 			lcd_update_enable(true);
 			KEEPALIVE_STATE(NOT_BUSY); //no need to print busy messages as we print current temperatures periodicaly
@@ -3500,7 +3537,9 @@ void process_commands()
 			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
 			st_synchronize();
 
-			find_bed_induction_sensor_point_z(-1.f);
+			bool find_z_result = find_bed_induction_sensor_point_z(-1.f);
+			if(find_z_result == false) lcd_temp_cal_show_result(find_z_result);
+
 			zero_z = current_position[Z_AXIS];
 
 			//current_position[Z_AXIS]
@@ -3549,7 +3588,9 @@ void process_commands()
 				current_position[Y_AXIS] = pgm_read_float(bed_ref_points + 1);
 				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
 				st_synchronize();
-				find_bed_induction_sensor_point_z(-1.f);
+				find_z_result = find_bed_induction_sensor_point_z(-1.f);
+				if (find_z_result == false) lcd_temp_cal_show_result(find_z_result);
+
 				z_shift = (int)((current_position[Z_AXIS] - zero_z)*axis_steps_per_unit[Z_AXIS]);
 
 				SERIAL_ECHOLNPGM("");
@@ -3562,25 +3603,8 @@ void process_commands()
 				EEPROM_save_B(EEPROM_PROBE_TEMP_SHIFT + i * 2, &z_shift);
 
 			}
-			custom_message_type = 0;
-			custom_message = false;
+			lcd_temp_cal_show_result(true);
 
-			eeprom_update_byte((uint8_t*)EEPROM_CALIBRATION_STATUS_PINDA, 1);
-			SERIAL_ECHOLNPGM("Temperature calibration done. Continue with pressing the knob.");
-			disable_x();
-			disable_y();
-			disable_z();
-			disable_e0();
-			disable_e1();
-			disable_e2();
-			setTargetBed(0); //set bed target temperature back to 0
-//			setTargetHotend(0,0); //set hotend target temperature back to 0
-			lcd_show_fullscreen_message_and_wait_P(MSG_TEMP_CALIBRATION_DONE);
-			temp_cal_active = true;
-			eeprom_update_byte((unsigned char *)EEPROM_TEMP_CAL_ACTIVE, 1);
-
-			lcd_update_enable(true);
-			lcd_update(2);
 			break;
 		}
 #endif //PINDA_THERMISTOR
@@ -3764,6 +3788,15 @@ void process_commands()
 #endif //MK1BP
 	case_G80:
 	{
+#ifdef TMC2130
+		//previously enqueued "G28 W0" failed (crash Z)
+		if (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] && !axis_known_position[Z_AXIS] && (READ(Z_TMC2130_DIAG) != 0))
+		{
+			kill(MSG_BED_LEVELING_FAILED_POINT_LOW);
+			break;
+		}
+#endif //TMC2130
+
 		mesh_bed_leveling_flag = true;
 		int8_t verbosity_level = 0;
 		static bool run = false;

+ 1 - 1
Firmware/MenuStack.cpp

@@ -9,7 +9,7 @@
  * @param menu
  * @param position selected position in menu being pushed
  */
-void MenuStack::push(menuFunc_t menu, uint8_t position)
+void MenuStack::push(menuFunc_t menu, int8_t position)
 {
     if (m_index >= max_depth) return;
     m_stack[m_index].menu = menu;

+ 2 - 2
Firmware/MenuStack.h

@@ -19,10 +19,10 @@ public:
     struct Record
     {
         menuFunc_t menu;
-        uint8_t position;
+        int8_t position;
     };
     MenuStack():m_stack(),m_index(0) {}
-    void push(menuFunc_t menu, uint8_t position);
+    void push(menuFunc_t menu, int8_t position);
     Record pop();
     void reset(){m_index = 0;}
 private:

+ 6 - 0
Firmware/Timer.cpp

@@ -6,6 +6,12 @@
 #include "Timer.h"
 #include "Arduino.h"
 
+/**
+ * @brief construct Timer
+ *
+ * It is guaranteed, that construction is equivalent with zeroing all members.
+ * This property can be exploited in MenuData union.
+ */
 Timer::Timer() : m_isRunning(false), m_started()
 {
 }

+ 20 - 6
Firmware/fsensor.cpp

@@ -63,7 +63,7 @@ bool fsensor_enable()
 {
 //	puts_P(PSTR("fsensor_enable\n"));
 	int pat9125 = pat9125_init();
-//    printf_P(PSTR("PAT9125_init:%d\n"), pat9125);
+    printf_P(PSTR("PAT9125_init:%d\n"), pat9125);
 	if (pat9125)
 		fsensor_not_responding = false;
 	else
@@ -74,6 +74,7 @@ bool fsensor_enable()
 	fsensor_err_cnt = 0;
 	eeprom_update_byte((uint8_t*)EEPROM_FSENSOR, fsensor_enabled?0x01:0x00); 
 	FSensorStateMenu = fsensor_enabled?1:0;
+//	printf_P(PSTR("fsensor_enable - end %d\n"), fsensor_enabled?1:0);
 	return fsensor_enabled;
 }
 
@@ -108,7 +109,14 @@ void fsensor_setup_interrupt()
 void fsensor_autoload_check_start(void)
 {
 //	puts_P(PSTR("fsensor_autoload_check_start\n"));
-	pat9125_update_y(); //update sensor
+	if (!pat9125_update_y()) //update sensor
+	{
+		puts_P(PSTR("pat9125 not responding (3).\n"));
+		fsensor_disable();
+		fsensor_not_responding = true;
+		fsensor_autoload_enabled = false;
+		return;
+	}
 	fsensor_autoload_y = pat9125_y; //save current y value
 	fsensor_autoload_c = 0; //reset number of changes counter
 	fsensor_autoload_sum = 0;
@@ -130,7 +138,13 @@ bool fsensor_check_autoload(void)
 	uint8_t fsensor_autoload_c_old = fsensor_autoload_c;
 	if ((millis() - fsensor_autoload_last_millis) < 25) return false;
 	fsensor_autoload_last_millis = millis();
-	pat9125_update_y(); //update sensor
+	if (!pat9125_update_y())
+	{
+		puts_P(PSTR("pat9125 not responding (2).\n"));
+		fsensor_disable();
+		fsensor_not_responding = true;
+		return false; //update sensor
+	}
 	int16_t dy = fsensor_autoload_y - pat9125_y;
 	if (dy) //? y value is different
 	{
@@ -170,9 +184,9 @@ ISR(PCINT2_vect)
 	*digitalPinToPCMSK(fsensor_int_pin) |= bit(digitalPinToPCMSKbit(fsensor_int_pin));*/
 	if (!pat9125_update_y())
 	{
-#ifdef DEBUG_FSENSOR_LOG
-		puts_P(PSTR("pat9125 not responding.\n"));
-#endif //DEBUG_FSENSOR_LOG
+//#ifdef DEBUG_FSENSOR_LOG
+		puts_P(PSTR("pat9125 not responding (1).\n"));
+//#endif //DEBUG_FSENSOR_LOG
 		fsensor_disable();
 		fsensor_not_responding = true;
 	}

+ 16 - 2
Firmware/language_all.cpp

@@ -2197,8 +2197,8 @@ const char * const MSG_TEMP_CALIBRATION_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_TEMP_CALIBRATION_CZ
 };
 
-const char MSG_TEMP_CALIBRATION_DONE_EN[] PROGMEM = "Temperature calibration is finished. Click to continue.";
-const char MSG_TEMP_CALIBRATION_DONE_CZ[] PROGMEM = "Teplotni kalibrace dokoncena. Pokracujte stiskem tlacitka.";
+const char MSG_TEMP_CALIBRATION_DONE_EN[] PROGMEM = "Temperature calibration is finished and active. Temp. calibration can be disabled in menu Settings->Temp. cal.";
+const char MSG_TEMP_CALIBRATION_DONE_CZ[] PROGMEM = "Teplotni kalibrace dokoncena a je nyni aktivni. Teplotni kalibraci je mozno deaktivovat v menu Nastaveni->Tepl. kal.";
 const char * const MSG_TEMP_CALIBRATION_DONE_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_TEMP_CALIBRATION_DONE_EN,
 	MSG_TEMP_CALIBRATION_DONE_CZ
@@ -2218,6 +2218,13 @@ const char * const MSG_TEMP_CALIBRATION_ON_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_TEMP_CALIBRATION_ON_CZ
 };
 
+const char MSG_TEMP_CAL_FAILED_EN[] PROGMEM = "Temperature calibration failed";
+const char MSG_TEMP_CAL_FAILED_CZ[] PROGMEM = "Teplotni kalibrace selhala";
+const char * const MSG_TEMP_CAL_FAILED_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_TEMP_CAL_FAILED_EN,
+	MSG_TEMP_CAL_FAILED_CZ
+};
+
 const char MSG_TEMP_CAL_WARNING_EN[] PROGMEM = "Stable ambient temperature 21-26C is needed a rigid stand is required.";
 const char * const MSG_TEMP_CAL_WARNING_LANG_TABLE[1] PROGMEM = {
 	MSG_TEMP_CAL_WARNING_EN
@@ -2354,6 +2361,13 @@ const char * const MSG_WAITING_TEMP_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_WAITING_TEMP_CZ
 };
 
+const char MSG_WAITING_TEMP_PINDA_EN[] PROGMEM = "Waiting for PINDA probe cooling";
+const char MSG_WAITING_TEMP_PINDA_CZ[] PROGMEM = "Cekani na zchladnuti PINDA";
+const char * const MSG_WAITING_TEMP_PINDA_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_WAITING_TEMP_PINDA_EN,
+	MSG_WAITING_TEMP_PINDA_CZ
+};
+
 const char MSG_WATCH_EN[] PROGMEM = "Info screen";
 const char MSG_WATCH_CZ[] PROGMEM = "Informace";
 const char * const MSG_WATCH_LANG_TABLE[LANG_NUM] PROGMEM = {

+ 4 - 0
Firmware/language_all.h

@@ -726,6 +726,8 @@ extern const char* const MSG_TEMP_CALIBRATION_OFF_LANG_TABLE[LANG_NUM];
 #define MSG_TEMP_CALIBRATION_OFF LANG_TABLE_SELECT(MSG_TEMP_CALIBRATION_OFF_LANG_TABLE)
 extern const char* const MSG_TEMP_CALIBRATION_ON_LANG_TABLE[LANG_NUM];
 #define MSG_TEMP_CALIBRATION_ON LANG_TABLE_SELECT(MSG_TEMP_CALIBRATION_ON_LANG_TABLE)
+extern const char* const MSG_TEMP_CAL_FAILED_LANG_TABLE[LANG_NUM];
+#define MSG_TEMP_CAL_FAILED LANG_TABLE_SELECT(MSG_TEMP_CAL_FAILED_LANG_TABLE)
 extern const char* const MSG_TEMP_CAL_WARNING_LANG_TABLE[1];
 #define MSG_TEMP_CAL_WARNING LANG_TABLE_SELECT_EXPLICIT(MSG_TEMP_CAL_WARNING_LANG_TABLE, 0)
 extern const char* const MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_OFF_LANG_TABLE[1];
@@ -770,6 +772,8 @@ extern const char* const MSG_VTRAV_MIN_LANG_TABLE[1];
 #define MSG_VTRAV_MIN LANG_TABLE_SELECT_EXPLICIT(MSG_VTRAV_MIN_LANG_TABLE, 0)
 extern const char* const MSG_WAITING_TEMP_LANG_TABLE[LANG_NUM];
 #define MSG_WAITING_TEMP LANG_TABLE_SELECT(MSG_WAITING_TEMP_LANG_TABLE)
+extern const char* const MSG_WAITING_TEMP_PINDA_LANG_TABLE[LANG_NUM];
+#define MSG_WAITING_TEMP_PINDA LANG_TABLE_SELECT(MSG_WAITING_TEMP_PINDA_LANG_TABLE)
 extern const char* const MSG_WATCH_LANG_TABLE[LANG_NUM];
 #define MSG_WATCH LANG_TABLE_SELECT(MSG_WATCH_LANG_TABLE)
 extern const char* const MSG_WATCHDOG_RESET_LANG_TABLE[1];

+ 3 - 1
Firmware/language_cz.h

@@ -303,7 +303,7 @@
 #define MSG_PINDA_NOT_CALIBRATED						"Tiskarna nebyla teplotne zkalibrovana"
 #define MSG_PINDA_PREHEAT								"Nahrivani PINDA"
 #define MSG_TEMP_CALIBRATION							"Tepl. kal.          "
-#define MSG_TEMP_CALIBRATION_DONE						"Teplotni kalibrace dokoncena. Pokracujte stiskem tlacitka."
+#define MSG_TEMP_CALIBRATION_DONE						"Teplotni kalibrace dokoncena a je nyni aktivni. Teplotni kalibraci je mozno deaktivovat v menu Nastaveni->Tepl. kal."
 #define MSG_TEMP_CALIBRATION_ON							"Tepl. kal.  [zap]"
 #define MSG_TEMP_CALIBRATION_OFF						"Tepl. kal.  [vyp]"
 #define MSG_PREPARE_FILAMENT							"Pripravte filament"
@@ -414,3 +414,5 @@
 #define MSG_CHANGED_MOTHERBOARD		 "Varovani: doslo ke zmene typu motherboardu."
 #define MSG_CHANGED_PRINTER			 "Varovani: doslo ke zmene typu tiskarny."
 #define MSG_CHANGED_BOTH			 "Varovani: doslo ke zmene typu tiskarny a motherboardu."
+#define MSG_WAITING_TEMP_PINDA		 "Cekani na zchladnuti PINDA"
+#define MSG_TEMP_CAL_FAILED			 "Teplotni kalibrace selhala"

+ 3 - 1
Firmware/language_en.h

@@ -303,7 +303,7 @@
 #define(length=20, lines=4) MSG_PINDA_NOT_CALIBRATED			"Temperature calibration has not been run yet"
 #define(length=20, lines=1) MSG_PINDA_PREHEAT					"PINDA Heating"
 #define(length=20, lines=1) MSG_TEMP_CALIBRATION				"Temp. cal.          "
-#define(length=20, lines=4) MSG_TEMP_CALIBRATION_DONE			"Temperature calibration is finished. Click to continue."
+#define(length=20, lines=12) MSG_TEMP_CALIBRATION_DONE			"Temperature calibration is finished and active. Temp. calibration can be disabled in menu Settings->Temp. cal."
 #define(length=20, lines=1) MSG_TEMP_CALIBRATION_ON				"Temp. cal.   [on]"
 #define(length=20, lines=1) MSG_TEMP_CALIBRATION_OFF			"Temp. cal.  [off]"
 #define(length=20, lines=1) MSG_PREPARE_FILAMENT				"Prepare new filament"
@@ -422,3 +422,5 @@
 #define(length=20, lines=4) MSG_CHANGED_MOTHERBOARD		 "Warning: motherboard type changed."
 #define(length=20, lines=4) MSG_CHANGED_PRINTER			 "Warning: printer type changed."
 #define(length=20, lines=4) MSG_CHANGED_BOTH			 "Warning: both printer type and motherboard type changed."
+#define(length=20, lines=3) MSG_WAITING_TEMP_PINDA		 "Waiting for PINDA probe cooling"
+#define(length=20, lines=8) MSG_TEMP_CAL_FAILED			 "Temperature calibration failed"

+ 104 - 29
Firmware/mesh_bed_calibration.cpp

@@ -7,6 +7,10 @@
 #include "stepper.h"
 #include "ultralcd.h"
 
+#ifdef TMC2130
+#include "tmc2130.h"
+#endif //TMC2130
+
 uint8_t world2machine_correction_mode;
 float   world2machine_rotation_and_skew[2][2];
 float   world2machine_rotation_and_skew_inv[2][2];
@@ -20,7 +24,7 @@ float   world2machine_shift[2];
 #define WEIGHT_FIRST_ROW_Y_LOW  (0.0f)
 
 #define BED_ZERO_REF_X (- 22.f + X_PROBE_OFFSET_FROM_EXTRUDER) // -22 + 23 = 1
-#define BED_ZERO_REF_Y (- 0.6f + Y_PROBE_OFFSET_FROM_EXTRUDER) // -0.6 + 5 = 4.4
+#define BED_ZERO_REF_Y (- 0.6f + Y_PROBE_OFFSET_FROM_EXTRUDER + 4.f) // -0.6 + 5 + 4 = 8.4
 
 // Scaling of the real machine axes against the programmed dimensions in the firmware.
 // The correction is tiny, here around 0.5mm on 250mm length.
@@ -56,10 +60,10 @@ const float bed_skew_angle_extreme = (0.25f * M_PI / 180.f);
 // 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 = {
-	13.f - BED_ZERO_REF_X,   10.4f - 4.f - BED_ZERO_REF_Y,
-	221.f - BED_ZERO_REF_X,  10.4f - 4.f - BED_ZERO_REF_Y,
-	221.f - BED_ZERO_REF_X, 202.4f - 4.f - BED_ZERO_REF_Y,
-	13.f - BED_ZERO_REF_X, 202.4f - 4.f - BED_ZERO_REF_Y
+	13.f - BED_ZERO_REF_X,   10.4f - BED_ZERO_REF_Y,
+	221.f - BED_ZERO_REF_X,  10.4f - BED_ZERO_REF_Y,
+	221.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
+	13.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y
 };
 
 const float bed_ref_points[] PROGMEM = {
@@ -159,22 +163,29 @@ static inline float point_weight_y(const uint8_t i, const uint8_t npts, const fl
     }
     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.
+/**
+ * @brief Calculate machine skew and offset
+ *
+ * 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.
+ * @param measured_pts Matrix of 2D points (maximum 18 floats)
+ * @param npts Number of points (maximum 9)
+ * @param true_pts
+ * @param [out] vec_x Resulting correction matrix. X axis vector
+ * @param [out] vec_y Resulting correction matrix. Y axis vector
+ * @param [out] cntr  Resulting correction matrix. [0;0] pont offset
+ * @param verbosity_level
+ * @return BedSkewOffsetDetectionResultType
+ */
 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
     )
 {
@@ -649,6 +660,9 @@ BedSkewOffsetDetectionResultType calculate_machine_skew_and_offset_LS(
     return result;
 }
 
+/**
+ * @brief Erase calibration data stored in EEPROM
+ */
 void reset_bed_offset_and_skew()
 {
     eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+0), 0x0FFFFFFFF);
@@ -703,6 +717,12 @@ static void world2machine_update(const float vec_x[2], const float vec_y[2], con
     }
 }
 
+/**
+ * @brief Set calibration matrix to identity
+ *
+ * In contrast with world2machine_revert_to_uncorrected(), it doesn't wait for finishing moves
+ * nor updates the current position with the absolute values.
+ */
 void world2machine_reset()
 {
     const float vx[] = { 1.f, 0.f };
@@ -711,15 +731,31 @@ void world2machine_reset()
     world2machine_update(vx, vy, cntr);
 }
 
+/**
+ * @brief Set calibration matrix to default value
+ *
+ * This is used if no valid calibration data can be read from EEPROM.
+ */
+static void world2machine_default()
+{
+#ifdef DEFAULT_Y_OFFSET
+    const float vx[] = { 1.f, 0.f };
+    const float vy[] = { 0.f, 1.f };
+    const float cntr[] = { 0.f, DEFAULT_Y_OFFSET };
+    world2machine_update(vx, vy, cntr);
+#else
+    world2machine_reset();
+#endif
+}
+/**
+ * @brief Set calibration matrix to identity and update current position with absolute position
+ *
+ * Wait for the motors to stop and then update the current position with the absolute values.
+ */
 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.
+        world2machine_reset();
         st_synchronize();
         current_position[X_AXIS] = st_get_position_mm(X_AXIS);
         current_position[Y_AXIS] = st_get_position_mm(Y_AXIS);
@@ -732,6 +768,15 @@ static inline bool vec_undef(const float v[2])
     return vx[0] == 0x0FFFFFFFF || vx[1] == 0x0FFFFFFFF;
 }
 
+/**
+ * @brief Read and apply calibration data from EEPROM
+ *
+ * If no calibration data has been stored in EEPROM or invalid,
+ * world2machine_default() is used.
+ *
+ * If stored calibration data is invalid, EEPROM storage is cleared.
+ *
+ */
 void world2machine_initialize()
 {
     //SERIAL_ECHOLNPGM("world2machine_initialize");
@@ -789,7 +834,7 @@ void world2machine_initialize()
     if (reset) {
 //        SERIAL_ECHOLNPGM("Invalid bed correction matrix. Resetting to identity.");
         reset_bed_offset_and_skew();
-        world2machine_reset();
+        world2machine_default();
     } else {
         world2machine_update(vec_x, vec_y, cntr);
         /*
@@ -810,10 +855,14 @@ void world2machine_initialize()
     }
 }
 
-// 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].
+/**
+ * @brief Update current position after switching to corrected coordinates
+ *
+ * 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];
@@ -872,8 +921,11 @@ inline bool find_bed_induction_sensor_point_z(float minimum_z, uint8_t n_iter, i
     update_current_position_z();
     if (! endstop_z_hit_on_purpose())
         goto error;
-
-    for (uint8_t i = 0; i < n_iter; ++ i) {
+#ifdef TMC2130
+	if ((tmc2130_mode == TMC2130_MODE_NORMAL) && (READ(Z_TMC2130_DIAG) != 0)) goto error; //crash Z detected
+#endif //TMC2130
+    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);
@@ -884,10 +936,16 @@ inline bool find_bed_induction_sensor_point_z(float minimum_z, uint8_t n_iter, i
         update_current_position_z();
         if (! endstop_z_hit_on_purpose())
             goto error;
+#ifdef TMC2130
+		if ((tmc2130_mode == TMC2130_MODE_NORMAL) && (READ(Z_TMC2130_DIAG) != 0)) goto error; //crash Z detected
+#endif //TMC2130
 //        SERIAL_ECHOPGM("Bed find_bed_induction_sensor_point_z low, height: ");
 //        MYSERIAL.print(current_position[Z_AXIS], 5);
 //        SERIAL_ECHOLNPGM("");
+		float dz = i?abs(current_position[Z_AXIS] - (z / i)):0;
         z += current_position[Z_AXIS];
+//		printf_P(PSTR(" Z[%d] = %d, dz=%d\n"), i, (int)(current_position[Z_AXIS] * 1000), (int)(dz * 1000));
+		if (dz > 0.05) goto error;//deviation > 50um
     }
     current_position[Z_AXIS] = z;
     if (n_iter > 1)
@@ -2675,8 +2733,21 @@ bool sample_mesh_and_store_reference()
         memcpy(destination, current_position, sizeof(destination));
         enable_endstops(true);
         homeaxis(Z_AXIS);
+
+#ifdef TMC2130
+		if (!axis_known_position[Z_AXIS] && (READ(Z_TMC2130_DIAG) != 0)) //Z crash
+		{
+			kill(MSG_BED_LEVELING_FAILED_POINT_LOW);
+			return false;
+		}
+#endif //TMC2130
+
         enable_endstops(false);
-        find_bed_induction_sensor_point_z();
+		if (!find_bed_induction_sensor_point_z()) //Z crash or deviation > 50um
+		{
+			kill(MSG_BED_LEVELING_FAILED_POINT_LOW);
+			return false;
+		}
         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) {
@@ -2694,7 +2765,11 @@ bool sample_mesh_and_store_reference()
         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();
+		if (!find_bed_induction_sensor_point_z()) //Z crash or deviation > 50um
+		{
+			kill(MSG_BED_LEVELING_FAILED_POINT_LOW);
+			return false;
+		}
         // 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;

+ 20 - 27
Firmware/mesh_bed_calibration.h

@@ -5,6 +5,7 @@
 // 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;
+extern const float bed_ref_points_4[] PROGMEM;
 
 extern const float bed_skew_angle_mild;
 extern const float bed_skew_angle_extreme;
@@ -37,26 +38,6 @@ extern void world2machine_initialize();
 // 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) {
@@ -77,6 +58,13 @@ inline void world2machine(float &x, float &y)
 	}
 }
 
+inline void world2machine(const float &x, const float &y, float &out_x, float &out_y)
+{
+    out_x = x;
+    out_y = y;
+    world2machine(out_x, out_y);
+}
+
 inline void machine2world(float x, float y, float &out_x, float &out_y)
 {
 	if (world2machine_correction_mode == WORLD2MACHINE_CORRECTION_NONE) {
@@ -147,17 +135,22 @@ extern bool find_bed_induction_sensor_point_z(float minimum_z = -10.f, uint8_t n
 extern bool find_bed_induction_sensor_point_xy(int verbosity_level = 0);
 extern void go_home_with_z_lift();
 
-// Positive or zero: ok
-// Negative: failed
+/**
+ * @brief Bed skew and offest detection result
+ *
+ * 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,
+	BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND   = -1, //!< Point not found.
+	BED_SKEW_OFFSET_DETECTION_FITTING_FAILED    = -2, //!< Fitting failed
 	
 	// Detection finished with success.
-	BED_SKEW_OFFSET_DETECTION_PERFECT 			= 0,
-	BED_SKEW_OFFSET_DETECTION_SKEW_MILD			= 1,
-	BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME		= 2
+	BED_SKEW_OFFSET_DETECTION_PERFECT 			= 0,  //!< Perfect.
+	BED_SKEW_OFFSET_DETECTION_SKEW_MILD			= 1,  //!< Mildly skewed.
+	BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME		= 2   //!< Extremely skewed.
 };
 
 extern BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level, uint8_t &too_far_mask);

+ 30 - 1
Firmware/pat9125.cpp

@@ -92,7 +92,10 @@ int pat9125_init()
 //	pat9125_PID2 = 0x91;
 	if ((pat9125_PID1 != 0x31) || (pat9125_PID2 != 0x91))
 	{
-		return 0;
+		pat9125_PID1 = pat9125_rd_reg(PAT9125_PID1);
+		pat9125_PID2 = pat9125_rd_reg(PAT9125_PID2);
+		if ((pat9125_PID1 != 0x31) || (pat9125_PID2 != 0x91))
+			return 0;
 	}
 	// Switch to bank0, not allowed to perform OTS_RegWriteRead.
 	pat9125_wr_reg(PAT9125_BANK_SELECTION, 0);
@@ -132,6 +135,9 @@ int pat9125_init()
 	pat9125_wr_reg(PAT9125_BANK_SELECTION, 0x00);
 	// Enable write protect.
 	pat9125_wr_reg(PAT9125_WP, 0x00);
+
+	pat9125_PID1 = pat9125_rd_reg(PAT9125_PID1);
+	pat9125_PID2 = pat9125_rd_reg(PAT9125_PID2);
 	return 1;
 }
 
@@ -142,11 +148,13 @@ int pat9125_update()
 		unsigned char ucMotion = pat9125_rd_reg(PAT9125_MOTION);
 		pat9125_b = pat9125_rd_reg(PAT9125_FRAME);
 		pat9125_s = pat9125_rd_reg(PAT9125_SHUTTER);
+		if (pat9125_PID1 == 0xff) return 0;
 		if (ucMotion & 0x80)
 		{
 			unsigned char ucXL = pat9125_rd_reg(PAT9125_DELTA_XL);
 			unsigned char ucYL = pat9125_rd_reg(PAT9125_DELTA_YL);
 			unsigned char ucXYH = pat9125_rd_reg(PAT9125_DELTA_XYH);
+			if (pat9125_PID1 == 0xff) return 0;
 			int iDX = ucXL | ((ucXYH << 4) & 0xf00);
 			int iDY = ucYL | ((ucXYH << 8) & 0xf00);
 			if (iDX & 0x800) iDX -= 4096;
@@ -164,10 +172,12 @@ int pat9125_update_y()
 	if ((pat9125_PID1 == 0x31) && (pat9125_PID2 == 0x91))
 	{
 		unsigned char ucMotion = pat9125_rd_reg(PAT9125_MOTION);
+		if (pat9125_PID1 == 0xff) return 0;
 		if (ucMotion & 0x80)
 		{
 			unsigned char ucYL = pat9125_rd_reg(PAT9125_DELTA_YL);
 			unsigned char ucXYH = pat9125_rd_reg(PAT9125_DELTA_XYH);
+			if (pat9125_PID1 == 0xff) return 0;
 			int iDY = ucYL | ((ucXYH << 8) & 0xf00);
 			if (iDY & 0x800) iDY -= 4096;
 			pat9125_y -= iDY; //negative number, because direction switching does not work
@@ -179,6 +189,7 @@ int pat9125_update_y()
 
 unsigned char pat9125_rd_reg(unsigned char addr)
 {
+//    printf_P(PSTR("pat9125_rd_reg 0x%hhx "), addr);
 	unsigned char data = 0;
 #ifdef PAT9125_SWSPI
 	swspi_start();
@@ -188,6 +199,14 @@ unsigned char pat9125_rd_reg(unsigned char addr)
 #endif //PAT9125_SWSPI
 #ifdef PAT9125_SWI2C
 	int iret = swi2c_readByte_A8(PAT9125_I2C_ADDR, addr, &data);
+	if (!iret) //NO ACK error
+	{
+		pat9125_PID1 = 0xff;
+		pat9125_PID2 = 0xff;
+//	    printf_P(PSTR("ERR\n"));
+		return 0;
+	}
+//    printf_P(PSTR("0x%hhx OK\n"), data);
 #endif //PAT9125_SWI2C
 #ifdef PAT9125_HWI2C
 	Wire.beginTransmission(PAT9125_I2C_ADDR);
@@ -202,6 +221,7 @@ unsigned char pat9125_rd_reg(unsigned char addr)
 
 void pat9125_wr_reg(unsigned char addr, unsigned char data)
 {
+//    printf_P(PSTR("pat9125_wr_reg 0x%hhx 0x%hhx  "), addr, data);
 #ifdef PAT9125_SWSPI
 	swspi_start();
 	swspi_tx(addr | 0x80);
@@ -210,6 +230,15 @@ void pat9125_wr_reg(unsigned char addr, unsigned char data)
 #endif //PAT9125_SWSPI
 #ifdef PAT9125_SWI2C
 	int iret = swi2c_writeByte_A8(PAT9125_I2C_ADDR, addr, &data);
+	if (!iret) //NO ACK error
+	{
+		pat9125_PID1 = 0xff;
+		pat9125_PID2 = 0xff;
+//	    printf_P(PSTR("ERR\n"));
+		return;
+	}
+//    printf_P(PSTR("OK\n"));
+
 #endif //PAT9125_SWI2C
 #ifdef PAT9125_HWI2C
 	Wire.beginTransmission(PAT9125_I2C_ADDR);

+ 1 - 1
Firmware/temperature.cpp

@@ -985,7 +985,7 @@ static void updateTemperaturesFromRawValues()
     }
 
 #ifdef PINDA_THERMISTOR
-	current_temperature_pinda = analog2tempPINDA(current_temperature_raw_pinda);
+	current_temperature_pinda = analog2tempBed(current_temperature_raw_pinda);
 #endif
 
 #ifdef AMBIENT_THERMISTOR

+ 142 - 34
Firmware/ultralcd.cpp

@@ -9,6 +9,7 @@
 #include "stepper.h"
 #include "ConfigurationStore.h"
 #include <string.h>
+#include "Timer.h"
 
 #include "util.h"
 #include "mesh_bed_leveling.h"
@@ -107,6 +108,11 @@ union MenuData
     // editMenuParentState is used when an edit menu is entered, so it knows
     // the return menu and encoder state.
     struct EditMenuParentState editMenuParentState;
+
+    struct AutoLoadFilamentMenu
+    {
+        Timer timer;
+    } autoLoadFilamentMenu;
 };
 
 // State of the currently active menu.
@@ -187,6 +193,7 @@ unsigned char firstrun = 1;
 
 /** forward declarations **/
 
+static const char* lcd_display_message_fullscreen_nonBlocking_P(const char *msg, uint8_t &nlines);
 // void copy_and_scalePID_i();
 // void copy_and_scalePID_d();
 
@@ -1595,8 +1602,7 @@ static void lcd_menu_fails_stats_total()
 	if (lcd_clicked())
     {
         lcd_quick_feedback();
-        //lcd_return_to_status();
-		lcd_goto_menu(lcd_menu_fails_stats, 4);
+        menu_action_back();
     }
 }
 
@@ -1616,8 +1622,7 @@ static void lcd_menu_fails_stats_print()
 	if (lcd_clicked())
     {
         lcd_quick_feedback();
-        //lcd_return_to_status();
-		lcd_goto_menu(lcd_menu_fails_stats, 2);
+		menu_action_back();
     }    
 }
 /**
@@ -1752,10 +1757,10 @@ static void lcd_preheat_menu()
   MENU_ITEM(back, MSG_MAIN, 0);
 
   if (farm_mode) {
-	  MENU_ITEM(function, PSTR("farm    -  " STRINGIFY(FARM_PREHEAT_HOTEND_TEMP) "/" STRINGIFY(FARM_PREHEAT_HPB_TEMP)), lcd_preheat_farm);
-	  MENU_ITEM(function, PSTR("nozzle  -  " STRINGIFY(FARM_PREHEAT_HOTEND_TEMP) "/0"), lcd_preheat_farm_nozzle);
+	  MENU_ITEM(function, PSTR("farm   -  " STRINGIFY(FARM_PREHEAT_HOTEND_TEMP) "/" STRINGIFY(FARM_PREHEAT_HPB_TEMP)), lcd_preheat_farm);
+	  MENU_ITEM(function, PSTR("nozzle -  " STRINGIFY(FARM_PREHEAT_HOTEND_TEMP) "/0"), lcd_preheat_farm_nozzle);
 	  MENU_ITEM(function, MSG_COOLDOWN, lcd_cooldown);
-	  MENU_ITEM(function, PSTR("ABS     -  " STRINGIFY(ABS_PREHEAT_HOTEND_TEMP) "/" STRINGIFY(ABS_PREHEAT_HPB_TEMP)), lcd_preheat_abs);
+	  MENU_ITEM(function, PSTR("ABS    -  " STRINGIFY(ABS_PREHEAT_HOTEND_TEMP) "/" STRINGIFY(ABS_PREHEAT_HPB_TEMP)), lcd_preheat_abs);
   } else {
 	  MENU_ITEM(function, PSTR("PLA  -  " STRINGIFY(PLA_PREHEAT_HOTEND_TEMP) "/" STRINGIFY(PLA_PREHEAT_HPB_TEMP)), lcd_preheat_pla);
 	  MENU_ITEM(function, PSTR("PET  -  " STRINGIFY(PET_PREHEAT_HOTEND_TEMP) "/" STRINGIFY(PET_PREHEAT_HPB_TEMP)), lcd_preheat_pet);
@@ -1834,9 +1839,9 @@ static void lcd_support_menu()
     
   MENU_ITEM(submenu, MSG_MENU_TEMPERATURES, lcd_menu_temperatures);
 
-#if defined (VOLT_BED_PIN) || defined (VOLT_BED_PIN)
+#if defined (VOLT_BED_PIN) || defined (VOLT_PWR_PIN)
   MENU_ITEM(submenu, MSG_MENU_VOLTAGES, lcd_menu_voltages);
-#endif //defined VOLT_BED_PIN || defined VOLT_BED_PIN
+#endif //defined VOLT_BED_PIN || defined VOLT_PWR_PIN
 
 #ifdef DEBUG_BUILD
   MENU_ITEM(submenu, PSTR("Debug"), lcd_menu_debug);
@@ -1877,7 +1882,7 @@ void lcd_unLoadFilament()
     lcd_implementation_clear();
   }
 
-  lcd_return_to_status();
+  menu_action_back();
 }
 
 void lcd_change_filament() {
@@ -2070,39 +2075,50 @@ void lcd_alright() {
 
 }
 
+#ifdef PAT9125
+static void lcd_menu_AutoLoadFilament()
+{
+    if (degHotend0() > EXTRUDE_MINTEMP)
+    {
+        uint8_t nlines;
+        lcd_display_message_fullscreen_nonBlocking_P(MSG_AUTOLOADING_ENABLED,nlines);
+    }
+    else
+    {
+        if (!menuData.autoLoadFilamentMenu.timer.running()) menuData.autoLoadFilamentMenu.timer.start();
+        lcd.setCursor(0, 0);
+        lcd_printPGM(MSG_ERROR);
+        lcd.setCursor(0, 2);
+        lcd_printPGM(MSG_PREHEAT_NOZZLE);
+        if (menuData.autoLoadFilamentMenu.timer.expired(2000ul)) menu_action_back();
+    }
+    if (lcd_clicked()) menu_action_back();
+}
+#endif //PAT9125
 
-
-void lcd_LoadFilament()
+static void lcd_LoadFilament()
 {
-  if (degHotend0() > EXTRUDE_MINTEMP) 
+  if (degHotend0() > EXTRUDE_MINTEMP)
   {
-#ifdef PAT9125
-	  if (filament_autoload_enabled && fsensor_enabled)
-	  {
-		  lcd_show_fullscreen_message_and_wait_P(MSG_AUTOLOADING_ENABLED);
-		  return;
-	  }
-#endif //PAT9125
-	  custom_message = true;
-	  loading_flag = true;
-	  enquecommand_P(PSTR("M701")); //load filament
-	  SERIAL_ECHOLN("Loading filament");	    
+      custom_message = true;
+      loading_flag = true;
+      enquecommand_P(PSTR("M701")); //load filament
+      SERIAL_ECHOLN("Loading filament");
+      lcd_return_to_status();
   }
-  else 
+  else
   {
 
     lcd_implementation_clear();
     lcd.setCursor(0, 0);
     lcd_printPGM(MSG_ERROR);
     lcd.setCursor(0, 2);
-	lcd_printPGM(MSG_PREHEAT_NOZZLE);
+    lcd_printPGM(MSG_PREHEAT_NOZZLE);
     delay(2000);
     lcd_implementation_clear();
   }
-  lcd_return_to_status();
 }
 
-
 void lcd_menu_statistics()
 {
 
@@ -2602,6 +2618,35 @@ void lcd_adjust_z() {
 
 }
 
+bool lcd_wait_for_pinda(float temp) {
+	lcd_set_custom_characters_degree();
+	setTargetHotend(0, 0);
+	setTargetBed(0);
+	Timer pinda_timeout;
+	pinda_timeout.start();
+	bool target_temp_reached = true;
+
+	while (current_temperature_pinda > temp){
+		lcd_display_message_fullscreen_P(MSG_WAITING_TEMP_PINDA);
+
+		lcd.setCursor(0, 4);
+		lcd.print(LCD_STR_THERMOMETER[0]);
+		lcd.print(ftostr3(current_temperature_pinda));
+		lcd.print("/");
+		lcd.print(ftostr3(temp));
+		lcd.print(LCD_STR_DEGREE);
+		delay_keep_alive(1000);
+		serialecho_temperatures();
+		if (pinda_timeout.expired(8 * 60 * 1000ul)) { //PINDA cooling from 60 C to 35 C takes about 7 minutes
+			target_temp_reached = false;
+			break;
+		}
+	}
+	lcd_set_custom_characters_arrows();
+	lcd_update_enable(true);
+	return(target_temp_reached);
+}
+
 void lcd_wait_for_heater() {
 	lcd_display_message_fullscreen_P(MSG_WIZARD_HEATING);
 
@@ -2749,11 +2794,16 @@ static inline bool pgm_is_interpunction(const char *c_addr)
     return c == '.' || c == ',' || c == ':'|| c == ';' || c == '?' || c == '!' || c == '/';
 }
 
-const char* lcd_display_message_fullscreen_P(const char *msg, uint8_t &nlines)
+/**
+ * @brief show full screen message
+ *
+ * This function is non-blocking
+ * @param msg message to be displayed from PROGMEM
+ * @param nlines
+ * @return rest of the text (to be displayed on next page)
+ */
+static const char* lcd_display_message_fullscreen_nonBlocking_P(const char *msg, uint8_t &nlines)
 {
-    // Disable update of the screen by the usual lcd_update() routine. 
-    lcd_update_enable(false);
-    lcd_implementation_clear();
     lcd.setCursor(0, 0);
     const char *msgend = msg;
     uint8_t row = 0;
@@ -2806,6 +2856,21 @@ const char* lcd_display_message_fullscreen_P(const char *msg, uint8_t &nlines)
     return multi_screen ? msgend : NULL;
 }
 
+const char* lcd_display_message_fullscreen_P(const char *msg, uint8_t &nlines)
+{
+    // Disable update of the screen by the usual lcd_update() routine.
+    lcd_update_enable(false);
+    lcd_implementation_clear();
+    return lcd_display_message_fullscreen_nonBlocking_P(msg, nlines);
+}
+
+
+/**
+ * @brief show full screen message and wait
+ *
+ * This function is blocking.
+ * @param msg message to be displayed from PROGMEM
+ */
 void lcd_show_fullscreen_message_and_wait_P(const char *msg)
 {
     const char *msg_next = lcd_display_message_fullscreen_P(msg);
@@ -3046,6 +3111,36 @@ void lcd_bed_calibration_show_result(BedSkewOffsetDetectionResultType result, ui
     }
 }
 
+void lcd_temp_cal_show_result(bool result) {
+	
+	custom_message_type = 0;
+	custom_message = false;
+	disable_x();
+	disable_y();
+	disable_z();
+	disable_e0();
+	disable_e1();
+	disable_e2();
+	setTargetBed(0); //set bed target temperature back to 0
+
+	if (result == true) {
+		eeprom_update_byte((uint8_t*)EEPROM_CALIBRATION_STATUS_PINDA, 1);
+		SERIAL_ECHOLNPGM("Temperature calibration done. Continue with pressing the knob.");
+		lcd_show_fullscreen_message_and_wait_P(MSG_TEMP_CALIBRATION_DONE);
+		temp_cal_active = true;
+		eeprom_update_byte((unsigned char *)EEPROM_TEMP_CAL_ACTIVE, 1);
+	}
+	else {
+		eeprom_update_byte((uint8_t*)EEPROM_CALIBRATION_STATUS_PINDA, 0);
+		SERIAL_ECHOLNPGM("Temperature calibration failed. Continue with pressing the knob.");
+		lcd_show_fullscreen_message_and_wait_P(MSG_TEMP_CAL_FAILED);
+		temp_cal_active = false;
+		eeprom_update_byte((unsigned char *)EEPROM_TEMP_CAL_ACTIVE, 0);
+	}
+	lcd_update_enable(true);
+	lcd_update(2);
+}
+
 static void lcd_show_end_stops() {
 	lcd.setCursor(0, 0);
 	lcd_printPGM((PSTR("End stops diag")));
@@ -5617,11 +5712,11 @@ static void lcd_main_menu()
 	#ifndef SNMM
 #ifdef PAT9125
 	if ( ((filament_autoload_enabled == true) && (fsensor_enabled == true)))
-        MENU_ITEM(function, MSG_AUTOLOAD_FILAMENT, lcd_LoadFilament);
+        MENU_ITEM(submenu, MSG_AUTOLOAD_FILAMENT, lcd_menu_AutoLoadFilament);
 	else
 #endif //PAT9125
 		MENU_ITEM(function, MSG_LOAD_FILAMENT, lcd_LoadFilament);
-	MENU_ITEM(function, MSG_UNLOAD_FILAMENT, lcd_unLoadFilament);
+	MENU_ITEM(submenu, MSG_UNLOAD_FILAMENT, lcd_unLoadFilament);
 	#endif
 	#ifdef SNMM
 	MENU_ITEM(submenu, MSG_LOAD_FILAMENT, fil_load_menu);
@@ -6164,6 +6259,11 @@ bool lcd_selftest()
 	_result = lcd_selftest_fan_dialog(0);
 #else //defined(TACH_0)
 	_result = lcd_selftest_manual_fan_check(0, false);
+	if (!_result)
+	{
+		const char *_err;
+		lcd_selftest_error(7, _err, _err); //extruder fan not spinning
+	}
 #endif //defined(TACH_0)
 	
 
@@ -6174,6 +6274,12 @@ bool lcd_selftest()
 		_result = lcd_selftest_fan_dialog(1);
 #else //defined(TACH_1)
 		_result = lcd_selftest_manual_fan_check(1, false);
+		if (!_result)
+		{			
+			const char *_err;
+			lcd_selftest_error(6, _err, _err); //print fan not spinning
+		}
+
 #endif //defined(TACH_1)
 	}
 
@@ -6881,6 +6987,8 @@ static bool lcd_selftest_manual_fan_check(int _fan, bool check_opposite)
 
 	int8_t enc_dif = 0;
 	KEEPALIVE_STATE(PAUSED_FOR_USER);
+
+	button_pressed = false; 
 	do
 	{
 		switch (_fan)

+ 4 - 0
Firmware/ultralcd.h

@@ -280,6 +280,10 @@ void lcd_wait_for_cool_down();
 void adjust_bed_reset();
 void lcd_extr_cal_reset();
 
+void lcd_temp_cal_show_result(bool result);
+bool lcd_wait_for_pinda(float temp);
+
+
 union MenuData;
 
 void bowden_menu();

+ 1 - 1
Firmware/ultralcd_implementation_hitachi_HD44780.h

@@ -194,7 +194,7 @@ extern volatile uint16_t buttons;  //an extended version of the last checked but
     LCD_CLASS lcd(LCD_I2C_ADDRESS, LCD_WIDTH, LCD_HEIGHT);
     
 // 2 wire Non-latching LCD SR from:
-// https://bitbucket.org/fmalpartida/new-LiquidCrystal_Prusa/wiki/schematics#!shiftregister-connection 
+// https://bitbucket.org/fmalpartida/new-LiquidCrystal/wiki/schematics#!shiftregister-connection 
 #elif defined(SR_LCD_2W_NL)
 
   extern "C" void __cxa_pure_virtual() { while (1); }

+ 4 - 0
Firmware/util.cpp

@@ -260,6 +260,10 @@ bool force_selftest_if_fw_version()
 		else if (ver_with_calibration[i] < ver_eeprom[i])
 			break;
 	}
+
+	//force selftest also in case that version used before flashing new firmware was 3.2.0-RC1
+	if ((ver_eeprom[0] == 3) && (ver_eeprom[1] == 2) && (ver_eeprom[2] == 0) && (ver_eeprom[3] == 3)) force_selftest = true;
+	
 	return force_selftest;
 }
 

+ 4 - 2
Firmware/variants/1_75mm_MK3-EINSy10a-E3Dv6full.h

@@ -76,6 +76,8 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 #define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
 #define HOMING_FEEDRATE {3000, 3000, 800, 0}  // set the homing speeds (mm/min) // 3000 is also valid for stallGuard homing. Valid range: 2200 - 3000
 
+#define DEFAULT_Y_OFFSET    4.f // Offset of [0;0] point, when the printer is not calibrated
+
 #define DEFAULT_MAX_FEEDRATE          {200, 200, 12, 120}      // (mm/sec)   max feedrate (M203)
 #define DEFAULT_MAX_ACCELERATION      {1000, 1000, 200, 5000}  // (mm/sec^2) max acceleration (M201)
 
@@ -242,8 +244,8 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 #define TMC2130_SG_THRS_E       3     // stallguard sensitivity for E axis
 
 //new settings is possible for vsense = 1, running current value > 31 set vsense to zero and shift both currents by 1 bit right (Z axis only)
-#define TMC2130_CURRENTS_H {16, 20, 28, 36}  // default holding currents for all axes
-#define TMC2130_CURRENTS_R {16, 20, 28, 36}  // default running currents for all axes
+#define TMC2130_CURRENTS_H {16, 20, 35, 26}  // default holding currents for all axes
+#define TMC2130_CURRENTS_R {16, 20, 35, 26}  // default running currents for all axes
 #define TMC2130_UNLOAD_CURRENT_R 12			 // lowe current for M600 to protect filament sensor 
 
 #define TMC2130_STEALTH_Z

+ 5 - 1
Firmware/xyzcal.cpp

@@ -384,8 +384,12 @@ void xyzcal_adjust_pixels(uint8_t* pixels, uint16_t* histo)
 	for (l = 14; l > 8; l--)
 		if (histo[l] >= 10)
 			break;
-	uint8_t pix_min = (max_l << 4) / 2;
+	uint8_t pix_min = 0;
 	uint8_t pix_max = l << 4;
+	if (histo[0] < (32*32 - 144))
+	{
+		pix_min = (max_l << 4) / 2;
+	}
 	uint8_t pix_dif = pix_max - pix_min;
 	DBG(_n(" min=%d max=%d dif=%d\n"), pix_min, pix_max, pix_dif);
 	for (int16_t i = 0; i < 32*32; i++)