Merge pull request #606 from PavelSindler/MK25_selftest

version changed - final

Firmware/Configuration.h

@@ -7,12 +7,12 @@
 #define STR(x) STR_HELPER(x)
 
 // Firmware version
-#define FW_VERSION "3.1.3-RC1"
-#define FW_COMMIT_NR   276
+#define FW_VERSION "3.1.4"
+#define FW_COMMIT_NR   330
 // 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
-#define FW_REPOSITORY "Prusa3D/MK3"
+#define FW_REPOSITORY "Unknown"
 #define FW_VERSION_FULL FW_VERSION "-" STR(FW_COMMIT_NR)
 
 // Debug version has debugging enabled (the symbol DEBUG_BUILD is set).

Firmware/Configuration_prusa.h

@@ -170,8 +170,12 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 
 #define PAT9125 //!< Filament sensor
 #define FANCHECK
+
+// New XYZ calibration
+#define NEW_XYZCAL
+
 //#define WATCHDOG
-#define SAFETYTIMER
+//#define SAFETYTIMER
 
 
 /*------------------------------------

Firmware/Marlin_main.cpp

@@ -2252,6 +2252,8 @@ bool gcode_M45(bool onlyZ, int8_t verbosity_level)
 					// if (result >= 0) babystep_apply();					
 					#endif //HEATBED_V2
 				}
+				lcd_update_enable(true);
+				lcd_update(2);
 				
 				lcd_bed_calibration_show_result(result, point_too_far_mask);
 				if (result >= 0)
@@ -3229,7 +3231,8 @@ void process_commands()
 #ifdef PINDA_THERMISTOR
 		if (true)
 		{
-			if (!(axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] && axis_known_position[Z_AXIS])) {
+			if (!(axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] && axis_known_position[Z_AXIS]))
+			{
 				// We don't know where we are! HOME!
 				// Push the commands to the front of the message queue in the reverse order!
 				// There shall be always enough space reserved for these commands.
@@ -3239,7 +3242,14 @@ 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) lcd_show_fullscreen_message_and_wait_P(MSG_REMOVE_STEEL_SHEET);
+			if (result)
+			{
+				current_position[Z_AXIS] = 50;
+				current_position[Y_AXIS] = 190;
+				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);
+			}
 			lcd_update_enable(true);
 			KEEPALIVE_STATE(NOT_BUSY); //no need to print busy messages as we print current temperatures periodicaly
 			SERIAL_ECHOLNPGM("PINDA probe calibration start");

Firmware/config.h

@@ -8,5 +8,8 @@
 #define ADC_OVRSAMPL      16        //oversampling multiplier
 #define ADC_CALLBACK      adc_ready //callback function ()
 
+//SM4 configuration
+#define SM4_DEFDELAY      500       //default step delay [us]
+
 
 #endif //_CONFIG_H

Firmware/language_all.cpp

@@ -61,8 +61,10 @@ const char * const MSG_AUTO_HOME_LANG_TABLE[1] PROGMEM = {
 };
 
 const char MSG_AUTO_MODE_ON_EN[] PROGMEM = "Mode [auto power]";
-const char * const MSG_AUTO_MODE_ON_LANG_TABLE[1] PROGMEM = {
-	MSG_AUTO_MODE_ON_EN
+const char MSG_AUTO_MODE_ON_CZ[] PROGMEM = "Mod [automaticky]";
+const char * const MSG_AUTO_MODE_ON_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_AUTO_MODE_ON_EN,
+	MSG_AUTO_MODE_ON_CZ
 };
 
 const char MSG_A_RETRACT_EN[] PROGMEM = "A-retract";
@@ -1986,14 +1988,14 @@ const char * const MSG_SHOW_END_STOPS_LANG_TABLE[LANG_NUM] PROGMEM = {
 };
 
 const char MSG_SILENT_MODE_OFF_EN[] PROGMEM = "Mode [high power]";
-const char MSG_SILENT_MODE_OFF_CZ[] PROGMEM = "Mod      [Normal]";
+const char MSG_SILENT_MODE_OFF_CZ[] PROGMEM = "Mod  [vys. vykon]";
 const char * const MSG_SILENT_MODE_OFF_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_SILENT_MODE_OFF_EN,
 	MSG_SILENT_MODE_OFF_CZ
 };
 
 const char MSG_SILENT_MODE_ON_EN[] PROGMEM = "Mode     [silent]";
-const char MSG_SILENT_MODE_ON_CZ[] PROGMEM = "Mod     [Stealth]";
+const char MSG_SILENT_MODE_ON_CZ[] PROGMEM = "Mod       [tichy]";
 const char * const MSG_SILENT_MODE_ON_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_SILENT_MODE_ON_EN,
 	MSG_SILENT_MODE_ON_CZ

Firmware/language_all.h

@@ -40,8 +40,8 @@ extern const char* const MSG_AUTOLOAD_FILAMENT_LANG_TABLE[LANG_NUM];
 #define MSG_AUTOLOAD_FILAMENT LANG_TABLE_SELECT(MSG_AUTOLOAD_FILAMENT_LANG_TABLE)
 extern const char* const MSG_AUTO_HOME_LANG_TABLE[1];
 #define MSG_AUTO_HOME LANG_TABLE_SELECT_EXPLICIT(MSG_AUTO_HOME_LANG_TABLE, 0)
-extern const char* const MSG_AUTO_MODE_ON_LANG_TABLE[1];
-#define MSG_AUTO_MODE_ON LANG_TABLE_SELECT_EXPLICIT(MSG_AUTO_MODE_ON_LANG_TABLE, 0)
+extern const char* const MSG_AUTO_MODE_ON_LANG_TABLE[LANG_NUM];
+#define MSG_AUTO_MODE_ON LANG_TABLE_SELECT(MSG_AUTO_MODE_ON_LANG_TABLE)
 extern const char* const MSG_A_RETRACT_LANG_TABLE[1];
 #define MSG_A_RETRACT LANG_TABLE_SELECT_EXPLICIT(MSG_A_RETRACT_LANG_TABLE, 0)
 extern const char* const MSG_BABYSTEPPING_X_LANG_TABLE[1];

Firmware/language_cz.h

@@ -103,8 +103,9 @@
 #define MSG_INSERT_FILAMENT		"Vlozte filament"
 #define MSG_CHANGING_FILAMENT	"Vymena filamentu!"
 
-#define MSG_SILENT_MODE_ON					"Mod     [Stealth]"
-#define MSG_SILENT_MODE_OFF					"Mod      [Normal]" 
+#define MSG_SILENT_MODE_ON					"Mod       [tichy]"
+#define MSG_SILENT_MODE_OFF					"Mod  [vys. vykon]" 
+#define MSG_AUTO_MODE_ON					"Mod [automaticky]"
 #define MSG_REBOOT							"Restartujte tiskarnu"
 #define MSG_TAKE_EFFECT						" pro projeveni zmen"	
 

Firmware/mesh_bed_calibration.cpp

@@ -20,7 +20,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) // -0.6 + 5 = 4.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 +56,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 = {
@@ -905,6 +905,9 @@ error:
     return false;
 }
 
+#ifdef NEW_XYZCAL
+extern bool xyzcal_find_bed_induction_sensor_point_xy();
+#endif //NEW_XYZCAL
 // 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.
@@ -921,6 +924,9 @@ error:
 #ifdef HEATBED_V2
 inline bool find_bed_induction_sensor_point_xy(int verbosity_level)
 {
+#ifdef NEW_XYZCAL
+	return xyzcal_find_bed_induction_sensor_point_xy();
+#else //NEW_XYZCAL
 	#ifdef SUPPORT_VERBOSITY
 	if (verbosity_level >= 10) MYSERIAL.println("find bed induction sensor point xy");
 	#endif // SUPPORT_VERBOSITY
@@ -1163,11 +1169,14 @@ inline bool find_bed_induction_sensor_point_xy(int verbosity_level)
 	enable_z_endstop(false);
 	invert_z_endstop(false);
 	return found;
-
+#endif //NEW_XYZCAL
 }
 #else //HEATBED_V2
 inline bool find_bed_induction_sensor_point_xy(int verbosity_level)
 {
+#ifdef NEW_XYZCAL
+	return xyzcal_find_bed_induction_sensor_point_xy();
+#else //NEW_XYZCAL
 	#ifdef SUPPORT_VERBOSITY
 	if (verbosity_level >= 10) MYSERIAL.println("find bed induction sensor point xy");
 	#endif // SUPPORT_VERBOSITY
@@ -1360,10 +1369,13 @@ inline bool find_bed_induction_sensor_point_xy(int verbosity_level)
 
 	enable_z_endstop(false);
 	return found;
+#endif //NEW_XYZCAL
 }
 
 #endif //HEATBED_V2
 
+#ifndef NEW_XYZCAL
+
 // 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.
@@ -1453,7 +1465,9 @@ inline bool improve_bed_induction_sensor_point()
     enable_z_endstop(endstop_z_enabled);
     return found;
 }
+#endif //NEW_XYZCAL
 
+#ifndef NEW_XYZCAL
 static inline void debug_output_point(const char *type, const float &x, const float &y, const float &z)
 {
     SERIAL_ECHOPGM("Measured ");
@@ -1466,7 +1480,9 @@ static inline void debug_output_point(const char *type, const float &x, const fl
     MYSERIAL.print(z, 5);
     SERIAL_ECHOLNPGM("");
 }
+#endif //NEW_XYZCAL
 
+#ifndef NEW_XYZCAL
 // 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.
@@ -1626,7 +1642,9 @@ canceled:
     go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
     return false;
 }
+#endif //NEW_XYZCAL
 
+#ifndef NEW_XYZCAL
 // 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.
@@ -1947,7 +1965,9 @@ canceled:
     go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
     return false;
 }
+#endif //NEW_XYZCAL
 
+#ifndef NEW_XYZCAL
 // 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.
@@ -1992,6 +2012,7 @@ inline void scan_bed_induction_sensor_point()
     current_position[Y_AXIS] = center_old_y;
     go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
 }
+#endif //NEW_XYZCAL
 
 #define MESH_BED_CALIBRATION_SHOW_LCD
 
@@ -2014,7 +2035,11 @@ BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level
 //    SERIAL_ECHO(int(verbosity_level));
 //    SERIAL_ECHOPGM("");
 	
+#ifdef NEW_XYZCAL
+	{
+#else //NEW_XYZCAL
 	while (iteration < 3) {
+#endif //NEW_XYZCAL
 
 		SERIAL_ECHOPGM("Iteration: ");
 		MYSERIAL.println(int(iteration + 1));
@@ -2118,6 +2143,7 @@ BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level
 		#endif // SUPPORT_VERBOSITY
 		if (!find_bed_induction_sensor_point_xy(verbosity_level))
 			return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
+#ifndef NEW_XYZCAL
 #ifndef HEATBED_V2
 		
 			if (k == 0 || k == 1) {
@@ -2140,6 +2166,7 @@ BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level
 					return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
 			}
 #endif //HEATBED_V2
+#endif
 			#ifdef SUPPORT_VERBOSITY
 			if (verbosity_level >= 10)
 				delay_keep_alive(3000);
@@ -2280,6 +2307,7 @@ BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level
 	return result;    
 }
 
+#ifndef NEW_XYZCAL
 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.
@@ -2576,6 +2604,7 @@ canceled:
     enable_z_endstop(endstop_z_enabled);
     return result;
 }
+#endif //NEW_XYZCAL
 
 bool sample_z() {
 	bool sampled = true;
@@ -2732,6 +2761,7 @@ bool sample_mesh_and_store_reference()
     return true;
 }
 
+#ifndef NEW_XYZCAL
 bool scan_bed_induction_points(int8_t verbosity_level)
 {
     // Don't let the manage_inactivity() function remove power from the motors.
@@ -2793,6 +2823,7 @@ bool scan_bed_induction_points(int8_t verbosity_level)
     enable_z_endstop(endstop_z_enabled);
     return true;
 }
+#endif //NEW_XYZCAL
 
 // Shift a Z axis by a given delta.
 // To replace loading of the babystep correction.

Firmware/mesh_bed_calibration.h

@@ -161,7 +161,9 @@ enum BedSkewOffsetDetectionResultType {
 };
 
 extern BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level, uint8_t &too_far_mask);
+#ifndef NEW_XYZCAL
 extern BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level, uint8_t &too_far_mask);
+#endif //NEW_XYZCAL
 
 extern bool sample_mesh_and_store_reference();
 

Firmware/sm4.c

@@ -0,0 +1,194 @@
+//sm4.c - simple 4-axis stepper control
+
+#include "sm4.h"
+#include <avr/io.h>
+#include <avr/pgmspace.h>
+
+#include "boards.h"
+#define bool int8_t
+#define false 0
+#define true 1
+#include "Configuration_prusa.h"
+
+
+#ifdef NEW_XYZCAL
+
+
+// Signal pinouts
+
+// direction signal - MiniRambo
+//#define X_DIR_PIN    48 //PL1 (-)
+//#define Y_DIR_PIN    49 //PL0 (-)
+//#define Z_DIR_PIN    47 //PL2 (-)
+//#define E0_DIR_PIN   43 //PL6 (+)
+
+//direction signal - EinsyRambo
+//#define X_DIR_PIN    49 //PL0 (+)
+//#define Y_DIR_PIN    48 //PL1 (-)
+//#define Z_DIR_PIN    47 //PL2 (+)
+//#define E0_DIR_PIN   43 //PL6 (-)
+
+//step signal pinout - common for all rambo boards
+//#define X_STEP_PIN   37 //PC0 (+)
+//#define Y_STEP_PIN   36 //PC1 (+)
+//#define Z_STEP_PIN   35 //PC2 (+)
+//#define E0_STEP_PIN  34 //PC3 (+)
+
+
+sm4_stop_cb_t sm4_stop_cb = 0;
+
+sm4_update_pos_cb_t sm4_update_pos_cb = 0;
+
+sm4_calc_delay_cb_t sm4_calc_delay_cb = 0;
+
+uint16_t sm4_cpu_time = 0;
+
+
+uint8_t sm4_get_dir(uint8_t axis)
+{
+	switch (axis)
+	{
+#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203))
+	case 0: return (PORTL & 2)?0:1;
+	case 1: return (PORTL & 1)?0:1;
+	case 2: return (PORTL & 4)?0:1;
+	case 3: return (PORTL & 64)?1:0;
+#else if ((MOTHERBOARD == 310))
+	case 0: return (PORTL & 1)?1:0;
+	case 1: return (PORTL & 2)?0:1;
+	case 2: return (PORTL & 4)?1:0;
+	case 3: return (PORTL & 64)?0:1;
+#endif
+	}
+	return 0;
+}
+
+void sm4_set_dir(uint8_t axis, uint8_t dir)
+{
+	switch (axis)
+	{
+#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203))
+	case 0: if (!dir) PORTL |= 2; else PORTL &= ~2; break;
+	case 1: if (!dir) PORTL |= 1; else PORTL &= ~1; break;
+	case 2: if (!dir) PORTL |= 4; else PORTL &= ~4; break;
+	case 3: if (dir) PORTL |= 64; else PORTL &= ~64; break;
+#else if ((MOTHERBOARD == 310))
+	case 0: if (dir) PORTL |= 1; else PORTL &= ~1; break;
+	case 1: if (!dir) PORTL |= 2; else PORTL &= ~2; break;
+	case 2: if (dir) PORTL |= 4; else PORTL &= ~4; break;
+	case 3: if (!dir) PORTL |= 64; else PORTL &= ~64; break;
+#endif
+	}
+	asm("nop");
+}
+
+uint8_t sm4_get_dir_bits(void)
+{
+	uint8_t register dir_bits = 0;
+	uint8_t register portL = PORTL;
+	//TODO -optimize in asm
+#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203))
+	if (portL & 2) dir_bits |= 1;
+	if (portL & 1) dir_bits |= 2;
+	if (portL & 4) dir_bits |= 4;
+	if (portL & 64) dir_bits |= 8;
+	dir_bits ^= 0x07; //invert XYZ, do not invert E
+#else if ((MOTHERBOARD == 310))
+	if (portL & 1) dir_bits |= 1;
+	if (portL & 2) dir_bits |= 2;
+	if (portL & 4) dir_bits |= 4;
+	if (portL & 64) dir_bits |= 8;
+	dir_bits ^= 0x0a; //invert YE, do not invert XZ
+#endif
+	return dir_bits;
+}
+
+void sm4_set_dir_bits(uint8_t dir_bits)
+{
+	uint8_t register portL = PORTL;
+	portL &= 0xb8; //set direction bits to zero
+	//TODO -optimize in asm
+#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203))
+	dir_bits ^= 0x07; //invert XYZ, do not invert E
+	if (dir_bits & 1) portL |= 2;  //set X direction bit
+	if (dir_bits & 2) portL |= 1;  //set Y direction bit
+	if (dir_bits & 4) portL |= 4;  //set Z direction bit
+	if (dir_bits & 8) portL |= 64; //set E direction bit
+#else if ((MOTHERBOARD == 310))
+	dir_bits ^= 0x0a; //invert YE, do not invert XZ
+	if (dir_bits & 1) portL |= 1;  //set X direction bit
+	if (dir_bits & 2) portL |= 2;  //set Y direction bit
+	if (dir_bits & 4) portL |= 4;  //set Z direction bit
+	if (dir_bits & 8) portL |= 64; //set E direction bit
+#endif
+	PORTL = portL;
+	asm("nop");
+}
+
+void sm4_do_step(uint8_t axes_mask)
+{
+#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203) || (MOTHERBOARD == 310))
+	uint8_t register portC = PORTC & 0xf0;
+	PORTC = portC | (axes_mask & 0x0f); //set step signals by mask
+	asm("nop");
+	PORTC = portC; //set step signals to zero
+	asm("nop");
+#endif //((MOTHERBOARD == 200) || (MOTHERBOARD == 203) || (MOTHERBOARD == 310))
+}
+
+uint16_t sm4_line_xyze_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de)
+{
+	uint16_t dd = (uint16_t)(sqrt((float)(((uint32_t)dx)*dx + ((uint32_t)dy*dy) + ((uint32_t)dz*dz) + ((uint32_t)de*de))) + 0.5);
+	uint16_t nd = dd;
+	uint16_t cx = dd;
+	uint16_t cy = dd;
+	uint16_t cz = dd;
+	uint16_t ce = dd;
+	uint16_t x = 0;
+	uint16_t y = 0;
+	uint16_t z = 0;
+	uint16_t e = 0;
+	while (nd)
+	{
+		if (sm4_stop_cb && (*sm4_stop_cb)()) break;
+		uint8_t sm = 0; //step mask
+		if (cx <= dx)
+		{
+			sm |= 1;
+			cx += dd;
+			x++;
+		}
+		if (cy <= dy)
+		{
+			sm |= 2;
+			cy += dd;
+			y++;
+		}
+		if (cz <= dz)
+		{
+			sm |= 4;
+			cz += dd;
+			z++;
+		}
+		if (ce <= de)
+		{
+			sm |= 4;
+			ce += dd;
+			e++;
+		}
+		cx -= dx;
+		cy -= dy;
+		cz -= dz;
+		ce -= de;
+		sm4_do_step(sm);
+		uint16_t delay = SM4_DEFDELAY;
+		if (sm4_calc_delay_cb) delay = (*sm4_calc_delay_cb)(nd, dd);
+		if (delay) delayMicroseconds(delay);
+		nd--;
+	}
+	if (sm4_update_pos_cb) (*sm4_update_pos_cb)(x, y, z, e);
+	return nd;
+}
+
+
+#endif //NEW_XYZCAL

Firmware/sm4.h

@@ -0,0 +1,56 @@
+//sm4.h - simple 4-axis stepper control
+#ifndef _SM4_H
+#define _SM4_H
+
+#include <inttypes.h>
+#include "config.h"
+
+
+#if defined(__cplusplus)
+extern "C" {
+#endif //defined(__cplusplus)
+
+
+// callback prototype for stop condition (return 0 - continue, return 1 - stop)
+typedef uint8_t (*sm4_stop_cb_t)();
+
+// callback prototype for updating position counters
+typedef void (*sm4_update_pos_cb_t)(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de);
+
+// callback prototype for calculating delay
+typedef uint16_t (*sm4_calc_delay_cb_t)(uint16_t nd, uint16_t dd);
+
+
+// callback pointer - stop
+extern sm4_stop_cb_t sm4_stop_cb;
+
+// callback pointer - update_pos
+extern sm4_update_pos_cb_t sm4_update_pos_cb;
+
+// callback pointer - calc_delay
+extern sm4_calc_delay_cb_t sm4_calc_delay_cb;
+
+
+// returns direction for single axis (0 - positive, 1 - negative)
+extern uint8_t sm4_get_dir(uint8_t axis);
+
+// set direction for single axis (0 - positive, 1 - negative)
+extern void sm4_set_dir(uint8_t axis, uint8_t dir);
+
+// returns direction of all axes as bitmask (0 - positive, 1 - negative)
+extern uint8_t sm4_get_dir_bits(void);
+
+// set direction for all axes as bitmask (0 - positive, 1 - negative)
+extern void sm4_set_dir_bits(uint8_t dir_bits);
+
+// step axes by bitmask
+extern void sm4_do_step(uint8_t axes_mask);
+
+// xyze linear-interpolated relative move, returns remaining diagonal steps (>0 means stoped)
+extern uint16_t sm4_line_xyze_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de);
+
+
+#if defined(__cplusplus)
+}
+#endif //defined(__cplusplus)
+#endif //_SM4_H

Firmware/temperature.h

@@ -27,6 +27,9 @@
   #include "stepper.h"
 #endif
 
+#define ENABLE_TEMPERATURE_INTERRUPT()  TIMSK0 |= (1<<OCIE0B)
+#define DISABLE_TEMPERATURE_INTERRUPT() TIMSK0 &= ~(1<<OCIE0B)
+
 // public functions
 void tp_init();  //initialize the heating
 void manage_heater(); //it is critical that this is called periodically.

Firmware/xyzcal.cpp

@@ -0,0 +1,807 @@
+//xyzcal.cpp - xyz calibration with image processing
+
+#include "Configuration_prusa.h"
+#ifdef NEW_XYZCAL
+
+#include "xyzcal.h"
+#include <avr/wdt.h>
+#include "stepper.h"
+#include "temperature.h"
+#include "sm4.h"
+
+
+#define XYZCAL_PINDA_HYST_MIN 20  //50um
+#define XYZCAL_PINDA_HYST_MAX 100 //250um
+#define XYZCAL_PINDA_HYST_DIF 5   //12.5um
+
+#define ENABLE_FANCHECK_INTERRUPT()  EIMSK |= (1<<7)
+#define DISABLE_FANCHECK_INTERRUPT() EIMSK &= ~(1<<7)
+
+#define _PINDA ((READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING)?1:0)
+
+#define DBG(args...) printf_P(args)
+//#define DBG(args...)
+#define _n PSTR
+
+#define _X ((int16_t)count_position[X_AXIS])
+#define _Y ((int16_t)count_position[Y_AXIS])
+#define _Z ((int16_t)count_position[Z_AXIS])
+#define _E ((int16_t)count_position[E_AXIS])
+
+#define _PI 3.14159265F
+
+extern long count_position[NUM_AXIS];
+
+uint8_t check_pinda_0();
+uint8_t check_pinda_1();
+void xyzcal_update_pos(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de);
+uint16_t xyzcal_calc_delay(uint16_t nd, uint16_t dd);
+
+
+void xyzcal_meassure_enter(void)
+{
+	DBG(_n("xyzcal_meassure_enter\n"));
+	disable_heater();
+	DISABLE_TEMPERATURE_INTERRUPT();
+#if (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
+	DISABLE_FANCHECK_INTERRUPT();
+#endif //(defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
+	DISABLE_STEPPER_DRIVER_INTERRUPT();
+#ifdef WATCHDOG
+	wdt_disable();
+#endif //WATCHDOG
+	sm4_stop_cb = 0;
+	sm4_update_pos_cb = xyzcal_update_pos;
+	sm4_calc_delay_cb = xyzcal_calc_delay;
+}
+
+void xyzcal_meassure_leave(void)
+{
+	DBG(_n("xyzcal_meassure_leave\n"));
+    planner_abort_hard();
+	ENABLE_TEMPERATURE_INTERRUPT();
+#if (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
+	ENABLE_FANCHECK_INTERRUPT();
+#endif //(defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
+	ENABLE_STEPPER_DRIVER_INTERRUPT();
+#ifdef WATCHDOG
+	wdt_enable(WDTO_4S);
+#endif //WATCHDOG
+	sm4_stop_cb = 0;
+	sm4_update_pos_cb = 0;
+	sm4_calc_delay_cb = 0;
+}
+
+
+uint8_t check_pinda_0()
+{
+	return _PINDA?0:1;
+}
+
+uint8_t check_pinda_1()
+{
+	return _PINDA?1:0;
+}
+
+uint8_t xyzcal_dm = 0;
+
+void xyzcal_update_pos(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de)
+{
+//	DBG(_n("xyzcal_update_pos dx=%d dy=%d dz=%d dir=%02x\n"), dx, dy, dz, xyzcal_dm);
+	if (xyzcal_dm&1) count_position[0] -= dx; else count_position[0] += dx;
+	if (xyzcal_dm&2) count_position[1] -= dy; else count_position[1] += dy;
+	if (xyzcal_dm&4) count_position[2] -= dz; else count_position[2] += dz;
+//	DBG(_n(" after xyzcal_update_pos x=%ld y=%ld z=%ld\n"), count_position[0], count_position[1], count_position[2]);
+}
+
+uint16_t xyzcal_sm4_delay = 0;
+
+//#define SM4_ACCEL_TEST
+#ifdef SM4_ACCEL_TEST
+uint16_t xyzcal_sm4_v0 = 2000;
+uint16_t xyzcal_sm4_vm = 45000;
+uint16_t xyzcal_sm4_v = xyzcal_sm4_v0;
+uint16_t xyzcal_sm4_ac = 2000;
+uint16_t xyzcal_sm4_ac2 = (uint32_t)xyzcal_sm4_ac * 1024 / 10000;
+//float xyzcal_sm4_vm = 10000;
+#endif //SM4_ACCEL_TEST
+
+uint16_t xyzcal_calc_delay(uint16_t nd, uint16_t dd)
+{
+	return xyzcal_sm4_delay;
+#ifdef SM4_ACCEL_TEST
+
+	uint16_t del_us = 0;
+	if (xyzcal_sm4_v & 0xf000) //>=4096
+	{
+		del_us = (uint16_t)62500 / (uint16_t)(xyzcal_sm4_v >> 4);
+		xyzcal_sm4_v += (xyzcal_sm4_ac2 * del_us + 512) >> 10;
+		if (xyzcal_sm4_v > xyzcal_sm4_vm) xyzcal_sm4_v = xyzcal_sm4_vm;
+		if (del_us > 25) return del_us - 25;
+	}
+	else
+	{
+		del_us = (uint32_t)1000000 / xyzcal_sm4_v;
+		xyzcal_sm4_v += ((uint32_t)xyzcal_sm4_ac2 * del_us + 512) >> 10;
+		if (xyzcal_sm4_v > xyzcal_sm4_vm) xyzcal_sm4_v = xyzcal_sm4_vm;
+		if (del_us > 50) return del_us - 50;
+	}
+
+//	uint16_t del_us = (uint16_t)(((float)1000000 / xyzcal_sm4_v) + 0.5);		
+//	uint16_t del_us = (uint32_t)1000000 / xyzcal_sm4_v;		
+//	uint16_t del_us = 100;		
+//	uint16_t del_us = (uint16_t)10000 / xyzcal_sm4_v;
+//	v += (ac * del_us + 500) / 1000;
+//	xyzcal_sm4_v += (xyzcal_sm4_ac * del_us) / 1000;
+//	return xyzcal_sm4_delay;
+//	DBG(_n("xyzcal_calc_delay nd=%d dd=%d v=%d  del_us=%d\n"), nd, dd, xyzcal_sm4_v, del_us);
+	return 0;
+#endif //SM4_ACCEL_TEST
+}
+
+
+bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_t check_pinda)
+{
+//	DBG(_n("xyzcal_lineXYZ_to x=%d y=%d z=%d  check=%d\n"), x, y, z, check_pinda);
+	x -= (int16_t)count_position[0];
+	y -= (int16_t)count_position[1];
+	z -= (int16_t)count_position[2];
+	xyzcal_dm = ((x<0)?1:0) | ((y<0)?2:0) | ((z<0)?4:0);
+	sm4_set_dir_bits(xyzcal_dm);
+	sm4_stop_cb = check_pinda?((check_pinda<0)?check_pinda_0:check_pinda_1):0;
+	xyzcal_sm4_delay = delay_us;
+//	uint32_t u = micros();
+	bool ret = sm4_line_xyze_ui(abs(x), abs(y), abs(z), 0)?true:false;
+//	u = micros() - u;
+	return ret;
+}
+
+bool xyzcal_spiral2(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radius, int16_t rotation, uint16_t delay_us, int8_t check_pinda, uint16_t* pad)
+{
+	bool ret = false;
+	float r = 0; //radius
+	uint8_t n = 0; //point number
+	uint16_t ad = 0; //angle [deg]
+	float ar; //angle [rad]
+	uint8_t dad = 0; //delta angle [deg]
+	uint8_t dad_min = 4; //delta angle min [deg]
+	uint8_t dad_max = 16; //delta angle max [deg]
+	uint8_t k = 720 / (dad_max - dad_min); //delta calculation constant
+	ad = 0;
+	if (pad) ad = *pad % 720;
+	DBG(_n("xyzcal_spiral2 cx=%d cy=%d z0=%d dz=%d radius=%d ad=%d\n"), cx, cy, z0, dz, radius, ad);
+	for (; ad < 720; ad++)
+	{
+		if (radius > 0)
+		{
+			dad = dad_max - (ad / k);
+			r = (float)(((uint32_t)ad) * radius) / 720;
+		}
+		else
+		{
+			dad = dad_max - ((719 - ad) / k);
+			r = (float)(((uint32_t)(719 - ad)) * (-radius)) / 720;
+		}
+		ar = (ad + rotation)* (float)_PI / 180;
+		float _cos = cos(ar);
+		float _sin = sin(ar);
+		int x = (int)(cx + (_cos * r));
+		int y = (int)(cy + (_sin * r));
+		int z = (int)(z0 - ((float)((int32_t)dz * ad) / 720));
+		if (xyzcal_lineXYZ_to(x, y, z, delay_us, check_pinda))
+		{
+			ad += dad + 1;
+			ret = true;
+			break;
+		}
+		n++;
+		ad += dad;
+	}
+	if (pad) *pad = ad;
+	return ret;
+}
+
+bool xyzcal_spiral8(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radius, uint16_t delay_us, int8_t check_pinda, uint16_t* pad)
+{
+	bool ret = false;
+	uint16_t ad = 0;
+	if (pad) ad = *pad;
+	DBG(_n("xyzcal_spiral8 cx=%d cy=%d z0=%d dz=%d radius=%d ad=%d\n"), cx, cy, z0, dz, radius, ad);
+	if (!ret && (ad < 720))
+		if (ret = xyzcal_spiral2(cx, cy, z0 - 0*dz, dz, radius, 0, delay_us, check_pinda, &ad))
+			ad += 0;
+	if (!ret && (ad < 1440))
+		if (ret = xyzcal_spiral2(cx, cy, z0 - 1*dz, dz, -radius, 0, delay_us, check_pinda, &ad))
+			ad += 720;
+	if (!ret && (ad < 2160))
+		if (ret = xyzcal_spiral2(cx, cy, z0 - 2*dz, dz, radius, 180, delay_us, check_pinda, &ad))
+			ad += 1440;
+	if (!ret && (ad < 2880))
+		if (ret = xyzcal_spiral2(cx, cy, z0 - 3*dz, dz, -radius, 180, delay_us, check_pinda, &ad))
+			ad += 2160;
+	if (pad) *pad = ad;
+	return ret;
+}
+
+#ifdef XYZCAL_MEASSURE_PINDA_HYSTEREZIS
+int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t samples)
+{
+	DBG(_n("xyzcal_meassure_pinda_hysterezis\n"));
+	int8_t ret = -1; // PINDA signal error
+	int16_t z = _Z;
+	int16_t sum_up = 0;
+	int16_t sum_dn = 0;
+	int16_t up;
+	int16_t dn;
+	uint8_t sample;
+	xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1);
+	xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1);
+	if (!_PINDA)
+	{
+		for (sample = 0; sample < samples; sample++)
+		{
+			dn = _Z;
+			if (!xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1)) break;
+			dn = dn - _Z;
+			up = _Z;
+			if (!xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1)) break;
+			up = _Z - up;
+			DBG(_n("%d. up=%d dn=%d\n"), sample, up, dn);
+			sum_up += up;
+			sum_dn += dn;
+			if (abs(up - dn) > XYZCAL_PINDA_HYST_DIF)
+			{
+				ret = -2; // difference between up-dn to high
+				break;
+			}
+		}
+		if (sample == samples)
+		{
+			up = sum_up / samples;
+			dn = sum_dn / samples;
+			uint16_t hyst = (up + dn) / 2;
+			if (abs(up - dn) > XYZCAL_PINDA_HYST_DIF)
+				ret = -2; // difference between up-dn to high
+			else if ((hyst < XYZCAL_PINDA_HYST_MIN) || (hyst > XYZCAL_PINDA_HYST_MAX))
+				ret = -3; // hysterezis out of range
+			else
+				ret = hyst;
+		}
+	}
+	xyzcal_lineXYZ_to(_X, _Y, z, delay_us, 0);
+	return ret;
+}
+#endif //XYZCAL_MEASSURE_PINDA_HYSTEREZIS
+
+
+void xyzcal_scan_pixels_32x32(int16_t cx, int16_t cy, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t* pixels)
+{
+	DBG(_n("xyzcal_scan_pixels_32x32 cx=%d cy=%d min_z=%d max_z=%d\n"), cx, cy, min_z, max_z);
+//	xyzcal_lineXYZ_to(cx - 1024, cy - 1024, max_z, 2*delay_us, 0);
+//	xyzcal_lineXYZ_to(cx, cy, max_z, delay_us, 0);
+	int16_t z = (int16_t)count_position[2];
+	xyzcal_lineXYZ_to(cx, cy, z, 2*delay_us, 0);
+	for (uint8_t r = 0; r < 32; r++)
+	{
+		int8_t _pinda = _PINDA;
+		xyzcal_lineXYZ_to((r&1)?(cx+1024):(cx-1024), cy - 1024 + r*64, z, 2*delay_us, 0);
+		xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1);
+		xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1);
+		z = (int16_t)count_position[2];
+		sm4_set_dir(X_AXIS, (r&1)?1:0);
+		for (uint8_t c = 0; c < 32; c++)
+		{
+			uint16_t sum = 0;
+			int16_t z_sum = 0;
+			for (uint8_t i = 0; i < 64; i++)
+			{
+				int8_t pinda = _PINDA;
+				int16_t pix = z - min_z;
+				pix += (pinda)?23:-24;
+				if (pix < 0) pix = 0;
+				if (pix > 255) pix = 255;
+				sum += pix;
+				z_sum += z;
+//				if (_pinda != pinda)
+//				{
+//					if (pinda)
+//						DBG(_n("!1 x=%d z=%d\n"), c*64+i, z+23);
+//					else
+//						DBG(_n("!0 x=%d z=%d\n"), c*64+i, z-24);
+//				}
+				sm4_set_dir(Z_AXIS, !pinda);
+				if (!pinda)
+				{
+					if (z > min_z)
+					{
+						sm4_do_step(Z_AXIS_MASK);
+						z--;
+					}
+				}
+				else
+				{
+					if (z < max_z)
+					{
+						sm4_do_step(Z_AXIS_MASK);
+						z++;
+					}
+				}
+				sm4_do_step(X_AXIS_MASK);
+				delayMicroseconds(600);
+				_pinda = pinda;
+			}
+			sum >>= 6; //div 64
+			if (z_sum < 0)
+			{
+				z_sum = -z_sum;
+				z_sum >>= 6; //div 64
+				z_sum = -z_sum;
+			}
+			else
+				z_sum >>= 6; //div 64
+			if (pixels) pixels[((uint16_t)r<<5) + ((r&1)?(31-c):c)] = sum;
+//			DBG(_n("c=%d r=%d l=%d z=%d\n"), c, r, sum, z_sum);
+			count_position[0] += (r&1)?-64:64;
+			count_position[2] = z;
+		}
+		if (pixels)
+			for (uint8_t c = 0; c < 32; c++)
+				DBG(_n("%02x"), pixels[((uint16_t)r<<5) + c]);
+		DBG(_n("\n"));
+	}
+//	xyzcal_lineXYZ_to(cx, cy, z, 2*delay_us, 0);
+}
+
+void xyzcal_histo_pixels_32x32(uint8_t* pixels, uint16_t* histo)
+{
+	for (uint8_t l = 0; l < 16; l++)
+		histo[l] = 0;
+	for (uint8_t r = 0; r < 32; r++)
+		for (uint8_t c = 0; c < 32; c++)
+		{
+			uint8_t pix = pixels[((uint16_t)r<<5) + c];
+			histo[pix >> 4]++;
+		}
+	for (uint8_t l = 0; l < 16; l++)
+		DBG(_n(" %2d %d\n"), l, histo[l]);
+}
+
+void xyzcal_adjust_pixels(uint8_t* pixels, uint16_t* histo)
+{
+	uint8_t l;
+	uint16_t max_c = histo[1];
+	uint8_t max_l = 1;
+	for (l = 1; l < 16; l++)
+	{
+		uint16_t c = histo[l];
+		if (c > max_c)
+		{
+			max_c = c;
+			max_l = l;
+		}
+	}
+	DBG(_n("max_c=%2d max_l=%d\n"), max_c, max_l);
+	for (l = 14; l > 8; l--)
+		if (histo[l] >= 10)
+			break;
+	uint8_t pix_min = (max_l << 4) / 2;
+	uint8_t pix_max = l << 4;
+	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++)
+	{
+		uint16_t pix = pixels[i];
+		if (pix > pix_min) pix -= pix_min;
+		else pix = 0;
+		pix <<= 8;
+		pix /= pix_dif;
+//		if (pix < 0) pix = 0;
+		if (pix > 255) pix = 255;
+		pixels[i] = (uint8_t)pix;
+	}
+	for (uint8_t r = 0; r < 32; r++)
+	{
+		for (uint8_t c = 0; c < 32; c++)
+			DBG(_n("%02x"), pixels[((uint16_t)r<<5) + c]);
+		DBG(_n("\n"));
+	}
+}
+
+/*
+void xyzcal_draw_pattern_12x12_in_32x32(uint8_t* pattern, uint32_t* pixels, int w, int h, uint8_t x, uint8_t y, uint32_t and, uint32_t or)
+{
+	for (int i = 0; i < 8; i++)
+		for (int j = 0; j < 8; j++)
+		{
+			int idx = (x + j) + w * (y + i);
+			if (pattern[i] & (1 << j))
+			{
+				pixels[idx] &= and;
+				pixels[idx] |= or;
+			}
+		}
+}
+*/
+
+int16_t xyzcal_match_pattern_12x12_in_32x32(uint16_t* pattern, uint8_t* pixels, uint8_t c, uint8_t r)
+{
+	uint8_t thr = 16;
+	int16_t match = 0;
+	for (uint8_t i = 0; i < 12; i++)
+		for (uint8_t j = 0; j < 12; j++)
+		{
+			if (((i == 0) || (i == 11)) && ((j < 2) || (j >= 10))) continue; //skip corners
+			if (((j == 0) || (j == 11)) && ((i < 2) || (i >= 10))) continue;
+			uint16_t idx = (c + j) + 32 * (r + i);
+			uint8_t val = pixels[idx];
+			if (pattern[i] & (1 << j))
+			{
+				if (val > thr) match ++;
+				else match --;
+			}
+			else
+			{
+				if (val <= thr) match ++;
+				else match --;
+			}
+		}
+	return match;
+}
+
+int16_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, uint8_t* pc, uint8_t* pr)
+{
+	uint8_t max_c = 0;
+	uint8_t max_r = 0;
+	int16_t max_match = 0;
+	for (uint8_t r = 0; r < (32 - 12); r++)
+		for (uint8_t c = 0; c < (32 - 12); c++)
+		{
+			int16_t match = xyzcal_match_pattern_12x12_in_32x32(pattern, pixels, c, r);
+			if (max_match < match)
+			{
+				max_c = c;
+				max_r = r;
+				max_match = match;
+			}
+		}
+	DBG(_n("max_c=%d max_r=%d max_match=%d\n"), max_c, max_r, max_match);
+	if (pc) *pc = max_c;
+	if (pr) *pr = max_r;
+	return max_match;
+}
+
+#define MAX_DIAMETR 600
+#define XYZCAL_FIND_CENTER_DIAGONAL
+
+int8_t xyzcal_find_point_center2(uint16_t delay_us)
+{
+	printf_P(PSTR("xyzcal_find_point_center2\n"));
+	int16_t x0 = _X;
+	int16_t y0 = _Y;
+	int16_t z0 = _Z;
+	printf_P(PSTR(" x0=%d\n"), x0);
+	printf_P(PSTR(" y0=%d\n"), y0);
+	printf_P(PSTR(" z0=%d\n"), z0);
+
+	xyzcal_lineXYZ_to(_X, _Y, z0 + 400, 500, -1);
+	xyzcal_lineXYZ_to(_X, _Y, z0 - 400, 500, 1);
+	xyzcal_lineXYZ_to(_X, _Y, z0 + 400, 500, -1);
+	xyzcal_lineXYZ_to(_X, _Y, z0 - 400, 500, 1);
+
+	z0 = _Z;
+
+//	xyzcal_lineXYZ_to(x0, y0, z0 - 100, 500, 1);
+//	z0 = _Z;
+//	printf_P(PSTR("  z0=%d\n"), z0);
+//	xyzcal_lineXYZ_to(x0, y0, z0 + 100, 500, -1);
+//	z0 += _Z;
+//	z0 /= 2;
+	printf_P(PSTR("   z0=%d\n"), z0);
+//	xyzcal_lineXYZ_to(x0, y0, z0 - 100, 500, 1);
+//	z0 = _Z - 10;
+
+	int8_t ret = 1;
+
+#ifdef XYZCAL_FIND_CENTER_DIAGONAL
+	int32_t xc = 0;
+	int32_t yc = 0;
+	int16_t ad = 45;
+	for (; ad < 360; ad += 90)
+	{
+		float ar = (float)ad * _PI / 180;
+		int16_t x = x0 + MAX_DIAMETR * cos(ar);
+		int16_t y = y0 + MAX_DIAMETR * sin(ar);
+		if (!xyzcal_lineXYZ_to(x, y, z0, delay_us, -1))
+		{
+			printf_P(PSTR("ERROR ad=%d\n"), ad);
+			ret = 0;
+			break;
+		}
+		xc += _X;
+		yc += _Y;
+		xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0);
+	}
+	if (ret)
+	{
+		printf_P(PSTR("OK\n"), ad);
+		x0 = xc / 4;
+		y0 = yc / 4;
+		printf_P(PSTR(" x0=%d\n"), x0);
+		printf_P(PSTR(" y0=%d\n"), y0);
+	}
+
+#else //XYZCAL_FIND_CENTER_DIAGONAL
+	xyzcal_lineXYZ_to(x0 - MAX_DIAMETR, y0, z0, delay_us, -1);
+	int16_t dx1 = x0 - _X;
+	if (dx1 >= MAX_DIAMETR)
+	{
+		printf_P(PSTR("!!! dx1 = %d\n"), dx1);
+		return 0;
+	}
+	xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0);
+	xyzcal_lineXYZ_to(x0 + MAX_DIAMETR, y0, z0, delay_us, -1);
+	int16_t dx2 = _X - x0;
+	if (dx2 >= MAX_DIAMETR)
+	{
+		printf_P(PSTR("!!! dx2 = %d\n"), dx2);
+		return 0;
+	}
+	xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0);
+	xyzcal_lineXYZ_to(x0 , y0 - MAX_DIAMETR, z0, delay_us, -1);
+	int16_t dy1 = y0 - _Y;
+	if (dy1 >= MAX_DIAMETR)
+	{
+		printf_P(PSTR("!!! dy1 = %d\n"), dy1);
+		return 0;
+	}
+	xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0);
+	xyzcal_lineXYZ_to(x0, y0 + MAX_DIAMETR, z0, delay_us, -1);
+	int16_t dy2 = _Y - y0;
+	if (dy2 >= MAX_DIAMETR)
+	{
+		printf_P(PSTR("!!! dy2 = %d\n"), dy2);
+		return 0;
+	}
+	printf_P(PSTR("dx1=%d\n"), dx1);
+	printf_P(PSTR("dx2=%d\n"), dx2);
+	printf_P(PSTR("dy1=%d\n"), dy1);
+	printf_P(PSTR("dy2=%d\n"), dy2);
+
+	x0 += (dx2 - dx1) / 2;
+	y0 += (dy2 - dy1) / 2;
+
+	printf_P(PSTR(" x0=%d\n"), x0);
+	printf_P(PSTR(" y0=%d\n"), y0);
+
+#endif //XYZCAL_FIND_CENTER_DIAGONAL
+
+	xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0);
+
+	return ret;
+}
+
+#ifdef XYZCAL_FIND_POINT_CENTER
+int8_t xyzcal_find_point_center(int16_t x0, int16_t y0, int16_t z0, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t turns)
+{
+	uint8_t n;
+	uint16_t ad;
+	float ar;
+	float _cos;
+	float _sin;
+	int16_t r_min = 0;
+	int16_t r_max = 0;
+	int16_t x_min = 0;
+	int16_t x_max = 0;
+	int16_t y_min = 0;
+	int16_t y_max = 0;
+	int16_t r = 10;
+	int16_t x = x0;
+	int16_t y = y0;
+	int16_t z = z0;
+	int8_t _pinda = _PINDA;
+	for (n = 0; n < turns; n++)
+	{
+		uint32_t r_sum = 0;
+		for (ad = 0; ad < 720; ad++)
+		{
+			ar = ad * _PI / 360;
+			_cos = cos(ar);
+			_sin = sin(ar);
+			x = x0 + (int)(_cos * r);
+			y = y0 + (int)(_sin * r);
+			xyzcal_lineXYZ_to(x, y, z, 1000, 0);
+			int8_t pinda = _PINDA;
+			if (pinda)
+				r += 1;
+			else
+			{
+				r -= 1;
+				ad--;
+				r_sum -= r;
+			}
+			if (ad == 0)
+			{
+				x_min = x0;
+				x_max = x0;
+				y_min = y0;
+				y_max = y0;
+				r_min = r;
+				r_max = r;
+			}
+			else if (pinda)
+			{
+				if (x_min > x) x_min = (2*x + x_min) / 3;
+				if (x_max < x) x_max = (2*x + x_max) / 3;
+				if (y_min > y) y_min = (2*y + y_min) / 3;
+				if (y_max < y) y_max = (2*y + y_max) / 3;
+/*				if (x_min > x) x_min = x;
+				if (x_max < x) x_max = x;
+				if (y_min > y) y_min = y;
+				if (y_max < y) y_max = y;*/
+				if (r_min > r) r_min = r;
+				if (r_max < r) r_max = r;
+			}
+			r_sum += r;
+/*			if (_pinda != pinda)
+			{
+				if (pinda)
+					DBG(_n("!1 x=%d y=%d\n"), x, y);
+				else
+					DBG(_n("!0 x=%d y=%d\n"), x, y);
+			}*/
+			_pinda = pinda;
+//			DBG(_n("x=%d y=%d rx=%d ry=%d\n"), x, y, rx, ry);
+		}
+		DBG(_n("x_min=%d x_max=%d y_min=%d y_max=%d r_min=%d r_max=%d r_avg=%d\n"), x_min, x_max, y_min, y_max, r_min, r_max, r_sum / 720);
+		if ((n > 2) && (n & 1))
+		{
+			x0 += (x_min + x_max);
+			y0 += (y_min + y_max);
+			x0 /= 3;
+			y0 /= 3;
+			int rx = (x_max - x_min) / 2;
+			int ry = (y_max - y_min) / 2;
+			r = (rx + ry) / 3;//(rx < ry)?rx:ry;
+			DBG(_n("x0=%d y0=%d r=%d\n"), x0, y0, r);
+		}
+	}
+	xyzcal_lineXYZ_to(x0, y0, z, 200, 0);
+}
+#endif //XYZCAL_FIND_POINT_CENTER
+
+
+uint8_t xyzcal_xycoords2point(int16_t x, int16_t y)
+{
+	uint8_t ix = (x > 10000)?1:0;
+	uint8_t iy = (y > 10000)?1:0;
+	return iy?(3-ix):ix;
+}
+
+//MK3
+#if ((MOTHERBOARD == 310))
+const int16_t PROGMEM xyzcal_point_xcoords[4] = {1200, 22000, 22000, 1200};
+const int16_t PROGMEM xyzcal_point_ycoords[4] = {600, 600, 19800, 19800};
+#endif //((MOTHERBOARD == 310))
+
+//MK2.5
+#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203))
+const int16_t PROGMEM xyzcal_point_xcoords[4] = {1200, 22000, 22000, 1200};
+const int16_t PROGMEM xyzcal_point_ycoords[4] = {700, 700, 19800, 19800};
+#endif //((MOTHERBOARD == 200) || (MOTHERBOARD == 203))
+
+const uint16_t PROGMEM xyzcal_point_pattern[12] = {0x000, 0x0f0, 0x1f8, 0x3fc, 0x7fe, 0x7fe, 0x7fe, 0x7fe, 0x3fc, 0x1f8, 0x0f0, 0x000};
+
+bool xyzcal_searchZ(void)
+{
+	DBG(_n("xyzcal_searchZ x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]);
+	int16_t x0 = _X;
+	int16_t y0 = _Y;
+	int16_t z0 = _Z;
+//	int16_t min_z = -6000;
+//	int16_t dz = 100;
+	int16_t z = z0;
+	while (z > -2300) //-6mm + 0.25mm
+	{
+		uint16_t ad = 0;
+		if (xyzcal_spiral8(x0, y0, z, 100, 900, 320, 1, &ad)) //dz=100 radius=900 delay=400
+		{
+			int16_t x_on = _X;
+			int16_t y_on = _Y;
+			int16_t z_on = _Z;
+			DBG(_n(" ON-SIGNAL at x=%d y=%d z=%d ad=%d\n"), x_on, y_on, z_on, ad);
+			return true;
+		}
+		z -= 400;
+	}
+	DBG(_n("xyzcal_searchZ no signal\n x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]);
+	return false;
+}
+
+bool xyzcal_scan_and_process(void)
+{
+	DBG(_n("sizeof(block_buffer)=%d\n"), sizeof(block_t)*BLOCK_BUFFER_SIZE);
+//	DBG(_n("sizeof(pixels)=%d\n"), 32*32);
+//	DBG(_n("sizeof(histo)=%d\n"), 2*16);
+//	DBG(_n("sizeof(pattern)=%d\n"), 2*12);
+	DBG(_n("sizeof(total)=%d\n"), 32*32+2*16+2*12);
+	bool ret = false;
+	int16_t x = _X;
+	int16_t y = _Y;
+	int16_t z = _Z;
+
+	uint8_t* pixels = (uint8_t*)block_buffer;
+	xyzcal_scan_pixels_32x32(x, y, z - 72, 2400, 200, pixels);
+
+	uint16_t* histo = (uint16_t*)(pixels + 32*32);
+	xyzcal_histo_pixels_32x32(pixels, histo);
+
+	xyzcal_adjust_pixels(pixels, histo);
+
+	uint16_t* pattern = (uint16_t*)(histo + 2*16);
+	for (uint8_t i = 0; i < 12; i++)
+	{
+		pattern[i] = pgm_read_word_far((uint16_t*)(xyzcal_point_pattern + i));
+//		DBG(_n(" pattern[%d]=%d\n"), i, pattern[i]);
+	}
+	uint8_t c = 0;
+	uint8_t r = 0;
+	if (xyzcal_find_pattern_12x12_in_32x32(pixels, pattern, &c, &r) > 66) //total pixels=144, corner=12 (1/2 = 66)
+	{
+		DBG(_n(" pattern found at %d %d\n"), c, r);
+		c += 6;
+		r += 6;
+		x += ((int16_t)c - 16) << 6;
+		y += ((int16_t)r - 16) << 6;
+		DBG(_n(" x=%d y=%d z=%d\n"), x, y, z);
+		xyzcal_lineXYZ_to(x, y, z, 200, 0);
+		ret = true;
+	}
+	for (uint16_t i = 0; i < sizeof(block_t)*BLOCK_BUFFER_SIZE; i++)
+		pixels[i] = 0;
+	return ret;
+}
+
+bool xyzcal_find_bed_induction_sensor_point_xy(void)
+{
+	DBG(_n("xyzcal_find_bed_induction_sensor_point_xy x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]);
+	bool ret = false;
+	st_synchronize();
+	int16_t x = _X;
+	int16_t y = _Y;
+	int16_t z = _Z;
+	uint8_t point = xyzcal_xycoords2point(x, y);
+	x = pgm_read_word_far((uint16_t*)(xyzcal_point_xcoords + point));
+	y = pgm_read_word_far((uint16_t*)(xyzcal_point_ycoords + point));
+	DBG(_n("point=%d x=%d y=%d z=%d\n"), point, x, y, z);
+	xyzcal_meassure_enter();
+	xyzcal_lineXYZ_to(x, y, z, 200, 0);
+	if (xyzcal_searchZ())
+	{
+		int16_t z = _Z;
+		xyzcal_lineXYZ_to(x, y, z, 200, 0);
+		if (xyzcal_scan_and_process())
+		{
+			if (xyzcal_find_point_center2(500))
+			{
+				uint32_t x_avg = 0;
+				uint32_t y_avg = 0;
+				uint8_t n; for (n = 0; n < 4; n++)
+				{
+					if (!xyzcal_find_point_center2(1000)) break;
+					x_avg += _X;
+					y_avg += _Y;	
+				}
+				if (n == 4)
+				{
+					xyzcal_lineXYZ_to(x_avg >> 2, y_avg >> 2, _Z, 200, 0);
+					ret = true;
+				}
+			}
+		}
+	}
+	xyzcal_meassure_leave();
+	return ret;
+}
+
+
+#endif //NEW_XYZCAL

Firmware/xyzcal.h

@@ -0,0 +1,41 @@
+//xyzcal.h - xyz calibration with image processing
+#ifndef _XYZCAL_H
+#define _XYZCAL_H
+
+#include <inttypes.h>
+
+
+extern void xyzcal_meassure_enter(void);
+
+extern void xyzcal_meassure_leave(void);
+
+extern bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_t check_pinda);
+
+extern bool xyzcal_spiral2(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radius, int16_t rotation, uint16_t delay_us, int8_t check_pinda, uint16_t* pad);
+
+extern bool xyzcal_spiral8(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radius, uint16_t delay_us, int8_t check_pinda, uint16_t* pad);
+
+//extern int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t samples);
+
+extern void xyzcal_scan_pixels_32x32(int16_t cx, int16_t cy, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t* pixels);
+
+extern void xyzcal_histo_pixels_32x32(uint8_t* pixels, uint16_t* histo);
+
+extern void xyzcal_adjust_pixels(uint8_t* pixels, uint16_t* histo);
+
+extern int16_t xyzcal_match_pattern_12x12_in_32x32(uint16_t* pattern, uint8_t* pixels, uint8_t x, uint8_t y);
+
+extern int16_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, uint8_t* pc, uint8_t* pr);
+
+extern int8_t xyzcal_find_point_center2(uint16_t delay_us);
+
+//extern int8_t xyzcal_find_point_center(int16_t x0, int16_t y0, int16_t z0, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t turns);
+
+extern bool xyzcal_searchZ(void);
+
+extern bool xyzcal_scan_and_process(void);
+
+extern bool xyzcal_find_bed_induction_sensor_point_xy(void);
+
+
+#endif //_XYZCAL_H