Merge pull request #2968 from espr14/PFW-1186

Calibration: reduce code size PFW-1186

Firmware/sm4.c

@@ -191,5 +191,45 @@ uint16_t sm4_line_xyze_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de)
 	return nd;
 }
 
+uint16_t sm4_line_xyz_ui(uint16_t dx, uint16_t dy, uint16_t dz){
+	uint16_t dd = (uint16_t)(sqrt((float)(((uint32_t)dx)*dx + ((uint32_t)dy*dy) + ((uint32_t)dz*dz))) + 0.5);
+	uint16_t nd = dd;
+	uint16_t cx = dd;
+	uint16_t cy = dd;
+	uint16_t cz = dd;
+	uint16_t x = 0;
+	uint16_t y = 0;
+	uint16_t z = 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++;
+		}
+		cx -= dx;
+		cy -= dy;
+		cz -= dz;
+		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, 0);
+	return nd;
+}
 
 #endif //NEW_XYZCAL

Firmware/sm4.h

@@ -1,56 +1,57 @@
-//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
+//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);
+extern uint16_t sm4_line_xyz_ui(uint16_t dx, uint16_t dy, uint16_t dz);
+
+
+#if defined(__cplusplus)
+}
+#endif //defined(__cplusplus)
+#endif //_SM4_H

Firmware/xyzcal.cpp

@@ -252,7 +252,7 @@ bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_
 	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;
+	bool ret = sm4_line_xyz_ui(abs(x), abs(y), abs(z)) ? true : false;
 	//	u = _micros() - u;
 	return ret;
 }
@@ -689,25 +689,6 @@ uint8_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, u
 	return max_match;
 }
 
-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 == BOARD_EINSY_1_0a))
-const int16_t xyzcal_point_xcoords[4] PROGMEM = {1200, 22000, 22000, 1200};
-const int16_t xyzcal_point_ycoords[4] PROGMEM = {600, 600, 19800, 19800};
-#endif //((MOTHERBOARD == BOARD_EINSY_1_0a))
-
-//MK2.5
-#if ((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
-const int16_t xyzcal_point_xcoords[4] PROGMEM = {1200, 22000, 22000, 1200};
-const int16_t xyzcal_point_ycoords[4] PROGMEM = {700, 700, 19800, 19800};
-#endif //((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
-
 const uint16_t xyzcal_point_pattern_10[12] PROGMEM = {0x000, 0x0f0, 0x1f8, 0x3fc, 0x7fe, 0x7fe, 0x7fe, 0x7fe, 0x3fc, 0x1f8, 0x0f0, 0x000};
 const uint16_t xyzcal_point_pattern_08[12] PROGMEM = {0x000, 0x000, 0x0f0, 0x1f8, 0x3fc, 0x3fc, 0x3fc, 0x3fc, 0x1f8, 0x0f0, 0x000, 0x000};
 
@@ -939,8 +920,7 @@ bool xyzcal_scan_and_process(void){
 		float radius = 4.5f; ///< default radius
 		const uint8_t iterations = 20;
 		dynamic_circle(matrix32, xf, yf, radius, iterations);
-		if (ABS(xf - (uc + 5.5f)) > 3 || ABS(yf - (ur + 5.5f)) > 3 || ABS(radius - 5) > 3)
-		{
+		if (fabs(xf - (uc + 5.5f)) > 3 || fabs(yf - (ur + 5.5f)) > 3 || fabs(radius - 5) > 3){
 			DBG(_n(" [%f %f][%f] mm divergence\n"), xf - (uc + 5.5f), yf - (ur + 5.5f), radius - 5);
 			/// dynamic algorithm diverged, use original position instead
 			xf = uc + 5.5f;
@@ -974,13 +954,7 @@ bool xyzcal_find_bed_induction_sensor_point_xy(void){
 	///< magic constant, lowers min_z after searchZ to obtain more dense data in scan
 	const pos_i16_t lower_z = 72; 
 
-	uint8_t point = xyzcal_xycoords2point(x, y);
-	x = pgm_read_word((uint16_t *)(xyzcal_point_xcoords + point));
-	y = pgm_read_word((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()){
 		xyzcal_lineXYZ_to(_X, _Y, _Z - lower_z, 200, 0);
 		ret = xyzcal_scan_and_process();