@ -1685,6 +1685,14 @@ static void setup_for_endstop_move() {
do_blocking_move_to ( current_position [ X_AXIS ] , current_position [ Y_AXIS ] , z ) ;
}
inline void raise_z_after_probing ( ) {
# if Z_RAISE_AFTER_PROBING > 0
# if ENABLED(DEBUG_LEVELING_FEATURE)
if ( DEBUGGING ( LEVELING ) ) SERIAL_ECHOLNPGM ( " raise_z_after_probing() " ) ;
# endif
do_blocking_move_to_z ( current_position [ Z_AXIS ] + Z_RAISE_AFTER_PROBING ) ;
# endif
}
# endif //HAS_BED_PROBE
# if HAS_Z_SERVO_ENDSTOP
@ -1710,18 +1718,6 @@ static void setup_for_endstop_move() {
# endif
# if HAS_BED_PROBE
inline void raise_z_after_probing ( ) {
# if Z_RAISE_AFTER_PROBING > 0
# if ENABLED(DEBUG_LEVELING_FEATURE)
if ( DEBUGGING ( LEVELING ) ) SERIAL_ECHOLNPGM ( " raise_z_after_probing() " ) ;
# endif
do_blocking_move_to_z ( current_position [ Z_AXIS ] + Z_RAISE_AFTER_PROBING ) ;
# endif
}
# endif
# if ENABLED(Z_PROBE_SLED)
# ifndef SLED_DOCKING_OFFSET
@ -1964,89 +1960,6 @@ static void setup_for_endstop_move() {
endstops . enable_z_probe ( false ) ;
}
# endif // HAS_BED_PROBE
# if ENABLED(AUTO_BED_LEVELING_FEATURE)
# if ENABLED(AUTO_BED_LEVELING_GRID)
# if DISABLED(DELTA)
static void set_bed_level_equation_lsq ( double * plane_equation_coefficients ) {
//planner.bed_level_matrix.debug("bed level before");
# if ENABLED(DEBUG_LEVELING_FEATURE)
planner . bed_level_matrix . set_to_identity ( ) ;
if ( DEBUGGING ( LEVELING ) ) {
vector_3 uncorrected_position = planner . adjusted_position ( ) ;
DEBUG_POS ( " >>> set_bed_level_equation_lsq " , uncorrected_position ) ;
DEBUG_POS ( " >>> set_bed_level_equation_lsq " , current_position ) ;
}
# endif
vector_3 planeNormal = vector_3 ( - plane_equation_coefficients [ 0 ] , - plane_equation_coefficients [ 1 ] , 1 ) ;
planner . bed_level_matrix = matrix_3x3 : : create_look_at ( planeNormal ) ;
vector_3 corrected_position = planner . adjusted_position ( ) ;
current_position [ X_AXIS ] = corrected_position . x ;
current_position [ Y_AXIS ] = corrected_position . y ;
current_position [ Z_AXIS ] = corrected_position . z ;
# if ENABLED(DEBUG_LEVELING_FEATURE)
if ( DEBUGGING ( LEVELING ) ) DEBUG_POS ( " <<< set_bed_level_equation_lsq " , corrected_position ) ;
# endif
sync_plan_position ( ) ;
}
# endif // !DELTA
# else // !AUTO_BED_LEVELING_GRID
static void set_bed_level_equation_3pts ( float z_at_pt_1 , float z_at_pt_2 , float z_at_pt_3 ) {
planner . bed_level_matrix . set_to_identity ( ) ;
vector_3 pt1 = vector_3 ( ABL_PROBE_PT_1_X , ABL_PROBE_PT_1_Y , z_at_pt_1 ) ;
vector_3 pt2 = vector_3 ( ABL_PROBE_PT_2_X , ABL_PROBE_PT_2_Y , z_at_pt_2 ) ;
vector_3 pt3 = vector_3 ( ABL_PROBE_PT_3_X , ABL_PROBE_PT_3_Y , z_at_pt_3 ) ;
vector_3 planeNormal = vector_3 : : cross ( pt1 - pt2 , pt3 - pt2 ) . get_normal ( ) ;
if ( planeNormal . z < 0 ) {
planeNormal . x = - planeNormal . x ;
planeNormal . y = - planeNormal . y ;
planeNormal . z = - planeNormal . z ;
}
planner . bed_level_matrix = matrix_3x3 : : create_look_at ( planeNormal ) ;
vector_3 corrected_position = planner . adjusted_position ( ) ;
# if ENABLED(DEBUG_LEVELING_FEATURE)
if ( DEBUGGING ( LEVELING ) ) {
vector_3 uncorrected_position = corrected_position ;
DEBUG_POS ( " set_bed_level_equation_3pts " , uncorrected_position ) ;
}
# endif
current_position [ X_AXIS ] = corrected_position . x ;
current_position [ Y_AXIS ] = corrected_position . y ;
current_position [ Z_AXIS ] = corrected_position . z ;
# if ENABLED(DEBUG_LEVELING_FEATURE)
if ( DEBUGGING ( LEVELING ) ) DEBUG_POS ( " set_bed_level_equation_3pts " , corrected_position ) ;
# endif
sync_plan_position ( ) ;
}
# endif // !AUTO_BED_LEVELING_GRID
# endif // AUTO_BED_LEVELING_FEATURE
# if HAS_BED_PROBE
static void run_z_probe ( ) {
float old_feedrate = feedrate ;
@ -2138,6 +2051,81 @@ static void setup_for_endstop_move() {
# if ENABLED(AUTO_BED_LEVELING_FEATURE)
# if ENABLED(AUTO_BED_LEVELING_GRID)
# if DISABLED(DELTA)
static void set_bed_level_equation_lsq ( double * plane_equation_coefficients ) {
//planner.bed_level_matrix.debug("bed level before");
# if ENABLED(DEBUG_LEVELING_FEATURE)
planner . bed_level_matrix . set_to_identity ( ) ;
if ( DEBUGGING ( LEVELING ) ) {
vector_3 uncorrected_position = planner . adjusted_position ( ) ;
DEBUG_POS ( " >>> set_bed_level_equation_lsq " , uncorrected_position ) ;
DEBUG_POS ( " >>> set_bed_level_equation_lsq " , current_position ) ;
}
# endif
vector_3 planeNormal = vector_3 ( - plane_equation_coefficients [ 0 ] , - plane_equation_coefficients [ 1 ] , 1 ) ;
planner . bed_level_matrix = matrix_3x3 : : create_look_at ( planeNormal ) ;
vector_3 corrected_position = planner . adjusted_position ( ) ;
current_position [ X_AXIS ] = corrected_position . x ;
current_position [ Y_AXIS ] = corrected_position . y ;
current_position [ Z_AXIS ] = corrected_position . z ;
# if ENABLED(DEBUG_LEVELING_FEATURE)
if ( DEBUGGING ( LEVELING ) ) DEBUG_POS ( " <<< set_bed_level_equation_lsq " , corrected_position ) ;
# endif
sync_plan_position ( ) ;
}
# endif // !DELTA
# else // !AUTO_BED_LEVELING_GRID
static void set_bed_level_equation_3pts ( float z_at_pt_1 , float z_at_pt_2 , float z_at_pt_3 ) {
planner . bed_level_matrix . set_to_identity ( ) ;
vector_3 pt1 = vector_3 ( ABL_PROBE_PT_1_X , ABL_PROBE_PT_1_Y , z_at_pt_1 ) ;
vector_3 pt2 = vector_3 ( ABL_PROBE_PT_2_X , ABL_PROBE_PT_2_Y , z_at_pt_2 ) ;
vector_3 pt3 = vector_3 ( ABL_PROBE_PT_3_X , ABL_PROBE_PT_3_Y , z_at_pt_3 ) ;
vector_3 planeNormal = vector_3 : : cross ( pt1 - pt2 , pt3 - pt2 ) . get_normal ( ) ;
if ( planeNormal . z < 0 ) {
planeNormal . x = - planeNormal . x ;
planeNormal . y = - planeNormal . y ;
planeNormal . z = - planeNormal . z ;
}
planner . bed_level_matrix = matrix_3x3 : : create_look_at ( planeNormal ) ;
vector_3 corrected_position = planner . adjusted_position ( ) ;
# if ENABLED(DEBUG_LEVELING_FEATURE)
if ( DEBUGGING ( LEVELING ) ) {
vector_3 uncorrected_position = corrected_position ;
DEBUG_POS ( " set_bed_level_equation_3pts " , uncorrected_position ) ;
}
# endif
current_position [ X_AXIS ] = corrected_position . x ;
current_position [ Y_AXIS ] = corrected_position . y ;
current_position [ Z_AXIS ] = corrected_position . z ;
# if ENABLED(DEBUG_LEVELING_FEATURE)
if ( DEBUGGING ( LEVELING ) ) DEBUG_POS ( " set_bed_level_equation_3pts " , corrected_position ) ;
# endif
sync_plan_position ( ) ;
}
# endif // !AUTO_BED_LEVELING_GRID
inline void do_blocking_move_to_xy ( float x , float y ) {
do_blocking_move_to ( x , y , current_position [ Z_AXIS ] ) ;
}
@ -4176,19 +4164,7 @@ inline void gcode_M42() {
} // code_seen('S')
}
# if ENABLED(AUTO_BED_LEVELING_FEATURE) && ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)
/**
* This is redundant since the SanityCheck . h already checks for a valid
* Z_MIN_PROBE_PIN , but here for clarity .
*/
# if ENABLED(Z_MIN_PROBE_ENDSTOP)
# if !HAS_Z_MIN_PROBE_PIN
# error "You must define Z_MIN_PROBE_PIN to enable Z probe repeatability calculation."
# endif
# elif !HAS_Z_MIN
# error "You must define Z_MIN_PIN to enable Z probe repeatability calculation."
# endif
# if ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)
/**
* M48 : Z probe repeatability measurement function .
@ -4451,7 +4427,7 @@ inline void gcode_M42() {
report_current_position ( ) ;
}
# endif // AUTO_BED_LEVELING_FEATURE && Z_MIN_PROBE_REPEATABILITY_TEST
# endif // Z_MIN_PROBE_REPEATABILITY_TEST
/**
* M75 : Start print timer
@ -6970,11 +6946,11 @@ void process_next_command() {
gcode_M42 ( ) ;
break ;
# if ENABLED( AUTO_BED_LEVELING_FEATURE) && ENABLED( Z_MIN_PROBE_REPEATABILITY_TEST)
# if ENABLED( Z_MIN_PROBE_REPEATABILITY_TEST)
case 48 : // M48 Z probe repeatability
gcode_M48 ( ) ;
break ;
# endif // AUTO_BED_LEVELING_FEATURE && Z_MIN_PROBE_REPEATABILITY_TEST
# endif // Z_MIN_PROBE_REPEATABILITY_TEST
case 75 : // Start print timer
gcode_M75 ( ) ;