From 9f295581f0d6ae274a1f7be683cdc175681bec80 Mon Sep 17 00:00:00 2001 From: oldmcg Date: Sat, 3 Jun 2017 17:11:43 -0500 Subject: [PATCH] Ubl delta fixes and improvements (#6941) * Change all direct changes of ubl.state.active to set_bed_leveling_enabled() which handles apply/unapply leveling to maintain current_position consistency. Fix invalidation of UBL mesh to invalid unreachable mesh points as well (delta corners). Fix UBL_DELTA unapply_leveling logic and when it gets applied, including fade_height changes. Add optional M114 D for detailed position information, disabled from compilation by default (M114_DETAIL). * UBL_DELTA raw and inline kinematics * UBL planner fall through fix * consistent variable names * Cleanup orphaned code and whitespace changes. Use _O2. * compile warnings cleanup * Remove redundant #ifdef condition --- Marlin/G26_Mesh_Validation_Tool.cpp | 5 +- Marlin/Marlin_main.cpp | 207 ++++++++++++++++++++++++---- Marlin/planner.cpp | 38 ++--- Marlin/ubl.cpp | 16 ++- Marlin/ubl.h | 3 +- Marlin/ubl_G29.cpp | 61 +++++--- Marlin/ubl_motion.cpp | 206 ++++++++++++++++----------- 7 files changed, 380 insertions(+), 156 deletions(-) diff --git a/Marlin/G26_Mesh_Validation_Tool.cpp b/Marlin/G26_Mesh_Validation_Tool.cpp index 23bb34198..7cdfab55d 100644 --- a/Marlin/G26_Mesh_Validation_Tool.cpp +++ b/Marlin/G26_Mesh_Validation_Tool.cpp @@ -718,7 +718,7 @@ /** * Wait until all parameters are verified before altering the state! */ - state.active = !parser.seen('D'); + set_bed_leveling_enabled(!parser.seen('D')); return UBL_OK; } @@ -734,7 +734,7 @@ * wait for them to get up to temperature. */ bool unified_bed_leveling::turn_on_heaters() { - millis_t next; + millis_t next = millis() + 5000UL; #if HAS_TEMP_BED #if ENABLED(ULTRA_LCD) if (g26_bed_temp > 25) { @@ -743,7 +743,6 @@ #endif has_control_of_lcd_panel = true; thermalManager.setTargetBed(g26_bed_temp); - next = millis() + 5000UL; while (abs(thermalManager.degBed() - g26_bed_temp) > 3) { if (ubl_lcd_clicked()) return exit_from_g26(); if (PENDING(millis(), next)) { diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 3acb1a0d0..6fbc9a60b 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -699,7 +699,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis); #endif void tool_change(const uint8_t tmp_extruder, const float fr_mm_s=0.0, bool no_move=false); -static void report_current_position(); +void report_current_position(); +void report_current_position_detail(); #if ENABLED(DEBUG_LEVELING_FEATURE) void print_xyz(const char* prefix, const char* suffix, const float x, const float y, const float z) { @@ -1536,14 +1537,21 @@ inline void set_destination_to_current() { COPY(destination, current_position); if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_uninterpolated_move_to_destination", destination); #endif - if ( current_position[X_AXIS] == destination[X_AXIS] - && current_position[Y_AXIS] == destination[Y_AXIS] - && current_position[Z_AXIS] == destination[Z_AXIS] - && current_position[E_AXIS] == destination[E_AXIS] - ) return; - refresh_cmd_timeout(); - planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder); + + #if UBL_DELTA + // ubl segmented line will do z-only moves in single segment + ubl.prepare_segmented_line_to(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s)); + #else + if ( current_position[X_AXIS] == destination[X_AXIS] + && current_position[Y_AXIS] == destination[Y_AXIS] + && current_position[Z_AXIS] == destination[Z_AXIS] + && current_position[E_AXIS] == destination[E_AXIS] + ) return; + + planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder); + #endif + set_current_to_destination(); } #endif // IS_KINEMATIC @@ -2345,18 +2353,21 @@ static void clean_up_after_endstop_or_probe_move() { if (enabling) planner.unapply_leveling(current_position); #elif ENABLED(AUTO_BED_LEVELING_UBL) - #if PLANNER_LEVELING - - if (!enable) // leveling from on to off + if (ubl.state.active) { // leveling from on to off + // change unleveled current_position to physical current_position without moving steppers. planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]); - else - planner.unapply_leveling(current_position); - + ubl.state.active = false; // disable only AFTER calling apply_leveling + } + else { // leveling from off to on + ubl.state.active = true; // enable BEFORE calling unapply_leveling, otherwise ignored + // change physical current_position to unleveled current_position without moving steppers. + planner.unapply_leveling(current_position); + } + #else + ubl.state.active = enable; // just flip the bit, current_position will be wrong until next move. #endif - ubl.state.active = enable; - #else // ABL #if ENABLED(AUTO_BED_LEVELING_BILINEAR) @@ -2384,17 +2395,34 @@ static void clean_up_after_endstop_or_probe_move() { #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) void set_z_fade_height(const float zfh) { - planner.z_fade_height = zfh; - planner.inverse_z_fade_height = RECIPROCAL(zfh); - if (leveling_is_active()) - set_current_from_steppers_for_axis( - #if ABL_PLANAR - ALL_AXES - #else - Z_AXIS - #endif - ); + #if ENABLED(AUTO_BED_LEVELING_UBL) + + const bool level_active = leveling_is_active(); + if (level_active) { + set_bed_leveling_enabled(false); // turn off before changing fade height for proper apply/unapply leveling to maintain current_position + } + planner.z_fade_height = zfh; + planner.inverse_z_fade_height = RECIPROCAL(zfh); + if (level_active) { + set_bed_leveling_enabled(true); // turn back on after changing fade height + } + + #else + + planner.z_fade_height = zfh; + planner.inverse_z_fade_height = RECIPROCAL(zfh); + + if (leveling_is_active()) { + set_current_from_steppers_for_axis( + #if ABL_PLANAR + ALL_AXES + #else + Z_AXIS + #endif + ); + } + #endif } #endif // LEVELING_FADE_HEIGHT @@ -3656,6 +3684,7 @@ inline void gcode_G28(const bool always_home_all) { #if ENABLED(DELTA) home_delta(); + UNUSED(always_home_all); #else // NOT DELTA @@ -7592,7 +7621,7 @@ inline void gcode_M92() { /** * Output the current position to serial */ -static void report_current_position() { +void report_current_position() { SERIAL_PROTOCOLPGM("X:"); SERIAL_PROTOCOL(current_position[X_AXIS]); SERIAL_PROTOCOLPGM(" Y:"); @@ -7611,10 +7640,119 @@ static void report_current_position() { #endif } +#ifdef M114_DETAIL + + static const char axis_char[XYZE] = {'X','Y','Z','E'}; + + void report_xyze(const float pos[XYZE], uint8_t n = 4, uint8_t precision = 3) { + char str[12]; + for(uint8_t i=0; i= planner.z_fade_height) + z_logical = LOGICAL_Z_POSITION(z_physical - ubl.state.z_offset); } #endif // ENABLE_LEVELING_FADE_HEIGHT - logical[Z_AXIS] = z_unlevel; + logical[Z_AXIS] = z_logical; } - return; // don't fall thru to HAS_ABL or other ENABLE_LEVELING_FADE_HEIGHT logic + return; // don't fall thru to other ENABLE_LEVELING_FADE_HEIGHT logic #endif diff --git a/Marlin/ubl.cpp b/Marlin/ubl.cpp index cdfc401ad..e243c6d88 100644 --- a/Marlin/ubl.cpp +++ b/Marlin/ubl.cpp @@ -83,7 +83,7 @@ } void unified_bed_leveling::reset() { - state.active = false; + set_bed_leveling_enabled(false); state.z_offset = 0; state.storage_slot = -1; #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) @@ -94,11 +94,17 @@ } void unified_bed_leveling::invalidate() { - state.active = false; + set_bed_leveling_enabled(false); state.z_offset = 0; - for (int x = 0; x < GRID_MAX_POINTS_X; x++) - for (int y = 0; y < GRID_MAX_POINTS_Y; y++) - z_values[x][y] = NAN; + set_all_mesh_points_to_value(NAN); + } + + void unified_bed_leveling::set_all_mesh_points_to_value(float value) { + for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) { + for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) { + z_values[x][y] = value; + } + } } void unified_bed_leveling::display_map(const int map_type) { diff --git a/Marlin/ubl.h b/Marlin/ubl.h index 028eff52f..29ee83c35 100644 --- a/Marlin/ubl.h +++ b/Marlin/ubl.h @@ -154,6 +154,7 @@ static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, unsigned int[16], bool); static void reset(); static void invalidate(); + static void set_all_mesh_points_to_value(float); static bool sanity_check(); static void G29() _O0; // O0 for no optimization @@ -385,7 +386,7 @@ FORCE_INLINE static float mesh_index_to_xpos(const uint8_t i) { return pgm_read_float(&_mesh_index_to_xpos[i]); } FORCE_INLINE static float mesh_index_to_ypos(const uint8_t i) { return pgm_read_float(&_mesh_index_to_ypos[i]); } - static bool prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate); + static bool prepare_segmented_line_to(const float ltarget[XYZE], const float &feedrate); static void line_to_destination_cartesian(const float &fr, uint8_t e); }; // class unified_bed_leveling diff --git a/Marlin/ubl_G29.cpp b/Marlin/ubl_G29.cpp index d6453a948..9201fe910 100644 --- a/Marlin/ubl_G29.cpp +++ b/Marlin/ubl_G29.cpp @@ -30,6 +30,7 @@ #include "configuration_store.h" #include "ultralcd.h" #include "stepper.h" + #include "planner.h" #include "gcode.h" #include @@ -48,6 +49,7 @@ extern long babysteps_done; extern float probe_pt(const float &x, const float &y, bool, int); extern bool set_probe_deployed(bool); + extern void set_bed_leveling_enabled(bool); #define SIZE_OF_LITTLE_RAISE 1 #define BIG_RAISE_NOT_NEEDED 0 @@ -325,15 +327,23 @@ if (parser.seen('I')) { uint8_t cnt = 0; g29_repetition_cnt = parser.has_value() ? parser.value_int() : 1; - while (g29_repetition_cnt--) { - if (cnt > 20) { cnt = 0; idle(); } - const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false); - if (location.x_index < 0) { - SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n"); - break; // No more invalid Mesh Points to populate + if (g29_repetition_cnt >= GRID_MAX_POINTS) { + set_all_mesh_points_to_value(NAN); + } else { + while (g29_repetition_cnt--) { + if (cnt > 20) { cnt = 0; idle(); } + const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false); + if (location.x_index < 0) { + // No more REACHABLE mesh points to invalidate, so we ASSUME the user + // meant to invalidate the ENTIRE mesh, which cannot be done with + // find_closest_mesh_point loop which only returns REACHABLE points. + set_all_mesh_points_to_value(NAN); + SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n"); + break; // No more invalid Mesh Points to populate + } + z_values[location.x_index][location.y_index] = NAN; + cnt++; } - z_values[location.x_index][location.y_index] = NAN; - cnt++; } SERIAL_PROTOCOLLNPGM("Locations invalidated.\n"); } @@ -497,18 +507,26 @@ * - Specify a constant with the 'C' parameter. * - Allow 'G29 P3' to choose a 'reasonable' constant. */ + if (g29_c_flag) { if (g29_repetition_cnt >= GRID_MAX_POINTS) { - for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) { - for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) { - z_values[x][y] = g29_constant; - } - } + set_all_mesh_points_to_value(g29_constant); } else { while (g29_repetition_cnt--) { // this only populates reachable mesh points near const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false); - if (location.x_index < 0) break; // No more reachable invalid Mesh Points to populate + if (location.x_index < 0) { + // No more REACHABLE INVALID mesh points to populate, so we ASSUME + // user meant to populate ALL INVALID mesh points to value + for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) { + for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) { + if ( isnan(z_values[x][y])) { + z_values[x][y] = g29_constant; + } + } + } + break; // No more invalid Mesh Points to populate + } z_values[location.x_index][location.y_index] = g29_constant; } } @@ -999,12 +1017,15 @@ serialprintPGM(parser.seen('B') ? PSTR("Place shim & measure") : PSTR("Measure")); // TODO: Make translatable strings + const float z_step = 0.01; // existing behavior: 0.01mm per click, occasionally step + //const float z_step = 1.0 / planner.axis_steps_per_mm[Z_AXIS]; // approx one step each click + while (ubl_lcd_clicked()) delay(50); // wait for user to release encoder wheel delay(50); // debounce while (!ubl_lcd_clicked()) { // we need the loop to move the nozzle based on the encoder wheel here! idle(); if (encoder_diff) { - do_blocking_move_to_z(current_position[Z_AXIS] + float(encoder_diff) / 100.0); + do_blocking_move_to_z(current_position[Z_AXIS] + float(encoder_diff) * z_step); encoder_diff = 0; } } @@ -1115,11 +1136,11 @@ SERIAL_PROTOCOLLNPGM("?Can't activate and deactivate at the same time.\n"); return UBL_ERR; } - state.active = true; + set_bed_leveling_enabled(true); report_state(); } else if (parser.seen('D')) { - state.active = false; + set_bed_leveling_enabled(false); report_state(); } @@ -1158,7 +1179,7 @@ return; } ubl_state_at_invocation = state.active; - state.active = 0; + set_bed_leveling_enabled(false); } void unified_bed_leveling::restore_ubl_active_state_and_leave() { @@ -1168,7 +1189,7 @@ lcd_quick_feedback(); return; } - state.active = ubl_state_at_invocation; + set_bed_leveling_enabled(ubl_state_at_invocation); } /** @@ -1695,6 +1716,8 @@ SERIAL_EOL; } #endif + + if (do_ubl_mesh_map) display_map(g29_map_type); } #if ENABLED(UBL_G29_P31) diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp index 5131315e1..3eccee9b5 100644 --- a/Marlin/ubl_motion.cpp +++ b/Marlin/ubl_motion.cpp @@ -32,7 +32,25 @@ extern float destination[XYZE]; extern void set_current_to_destination(); - extern float delta_segments_per_second; + +#if ENABLED(DELTA) + + extern float delta[ABC], + endstop_adj[ABC]; + + extern float delta_radius, + delta_tower_angle_trim[2], + delta_tower[ABC][2], + delta_diagonal_rod, + delta_calibration_radius, + delta_diagonal_rod_2_tower[ABC], + delta_segments_per_second, + delta_clip_start_height; + + extern float delta_safe_distance_from_top(); + +#endif + static void debug_echo_axis(const AxisEnum axis) { if (current_position[axis] == destination[axis]) @@ -470,51 +488,76 @@ #endif // We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic, - // so we call _buffer_line directly here. Per-segmented leveling performed first. + // so we call _buffer_line directly here. Per-segmented leveling and kinematics performed first. - static inline void ubl_buffer_line_segment(const float ltarget[XYZE], const float &fr_mm_s, const uint8_t extruder) { + inline void _O2 ubl_buffer_segment_raw( float rx, float ry, float rz, float le, float fr ) { - #if IS_KINEMATIC + #if ENABLED(DELTA) // apply delta inverse_kinematics - inverse_kinematics(ltarget); // this writes delta[ABC] from ltarget[XYZ] but does not modify ltarget - float feedrate = fr_mm_s; + const float delta_A = rz + sqrt( delta_diagonal_rod_2_tower[A_AXIS] + - HYPOT2( delta_tower[A_AXIS][X_AXIS] - rx, + delta_tower[A_AXIS][Y_AXIS] - ry )); - #if IS_SCARA // scale the feed rate from mm/s to degrees/s - float adiff = abs(delta[A_AXIS] - scara_oldA), - bdiff = abs(delta[B_AXIS] - scara_oldB); - scara_oldA = delta[A_AXIS]; - scara_oldB = delta[B_AXIS]; - feedrate = max(adiff, bdiff) * scara_feed_factor; - #endif + const float delta_B = rz + sqrt( delta_diagonal_rod_2_tower[B_AXIS] + - HYPOT2( delta_tower[B_AXIS][X_AXIS] - rx, + delta_tower[B_AXIS][Y_AXIS] - ry )); - planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], feedrate, extruder); + const float delta_C = rz + sqrt( delta_diagonal_rod_2_tower[C_AXIS] + - HYPOT2( delta_tower[C_AXIS][X_AXIS] - rx, + delta_tower[C_AXIS][Y_AXIS] - ry )); - #else // cartesian + planner._buffer_line(delta_A, delta_B, delta_C, le, fr, active_extruder); - planner._buffer_line(ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS], ltarget[E_AXIS], fr_mm_s, extruder); + #elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw) + + const float lseg[XYZ] = { LOGICAL_X_POSITION(rx), + LOGICAL_Y_POSITION(ry), + LOGICAL_Z_POSITION(rz) + }; + + inverse_kinematics(lseg); // this writes delta[ABC] from lseg[XYZ] + // should move the feedrate scaling to scara inverse_kinematics + + float adiff = abs(delta[A_AXIS] - scara_oldA), + bdiff = abs(delta[B_AXIS] - scara_oldB); + scara_oldA = delta[A_AXIS]; + scara_oldB = delta[B_AXIS]; + float s_feedrate = max(adiff, bdiff) * scara_feed_factor; + + planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], le, s_feedrate, active_extruder); + + #else // CARTESIAN + + // Cartesian _buffer_line seems to take LOGICAL, not RAW coordinates + + const float lx = LOGICAL_X_POSITION(rx), + ly = LOGICAL_Y_POSITION(ry), + lz = LOGICAL_Z_POSITION(rz); + + planner._buffer_line(lx, ly, lz, le, fr, active_extruder); #endif + } + /** - * Prepare a linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics. + * Prepare a segmented linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics. * This calls planner._buffer_line multiple times for small incremental moves. - * Returns true if the caller did NOT update current_position, otherwise false. + * Returns true if did NOT move, false if moved (requires current_position update). */ - static bool unified_bed_leveling::prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate) { + bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float ltarget[XYZE], const float &feedrate) { if (!position_is_reachable_xy(ltarget[X_AXIS], ltarget[Y_AXIS])) // fail if moving outside reachable boundary return true; // did not move, so current_position still accurate - const float difference[XYZE] = { // cartesian distances moved in XYZE - ltarget[X_AXIS] - current_position[X_AXIS], - ltarget[Y_AXIS] - current_position[Y_AXIS], - ltarget[Z_AXIS] - current_position[Z_AXIS], - ltarget[E_AXIS] - current_position[E_AXIS] - }; + const float tot_dx = ltarget[X_AXIS] - current_position[X_AXIS], + tot_dy = ltarget[Y_AXIS] - current_position[Y_AXIS], + tot_dz = ltarget[Z_AXIS] - current_position[Z_AXIS], + tot_de = ltarget[E_AXIS] - current_position[E_AXIS]; - const float cartesian_xy_mm = HYPOT(difference[X_AXIS], difference[Y_AXIS]); // total horizontal xy distance + const float cartesian_xy_mm = HYPOT(tot_dx, tot_dy); // total horizontal xy distance #if IS_KINEMATIC const float seconds = cartesian_xy_mm / feedrate; // seconds to move xy distance at requested rate @@ -534,16 +577,19 @@ scara_oldB = stepper.get_axis_position_degrees(B_AXIS); #endif - const float segment_distance[XYZE] = { // length for each segment - difference[X_AXIS] * inv_segments, - difference[Y_AXIS] * inv_segments, - difference[Z_AXIS] * inv_segments, - difference[E_AXIS] * inv_segments - }; + const float seg_dx = tot_dx * inv_segments, + seg_dy = tot_dy * inv_segments, + seg_dz = tot_dz * inv_segments, + seg_de = tot_de * inv_segments; // Note that E segment distance could vary slightly as z mesh height // changes for each segment, but small enough to ignore. + float seg_rx = RAW_X_POSITION(current_position[X_AXIS]), + seg_ry = RAW_Y_POSITION(current_position[Y_AXIS]), + seg_rz = RAW_Z_POSITION(current_position[Z_AXIS]), + seg_le = current_position[E_AXIS]; + const bool above_fade_height = ( #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) planner.z_fade_height != 0 && planner.z_fade_height < RAW_Z_POSITION(ltarget[Z_AXIS]) @@ -558,21 +604,24 @@ const float z_offset = state.active ? state.z_offset : 0.0; - float seg_dest[XYZE]; // per-segment destination, - COPY_XYZE(seg_dest, current_position); // starting from current position + do { - while (--segments) { - LOOP_XYZE(i) seg_dest[i] += segment_distance[i]; - float ztemp = seg_dest[Z_AXIS]; - seg_dest[Z_AXIS] += z_offset; - ubl_buffer_line_segment(seg_dest, feedrate, active_extruder); - seg_dest[Z_AXIS] = ztemp; - } + if (--segments) { // not the last segment + seg_rx += seg_dx; + seg_ry += seg_dy; + seg_rz += seg_dz; + seg_le += seg_de; + } else { // last segment, use exact destination + seg_rx = RAW_X_POSITION(ltarget[X_AXIS]); + seg_ry = RAW_Y_POSITION(ltarget[Y_AXIS]); + seg_rz = RAW_Z_POSITION(ltarget[Z_AXIS]); + seg_le = ltarget[E_AXIS]; + } + + ubl_buffer_segment_raw( seg_rx, seg_ry, seg_rz + z_offset, seg_le, feedrate ); + + } while (segments); - // Since repeated adding segment_distance accumulates small errors, final move to exact destination. - COPY_XYZE(seg_dest, ltarget); - seg_dest[Z_AXIS] += z_offset; - ubl_buffer_line_segment(seg_dest, feedrate, active_extruder); return false; // moved but did not set_current_to_destination(); } @@ -582,14 +631,11 @@ const float fade_scaling_factor = fade_scaling_factor_for_z(ltarget[Z_AXIS]); #endif - float seg_dest[XYZE]; // per-segment destination, initialize to first segment - LOOP_XYZE(i) seg_dest[i] = current_position[i] + segment_distance[i]; - - const float &dx_seg = segment_distance[X_AXIS]; // alias for clarity - const float &dy_seg = segment_distance[Y_AXIS]; - - float rx = RAW_X_POSITION(seg_dest[X_AXIS]), // assume raw vs logical coordinates shifted but not scaled. - ry = RAW_Y_POSITION(seg_dest[Y_AXIS]); + // increment to first segment destination + seg_rx += seg_dx; + seg_ry += seg_dy; + seg_rz += seg_dz; + seg_le += seg_de; for(;;) { // for each mesh cell encountered during the move @@ -600,20 +646,16 @@ // in top of loop and again re-find same adjacent cell and use it, just less efficient // for mesh inset area. - int8_t cell_xi = (rx - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)), - cell_yi = (ry - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_X_DIST)); + int8_t cell_xi = (seg_rx - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)), + cell_yi = (seg_ry - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_X_DIST)); cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1); cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1); - const float x0 = mesh_index_to_xpos(cell_xi), // 64 byte table lookup avoids mul+add - y0 = mesh_index_to_ypos(cell_yi), // 64 byte table lookup avoids mul+add - x1 = mesh_index_to_xpos(cell_xi + 1), // 64 byte table lookup avoids mul+add - y1 = mesh_index_to_ypos(cell_yi + 1); // 64 byte table lookup avoids mul+add + const float x0 = mesh_index_to_xpos(cell_xi), // 64 byte table lookup avoids mul+add + y0 = mesh_index_to_ypos(cell_yi); - float cx = rx - x0, // cell-relative x - cy = ry - y0, // cell-relative y - z_x0y0 = z_values[cell_xi ][cell_yi ], // z at lower left corner + float z_x0y0 = z_values[cell_xi ][cell_yi ], // z at lower left corner z_x1y0 = z_values[cell_xi+1][cell_yi ], // z at upper left corner z_x0y1 = z_values[cell_xi ][cell_yi+1], // z at lower right corner z_x1y1 = z_values[cell_xi+1][cell_yi+1]; // z at upper right corner @@ -623,15 +665,18 @@ if (isnan(z_x0y1)) z_x0y1 = 0; // in order to avoid isnan tests per cell, if (isnan(z_x1y1)) z_x1y1 = 0; // thus guessing zero for undefined points + float cx = seg_rx - x0, // cell-relative x and y + cy = seg_ry - y0; + const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0 / (MESH_X_DIST)), // z slope per x along y0 (lower left to lower right) z_xmy1 = (z_x1y1 - z_x0y1) * (1.0 / (MESH_X_DIST)); // z slope per x along y1 (upper left to upper right) - float z_cxy0 = z_x0y0 + z_xmy0 * cx; // z height along y0 at cx + float z_cxy0 = z_x0y0 + z_xmy0 * cx; // z height along y0 at cx (changes for each cx in cell) const float z_cxy1 = z_x0y1 + z_xmy1 * cx, // z height along y1 at cx z_cxyd = z_cxy1 - z_cxy0; // z height difference along cx from y0 to y1 - float z_cxym = z_cxyd * (1.0 / (MESH_Y_DIST)); // z slope per y along cx from y0 to y1 + float z_cxym = z_cxyd * (1.0 / (MESH_Y_DIST)); // z slope per y along cx from y0 to y1 (changes for each cx in cell) // float z_cxcy = z_cxy0 + z_cxym * cy; // interpolated mesh z height along cx at cy (do inside the segment loop) @@ -639,8 +684,8 @@ // and the z_cxym slope will change, both as a function of cx within the cell, and // each change by a constant for fixed segment lengths. - const float z_sxy0 = z_xmy0 * dx_seg, // per-segment adjustment to z_cxy0 - z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * dx_seg; // per-segment adjustment to z_cxym + const float z_sxy0 = z_xmy0 * seg_dx, // per-segment adjustment to z_cxy0 + z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * seg_dx; // per-segment adjustment to z_cxym for(;;) { // for all segments within this mesh cell @@ -650,28 +695,29 @@ z_cxcy *= fade_scaling_factor; // apply fade factor to interpolated mesh height #endif - z_cxcy += state.z_offset; // add fixed mesh offset from G29 Z + z_cxcy += state.z_offset; // add fixed mesh offset from G29 Z if (--segments == 0) { // if this is last segment, use ltarget for exact - COPY_XYZE(seg_dest, ltarget); - seg_dest[Z_AXIS] += z_cxcy; - ubl_buffer_line_segment(seg_dest, feedrate, active_extruder); - return false; // did not set_current_to_destination() + seg_rx = RAW_X_POSITION(ltarget[X_AXIS]); + seg_ry = RAW_Y_POSITION(ltarget[Y_AXIS]); + seg_rz = RAW_Z_POSITION(ltarget[Z_AXIS]); + seg_le = ltarget[E_AXIS]; } - const float z_orig = seg_dest[Z_AXIS]; // remember the pre-leveled segment z value - seg_dest[Z_AXIS] = z_orig + z_cxcy; // adjust segment z height per mesh leveling - ubl_buffer_line_segment(seg_dest, feedrate, active_extruder); - seg_dest[Z_AXIS] = z_orig; // restore pre-leveled z before incrementing + ubl_buffer_segment_raw( seg_rx, seg_ry, seg_rz + z_cxcy, seg_le, feedrate ); - LOOP_XYZE(i) seg_dest[i] += segment_distance[i]; // adjust seg_dest for next segment + if (segments == 0 ) // done with last segment + return false; // did not set_current_to_destination() - cx += dx_seg; - cy += dy_seg; + seg_rx += seg_dx; + seg_ry += seg_dy; + seg_rz += seg_dz; + seg_le += seg_de; + + cx += seg_dx; + cy += seg_dy; if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) { // done within this cell, break to next - rx = RAW_X_POSITION(seg_dest[X_AXIS]); - ry = RAW_Y_POSITION(seg_dest[Y_AXIS]); break; }