@ -122,8 +122,8 @@ float Planner::min_feedrate_mm_s,
Planner : : max_jerk [ XYZE ] , // The largest speed change requiring no acceleration
Planner : : min_travel_feedrate_mm_s ;
# if OLDSCHOOL_ABL
bool Planner : : abl_enabled = false ; // Flag that auto bed leveling is enabled
# if HAS_LEVELING
bool Planner : : leveling_active = false ; // Flag that auto bed leveling is enabled
# if ABL_PLANAR
matrix_3x3 Planner : : bed_level_matrix ; // Transform to compensate for bed level
# endif
@ -131,7 +131,8 @@ float Planner::min_feedrate_mm_s,
# if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
float Planner : : z_fade_height , // Initialized by settings.load()
Planner : : inverse_z_fade_height ;
Planner : : inverse_z_fade_height ,
Planner : : last_raw_lz ;
# endif
# if ENABLED(AUTOTEMP)
@ -555,46 +556,31 @@ void Planner::calculate_volumetric_multipliers() {
*/
void Planner : : apply_leveling ( float & lx , float & ly , float & lz ) {
# if ENABLED(AUTO_BED_LEVELING_UBL)
if ( ! ubl . state . active ) return ;
# if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
// if z_fade_height enabled (nonzero) and raw_z above it, no leveling required
if ( planner . z_fade_height & & planner . z_fade_height < = RAW_Z_POSITION ( lz ) ) return ;
lz + = ubl . get_z_correction ( lx , ly ) * ubl . fade_scaling_factor_for_z ( lz ) ;
# else // no fade
lz + = ubl . get_z_correction ( lx , ly ) ;
# endif // FADE
# endif // UBL
# if OLDSCHOOL_ABL
if ( ! abl_enabled ) return ;
# endif
if ( ! planner . leveling_active ) return ;
# if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(AUTO_BED_LEVELING_UBL)
static float z_fade_factor = 1.0 , last_raw_lz = - 999.0 ;
if ( z_fade_height ) {
const float raw_lz = RAW_Z_POSITION ( lz ) ;
if ( raw_lz > = z_fade_height ) return ;
if ( last_raw_lz ! = raw_lz ) {
last_raw_lz = raw_lz ;
z_fade_factor = 1.0 - raw_lz * inverse_z_fade_height ;
}
}
else
z_fade_factor = 1.0 ;
# if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
const float fade_scaling_factor = fade_scaling_factor_for_z ( lz ) ;
if ( ! fade_scaling_factor ) return ;
# else
constexpr float fade_scaling_factor = 1.0 ;
# endif
# if ENABLED( MESH_BED_LEVELING )
# if ENABLED(AUTO_BED_LEVELING_UBL)
if ( mbl . active ( ) )
lz + = mbl . get_z ( RAW_X_POSITION ( lx ) , RAW_Y_POSITION ( ly )
# if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
, z_fade_factor
# endif
) ;
lz + = ubl . get_z_correction ( lx , ly ) * fade_scaling_factor ;
# elif ENABLED(MESH_BED_LEVELING)
lz + = mbl . get_z ( RAW_X_POSITION ( lx ) , RAW_Y_POSITION ( ly )
# if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
, fade_scaling_factor
# endif
) ;
# elif ABL_PLANAR
UNUSED ( fade_scaling_factor ) ;
float dx = RAW_X_POSITION ( lx ) - ( X_TILT_FULCRUM ) ,
dy = RAW_Y_POSITION ( ly ) - ( Y_TILT_FULCRUM ) ,
dz = RAW_Z_POSITION ( lz ) ;
@ -608,70 +594,55 @@ void Planner::calculate_volumetric_multipliers() {
# elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
float tmp [ XYZ ] = { lx , ly , 0 } ;
lz + = bilinear_z_offset ( tmp )
# if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
* z_fade_factor
# endif
;
lz + = bilinear_z_offset ( tmp ) * fade_scaling_factor ;
# endif
}
void Planner : : unapply_leveling ( float logical [ XYZ ] ) {
# if ENABLED(AUTO_BED_LEVELING_UBL)
if ( ubl . state . active ) {
if ( ! planner . leveling_active ) return ;
const float z_physical = RAW_Z_POSITION ( logical [ Z_AXIS ] ) ,
z_correct = ubl . get_z_correction ( logical [ X_AXIS ] , logical [ Y_AXIS ] ) ,
z_virtual = z_physical - z_correct ;
float z_logical = LOGICAL_Z_POSITION ( z_virtual ) ;
# if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
# if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
if ( z_fade_height & & RAW_Z_POSITION ( logical [ Z_AXIS ] ) > = z_fade_height ) return ;
# endif
// for P=physical_z, L=logical_z, M=mesh_z, H=fade_height,
// Given P=L+M(1-L/H) (faded mesh correction formula for L<H)
// then L=P-M(1-L/H)
// so L=P-M+ML/H
// so L-ML/H=P-M
// so L(1-M/H)=P-M
// so L=(P-M)/(1-M/H) for L<H
# if ENABLED(AUTO_BED_LEVELING_UBL)
if ( planner . z_fade_height ) {
if ( z_logical > = planner . z_fade_height )
z_logical = LOGICAL_Z_POSITION ( z_physical ) ;
else
z_logical / = 1.0 - z_correct * planner . inverse_z_fade_height ;
}
const float z_physical = RAW_Z_POSITION ( logical [ Z_AXIS ] ) ,
z_correct = ubl . get_z_correction ( logical [ X_AXIS ] , logical [ Y_AXIS ] ) ,
z_virtual = z_physical - z_correct ;
float z_logical = LOGICAL_Z_POSITION ( z_virtual ) ;
# endif // ENABLE_LEVELING_FADE_HEIGHT
# if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
logical [ Z_AXIS ] = z_logical ;
}
// for P=physical_z, L=logical_z, M=mesh_z, H=fade_height,
// Given P=L+M(1-L/H) (faded mesh correction formula for L<H)
// then L=P-M(1-L/H)
// so L=P-M+ML/H
// so L-ML/H=P-M
// so L(1-M/H)=P-M
// so L=(P-M)/(1-M/H) for L<H
if ( planner . z_fade_height ) {
if ( z_logical > = planner . z_fade_height )
z_logical = LOGICAL_Z_POSITION ( z_physical ) ;
else
z_logical / = 1.0 - z_correct * planner . inverse_z_fade_height ;
}
return ; // don't fall thru to other ENABLE_LEVELING_FADE_HEIGHT logic
# endif // ENABLE_LEVELING_FADE_HEIGHT
# endif
logical [ Z_AXIS ] = z_logical ;
# if OLDSCHOOL_ABL
if ( ! abl_enabled ) return ;
# endif
# elif ENABLED(MESH_BED_LEVELING)
# if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
if ( z_fade_height & & RAW_Z_POSITION ( logical [ Z_AXIS ] ) > = z_fade_height ) return ;
# endif
# if ENABLED(MESH_BED_LEVELING)
if ( mbl . active ( ) ) {
# if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
const float c = mbl . get_z ( RAW_X_POSITION ( logical [ X_AXIS ] ) , RAW_Y_POSITION ( logical [ Y_AXIS ] ) , 1.0 ) ;
logical [ Z_AXIS ] = ( z_fade_height * ( RAW_Z_POSITION ( logical [ Z_AXIS ] ) - c ) ) / ( z_fade_height - c ) ;
# else
logical [ Z_AXIS ] - = mbl . get_z ( RAW_X_POSITION ( logical [ X_AXIS ] ) , RAW_Y_POSITION ( logical [ Y_AXIS ] ) ) ;
# endif
}
# if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
const float c = mbl . get_z ( RAW_X_POSITION ( logical [ X_AXIS ] ) , RAW_Y_POSITION ( logical [ Y_AXIS ] ) , 1.0 ) ;
logical [ Z_AXIS ] = ( z_fade_height * ( RAW_Z_POSITION ( logical [ Z_AXIS ] ) - c ) ) / ( z_fade_height - c ) ;
# else
logical [ Z_AXIS ] - = mbl . get_z ( RAW_X_POSITION ( logical [ X_AXIS ] ) , RAW_Y_POSITION ( logical [ Y_AXIS ] ) ) ;
# endif
# elif ABL_PLANAR
Scott Lahteine
7 years ago
committed by