Преглед на файлове

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
oldmcg преди 8 години
родител
ревизия
9f295581f0
променени са 7 файла, в които са добавени 380 реда и са изтрити 156 реда
  1. 2
    3
      Marlin/G26_Mesh_Validation_Tool.cpp
  2. 177
    30
      Marlin/Marlin_main.cpp
  3. 20
    18
      Marlin/planner.cpp
  4. 11
    5
      Marlin/ubl.cpp
  5. 2
    1
      Marlin/ubl.h
  6. 42
    19
      Marlin/ubl_G29.cpp
  7. 126
    80
      Marlin/ubl_motion.cpp

+ 2
- 3
Marlin/G26_Mesh_Validation_Tool.cpp Целия файл

718
     /**
718
     /**
719
      * Wait until all parameters are verified before altering the state!
719
      * Wait until all parameters are verified before altering the state!
720
      */
720
      */
721
-    state.active = !parser.seen('D');
721
+    set_bed_leveling_enabled(!parser.seen('D'));
722
 
722
 
723
     return UBL_OK;
723
     return UBL_OK;
724
   }
724
   }
734
    * wait for them to get up to temperature.
734
    * wait for them to get up to temperature.
735
    */
735
    */
736
   bool unified_bed_leveling::turn_on_heaters() {
736
   bool unified_bed_leveling::turn_on_heaters() {
737
-    millis_t next;
737
+    millis_t next = millis() + 5000UL;
738
     #if HAS_TEMP_BED
738
     #if HAS_TEMP_BED
739
       #if ENABLED(ULTRA_LCD)
739
       #if ENABLED(ULTRA_LCD)
740
         if (g26_bed_temp > 25) {
740
         if (g26_bed_temp > 25) {
743
       #endif
743
       #endif
744
           has_control_of_lcd_panel = true;
744
           has_control_of_lcd_panel = true;
745
           thermalManager.setTargetBed(g26_bed_temp);
745
           thermalManager.setTargetBed(g26_bed_temp);
746
-          next = millis() + 5000UL;
747
           while (abs(thermalManager.degBed() - g26_bed_temp) > 3) {
746
           while (abs(thermalManager.degBed() - g26_bed_temp) > 3) {
748
             if (ubl_lcd_clicked()) return exit_from_g26();
747
             if (ubl_lcd_clicked()) return exit_from_g26();
749
             if (PENDING(millis(), next)) {
748
             if (PENDING(millis(), next)) {

+ 177
- 30
Marlin/Marlin_main.cpp Целия файл

699
 #endif
699
 #endif
700
 
700
 
701
 void tool_change(const uint8_t tmp_extruder, const float fr_mm_s=0.0, bool no_move=false);
701
 void tool_change(const uint8_t tmp_extruder, const float fr_mm_s=0.0, bool no_move=false);
702
-static void report_current_position();
702
+void report_current_position();
703
+void report_current_position_detail();
703
 
704
 
704
 #if ENABLED(DEBUG_LEVELING_FEATURE)
705
 #if ENABLED(DEBUG_LEVELING_FEATURE)
705
   void print_xyz(const char* prefix, const char* suffix, const float x, const float y, const float z) {
706
   void print_xyz(const char* prefix, const char* suffix, const float x, const float y, const float z) {
1536
       if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_uninterpolated_move_to_destination", destination);
1537
       if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_uninterpolated_move_to_destination", destination);
1537
     #endif
1538
     #endif
1538
 
1539
 
1539
-    if ( current_position[X_AXIS] == destination[X_AXIS]
1540
-      && current_position[Y_AXIS] == destination[Y_AXIS]
1541
-      && current_position[Z_AXIS] == destination[Z_AXIS]
1542
-      && current_position[E_AXIS] == destination[E_AXIS]
1543
-    ) return;
1544
-
1545
     refresh_cmd_timeout();
1540
     refresh_cmd_timeout();
1546
-    planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
1541
+
1542
+    #if UBL_DELTA
1543
+      // ubl segmented line will do z-only moves in single segment
1544
+      ubl.prepare_segmented_line_to(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s));
1545
+    #else
1546
+      if ( current_position[X_AXIS] == destination[X_AXIS]
1547
+        && current_position[Y_AXIS] == destination[Y_AXIS]
1548
+        && current_position[Z_AXIS] == destination[Z_AXIS]
1549
+        && current_position[E_AXIS] == destination[E_AXIS]
1550
+      ) return;
1551
+
1552
+      planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
1553
+    #endif
1554
+    
1547
     set_current_to_destination();
1555
     set_current_to_destination();
1548
   }
1556
   }
1549
 #endif // IS_KINEMATIC
1557
 #endif // IS_KINEMATIC
2345
         if (enabling) planner.unapply_leveling(current_position);
2353
         if (enabling) planner.unapply_leveling(current_position);
2346
 
2354
 
2347
       #elif ENABLED(AUTO_BED_LEVELING_UBL)
2355
       #elif ENABLED(AUTO_BED_LEVELING_UBL)
2348
-
2349
         #if PLANNER_LEVELING
2356
         #if PLANNER_LEVELING
2350
-
2351
-          if (!enable)   // leveling from on to off
2357
+          if (ubl.state.active) {                       // leveling from on to off
2358
+            // change unleveled current_position to physical current_position without moving steppers.
2352
             planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
2359
             planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
2353
-          else
2354
-            planner.unapply_leveling(current_position);
2355
-
2360
+            ubl.state.active = false;                   // disable only AFTER calling apply_leveling
2361
+          }
2362
+          else {                                        // leveling from off to on
2363
+            ubl.state.active = true;                    // enable BEFORE calling unapply_leveling, otherwise ignored
2364
+            // change physical current_position to unleveled current_position without moving steppers.
2365
+            planner.unapply_leveling(current_position); 
2366
+          }
2367
+        #else
2368
+          ubl.state.active = enable;                    // just flip the bit, current_position will be wrong until next move.
2356
         #endif
2369
         #endif
2357
 
2370
 
2358
-        ubl.state.active = enable;
2359
-
2360
       #else // ABL
2371
       #else // ABL
2361
 
2372
 
2362
         #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
2373
         #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
2384
   #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
2395
   #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
2385
 
2396
 
2386
     void set_z_fade_height(const float zfh) {
2397
     void set_z_fade_height(const float zfh) {
2387
-      planner.z_fade_height = zfh;
2388
-      planner.inverse_z_fade_height = RECIPROCAL(zfh);
2389
 
2398
 
2390
-      if (leveling_is_active())
2391
-        set_current_from_steppers_for_axis(
2392
-          #if ABL_PLANAR
2393
-            ALL_AXES
2394
-          #else
2395
-            Z_AXIS
2396
-          #endif
2397
-        );
2399
+      #if ENABLED(AUTO_BED_LEVELING_UBL)
2400
+
2401
+        const bool level_active = leveling_is_active();
2402
+        if (level_active) {
2403
+          set_bed_leveling_enabled(false);  // turn off before changing fade height for proper apply/unapply leveling to maintain current_position
2404
+        }
2405
+        planner.z_fade_height = zfh;
2406
+        planner.inverse_z_fade_height = RECIPROCAL(zfh);
2407
+        if (level_active) {
2408
+          set_bed_leveling_enabled(true);  // turn back on after changing fade height
2409
+        }
2410
+
2411
+      #else
2412
+
2413
+        planner.z_fade_height = zfh;
2414
+        planner.inverse_z_fade_height = RECIPROCAL(zfh);
2415
+
2416
+        if (leveling_is_active()) {
2417
+          set_current_from_steppers_for_axis(
2418
+            #if ABL_PLANAR
2419
+              ALL_AXES
2420
+            #else
2421
+              Z_AXIS
2422
+            #endif
2423
+          );
2424
+        }
2425
+      #endif
2398
     }
2426
     }
2399
 
2427
 
2400
   #endif // LEVELING_FADE_HEIGHT
2428
   #endif // LEVELING_FADE_HEIGHT
3656
   #if ENABLED(DELTA)
3684
   #if ENABLED(DELTA)
3657
 
3685
 
3658
     home_delta();
3686
     home_delta();
3687
+    UNUSED(always_home_all);
3659
 
3688
 
3660
   #else // NOT DELTA
3689
   #else // NOT DELTA
3661
 
3690
 
7592
 /**
7621
 /**
7593
  * Output the current position to serial
7622
  * Output the current position to serial
7594
  */
7623
  */
7595
-static void report_current_position() {
7624
+void report_current_position() {
7596
   SERIAL_PROTOCOLPGM("X:");
7625
   SERIAL_PROTOCOLPGM("X:");
7597
   SERIAL_PROTOCOL(current_position[X_AXIS]);
7626
   SERIAL_PROTOCOL(current_position[X_AXIS]);
7598
   SERIAL_PROTOCOLPGM(" Y:");
7627
   SERIAL_PROTOCOLPGM(" Y:");
7611
   #endif
7640
   #endif
7612
 }
7641
 }
7613
 
7642
 
7643
+#ifdef M114_DETAIL
7644
+
7645
+  static const char axis_char[XYZE] = {'X','Y','Z','E'};
7646
+
7647
+  void report_xyze(const float pos[XYZE], uint8_t n = 4, uint8_t precision = 3) {
7648
+    char str[12];
7649
+    for(uint8_t i=0; i<n; i++) {
7650
+      SERIAL_CHAR(' ');
7651
+      SERIAL_CHAR(axis_char[i]);
7652
+      SERIAL_CHAR(':');
7653
+      SERIAL_PROTOCOL(dtostrf(pos[i],8,precision,str));
7654
+    }
7655
+    SERIAL_EOL;
7656
+  }
7657
+
7658
+  inline void report_xyz(const float pos[XYZ]) {
7659
+    report_xyze(pos,3);
7660
+  }
7661
+
7662
+  void report_current_position_detail() {
7663
+
7664
+    stepper.synchronize();
7665
+
7666
+    SERIAL_EOL;
7667
+    SERIAL_PROTOCOLPGM("Logical:");
7668
+    report_xyze(current_position);
7669
+
7670
+    SERIAL_PROTOCOLPGM("Raw:    ");
7671
+    const float raw[XYZ] = {
7672
+      RAW_X_POSITION(current_position[X_AXIS]),
7673
+      RAW_Y_POSITION(current_position[Y_AXIS]),
7674
+      RAW_Z_POSITION(current_position[Z_AXIS])
7675
+    };
7676
+    report_xyz(raw);
7677
+
7678
+    SERIAL_PROTOCOLPGM("Leveled:");
7679
+    float leveled[XYZ] = {
7680
+      current_position[X_AXIS],
7681
+      current_position[Y_AXIS],
7682
+      current_position[Z_AXIS]
7683
+    };
7684
+    planner.apply_leveling(leveled);
7685
+    report_xyz(leveled);
7686
+
7687
+    SERIAL_PROTOCOLPGM("UnLevel:");
7688
+    float unleveled[XYZ] = { leveled[X_AXIS], leveled[Y_AXIS], leveled[Z_AXIS] };
7689
+    planner.unapply_leveling(unleveled);
7690
+    report_xyz(unleveled);
7691
+
7692
+    #if IS_KINEMATIC
7693
+      #if IS_SCARA
7694
+        SERIAL_PROTOCOLPGM("ScaraK: ");
7695
+      #else
7696
+        SERIAL_PROTOCOLPGM("DeltaK: ");
7697
+      #endif
7698
+      inverse_kinematics(leveled);  // writes delta[]
7699
+      report_xyz(delta);
7700
+    #endif
7701
+
7702
+    SERIAL_PROTOCOLPGM("Stepper:");
7703
+    const float step_count[XYZE] = {
7704
+      (float)stepper.position(X_AXIS),
7705
+      (float)stepper.position(Y_AXIS),
7706
+      (float)stepper.position(Z_AXIS),
7707
+      (float)stepper.position(E_AXIS)
7708
+    };
7709
+    report_xyze(step_count,4,0);
7710
+
7711
+    #if IS_SCARA
7712
+      const float deg[XYZ] = {
7713
+        stepper.get_axis_position_degrees(A_AXIS),
7714
+        stepper.get_axis_position_degrees(B_AXIS)
7715
+      };
7716
+      SERIAL_PROTOCOLPGM("Degrees:");
7717
+      report_xyze(deg,2);
7718
+    #endif
7719
+
7720
+    SERIAL_PROTOCOLPGM("FromStp:");
7721
+    get_cartesian_from_steppers();  // writes cartes[XYZ] (with forward kinematics)
7722
+    const float from_steppers[XYZE] = {
7723
+      cartes[X_AXIS],
7724
+      cartes[Y_AXIS],
7725
+      cartes[Z_AXIS],
7726
+      stepper.get_axis_position_mm(E_AXIS)
7727
+    };
7728
+    report_xyze(from_steppers);
7729
+
7730
+    const float diff[XYZE] = {
7731
+      from_steppers[X_AXIS] - leveled[X_AXIS],
7732
+      from_steppers[Y_AXIS] - leveled[Y_AXIS],
7733
+      from_steppers[Z_AXIS] - leveled[Z_AXIS],
7734
+      from_steppers[E_AXIS] - current_position[E_AXIS]
7735
+    };
7736
+    SERIAL_PROTOCOLPGM("Differ: ");
7737
+    report_xyze(diff);
7738
+  }
7739
+#endif // M114_DETAIL
7740
+
7614
 /**
7741
 /**
7615
  * M114: Output current position to serial port
7742
  * M114: Output current position to serial port
7616
  */
7743
  */
7617
-inline void gcode_M114() { stepper.synchronize(); report_current_position(); }
7744
+inline void gcode_M114() {
7745
+
7746
+  #ifdef M114_DETAIL
7747
+    if ( parser.seen('D') ) {
7748
+      report_current_position_detail();
7749
+      return;
7750
+    }
7751
+  #endif
7752
+
7753
+  stepper.synchronize();
7754
+  report_current_position();
7755
+  }
7618
 
7756
 
7619
 /**
7757
 /**
7620
  * M115: Capabilities string
7758
  * M115: Capabilities string
10804
   /**
10942
   /**
10805
    * Constrain the given coordinates to the software endstops.
10943
    * Constrain the given coordinates to the software endstops.
10806
    */
10944
    */
10945
+
10946
+  // NOTE: This makes no sense for delta beds other than Z-axis.
10947
+  //       For delta the X/Y would need to be clamped at
10948
+  //       DELTA_PRINTABLE_RADIUS from center of bed, but delta
10949
+  //       now enforces is_position_reachable for X/Y regardless
10950
+  //       of HAS_SOFTWARE_ENDSTOPS, so that enforcement would be
10951
+  //       redundant here.  Probably should #ifdef out the X/Y
10952
+  //       axis clamps here for delta and just leave the Z clamp.
10953
+
10807
   void clamp_to_software_endstops(float target[XYZ]) {
10954
   void clamp_to_software_endstops(float target[XYZ]) {
10808
     if (!soft_endstops_enabled) return;
10955
     if (!soft_endstops_enabled) return;
10809
     #if ENABLED(MIN_SOFTWARE_ENDSTOPS)
10956
     #if ENABLED(MIN_SOFTWARE_ENDSTOPS)
11597
   if (
11744
   if (
11598
     #if IS_KINEMATIC
11745
     #if IS_KINEMATIC
11599
       #if UBL_DELTA
11746
       #if UBL_DELTA
11600
-        ubl.prepare_linear_move_to(destination, feedrate_mm_s)
11747
+        ubl.prepare_segmented_line_to(destination, feedrate_mm_s)
11601
       #else
11748
       #else
11602
         prepare_kinematic_move_to(destination)
11749
         prepare_kinematic_move_to(destination)
11603
       #endif
11750
       #endif
11604
     #elif ENABLED(DUAL_X_CARRIAGE)
11751
     #elif ENABLED(DUAL_X_CARRIAGE)
11605
       prepare_move_to_destination_dualx()
11752
       prepare_move_to_destination_dualx()
11606
     #elif UBL_DELTA // will work for CARTESIAN too (smaller segments follow mesh more closely)
11753
     #elif UBL_DELTA // will work for CARTESIAN too (smaller segments follow mesh more closely)
11607
-      ubl.prepare_linear_move_to(destination, feedrate_mm_s)
11754
+      ubl.prepare_segmented_line_to(destination, feedrate_mm_s)
11608
     #else
11755
     #else
11609
       prepare_move_to_destination_cartesian()
11756
       prepare_move_to_destination_cartesian()
11610
     #endif
11757
     #endif

+ 20
- 18
Marlin/planner.cpp Целия файл

535
    */
535
    */
536
   void Planner::apply_leveling(float &lx, float &ly, float &lz) {
536
   void Planner::apply_leveling(float &lx, float &ly, float &lz) {
537
 
537
 
538
-    #if ENABLED(AUTO_BED_LEVELING_UBL) && UBL_DELTA  // probably should also be enabled for UBL without UBL_DELTA
538
+    #if ENABLED(AUTO_BED_LEVELING_UBL)
539
       if (!ubl.state.active) return;
539
       if (!ubl.state.active) return;
540
       #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
540
       #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
541
         // if z_fade_height enabled (nonzero) and raw_z above it, no leveling required
541
         // if z_fade_height enabled (nonzero) and raw_z above it, no leveling required
550
       if (!abl_enabled) return;
550
       if (!abl_enabled) return;
551
     #endif
551
     #endif
552
 
552
 
553
-    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
553
+    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(AUTO_BED_LEVELING_UBL)
554
       static float z_fade_factor = 1.0, last_raw_lz = -999.0;
554
       static float z_fade_factor = 1.0, last_raw_lz = -999.0;
555
       if (z_fade_height) {
555
       if (z_fade_height) {
556
         const float raw_lz = RAW_Z_POSITION(lz);
556
         const float raw_lz = RAW_Z_POSITION(lz);
599
 
599
 
600
   void Planner::unapply_leveling(float logical[XYZ]) {
600
   void Planner::unapply_leveling(float logical[XYZ]) {
601
 
601
 
602
-    #if ENABLED(AUTO_BED_LEVELING_UBL) && UBL_DELTA
602
+    #if ENABLED(AUTO_BED_LEVELING_UBL)
603
 
603
 
604
       if (ubl.state.active) {
604
       if (ubl.state.active) {
605
 
605
 
606
-        const float z_leveled = RAW_Z_POSITION(logical[Z_AXIS]),
607
-                    z_ublmesh = ubl.get_z_correction(logical[X_AXIS], logical[Y_AXIS]);
608
-              float z_unlevel = z_leveled - ubl.state.z_offset - z_ublmesh;
606
+        const float z_physical = RAW_Z_POSITION(logical[Z_AXIS]);
607
+        const float z_ublmesh  = ubl.get_z_correction(logical[X_AXIS], logical[Y_AXIS]);
608
+        const float z_virtual  = z_physical - ubl.state.z_offset - z_ublmesh;
609
+              float z_logical  = LOGICAL_Z_POSITION(z_virtual);
609
 
610
 
610
         #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
611
         #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
611
 
612
 
612
-          // for L=leveled, U=unleveled, M=mesh, O=offset, H=fade_height,
613
-          // Given L==U+O+M(1-U/H) (faded mesh correction formula for U<H)
614
-          //  then U==L-O-M(1-U/H)
615
-          //    so U==L-O-M+MU/H
616
-          //    so U-MU/H==L-O-M
617
-          //    so U(1-M/H)==L-O-M
618
-          //    so U==(L-O-M)/(1-M/H) for U<H
613
+          // for P=physical_z, L=logical_z, M=mesh_z, O=z_offset, H=fade_height,
614
+          // Given P=L+O+M(1-L/H) (faded mesh correction formula for L<H)
615
+          //  then L=P-O-M(1-L/H)
616
+          //    so L=P-O-M+ML/H
617
+          //    so L-ML/H=P-O-M
618
+          //    so L(1-M/H)=P-O-M
619
+          //    so L=(P-O-M)/(1-M/H) for L<H
619
 
620
 
620
           if (planner.z_fade_height) {
621
           if (planner.z_fade_height) {
621
-            const float z_unfaded = z_unlevel / (1.0 - z_ublmesh * planner.inverse_z_fade_height);
622
-            if (z_unfaded < planner.z_fade_height)  // don't know until after compute
623
-              z_unlevel = z_unfaded;
622
+            if (z_logical < planner.z_fade_height )
623
+              z_logical = z_logical / (1.0 - (z_ublmesh * planner.inverse_z_fade_height));
624
+            if (z_logical >= planner.z_fade_height)
625
+              z_logical = LOGICAL_Z_POSITION(z_physical - ubl.state.z_offset);
624
           }
626
           }
625
 
627
 
626
         #endif // ENABLE_LEVELING_FADE_HEIGHT
628
         #endif // ENABLE_LEVELING_FADE_HEIGHT
627
 
629
 
628
-        logical[Z_AXIS] = z_unlevel;
630
+        logical[Z_AXIS] = z_logical;
629
       }
631
       }
630
 
632
 
631
-      return; // don't fall thru to HAS_ABL or other ENABLE_LEVELING_FADE_HEIGHT logic
633
+      return; // don't fall thru to other ENABLE_LEVELING_FADE_HEIGHT logic
632
 
634
 
633
     #endif
635
     #endif
634
 
636
 

+ 11
- 5
Marlin/ubl.cpp Целия файл

83
   }
83
   }
84
 
84
 
85
   void unified_bed_leveling::reset() {
85
   void unified_bed_leveling::reset() {
86
-    state.active = false;
86
+    set_bed_leveling_enabled(false);
87
     state.z_offset = 0;
87
     state.z_offset = 0;
88
     state.storage_slot = -1;
88
     state.storage_slot = -1;
89
     #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
89
     #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
94
   }
94
   }
95
 
95
 
96
   void unified_bed_leveling::invalidate() {
96
   void unified_bed_leveling::invalidate() {
97
-    state.active = false;
97
+    set_bed_leveling_enabled(false);
98
     state.z_offset = 0;
98
     state.z_offset = 0;
99
-    for (int x = 0; x < GRID_MAX_POINTS_X; x++)
100
-      for (int y = 0; y < GRID_MAX_POINTS_Y; y++)
101
-        z_values[x][y] = NAN;
99
+    set_all_mesh_points_to_value(NAN);
100
+  }
101
+
102
+  void unified_bed_leveling::set_all_mesh_points_to_value(float value) {
103
+    for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) {
104
+      for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
105
+        z_values[x][y] = value;
106
+      }
107
+    }
102
   }
108
   }
103
 
109
 
104
   void unified_bed_leveling::display_map(const int map_type) {
110
   void unified_bed_leveling::display_map(const int map_type) {

+ 2
- 1
Marlin/ubl.h Целия файл

154
       static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, unsigned int[16], bool);
154
       static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, unsigned int[16], bool);
155
       static void reset();
155
       static void reset();
156
       static void invalidate();
156
       static void invalidate();
157
+      static void set_all_mesh_points_to_value(float);
157
       static bool sanity_check();
158
       static bool sanity_check();
158
 
159
 
159
       static void G29() _O0;                          // O0 for no optimization
160
       static void G29() _O0;                          // O0 for no optimization
385
       FORCE_INLINE static float mesh_index_to_xpos(const uint8_t i) { return pgm_read_float(&_mesh_index_to_xpos[i]); }
386
       FORCE_INLINE static float mesh_index_to_xpos(const uint8_t i) { return pgm_read_float(&_mesh_index_to_xpos[i]); }
386
       FORCE_INLINE static float mesh_index_to_ypos(const uint8_t i) { return pgm_read_float(&_mesh_index_to_ypos[i]); }
387
       FORCE_INLINE static float mesh_index_to_ypos(const uint8_t i) { return pgm_read_float(&_mesh_index_to_ypos[i]); }
387
 
388
 
388
-      static bool prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate);
389
+      static bool prepare_segmented_line_to(const float ltarget[XYZE], const float &feedrate);
389
       static void line_to_destination_cartesian(const float &fr, uint8_t e);
390
       static void line_to_destination_cartesian(const float &fr, uint8_t e);
390
 
391
 
391
   }; // class unified_bed_leveling
392
   }; // class unified_bed_leveling

+ 42
- 19
Marlin/ubl_G29.cpp Целия файл

30
   #include "configuration_store.h"
30
   #include "configuration_store.h"
31
   #include "ultralcd.h"
31
   #include "ultralcd.h"
32
   #include "stepper.h"
32
   #include "stepper.h"
33
+  #include "planner.h"
33
   #include "gcode.h"
34
   #include "gcode.h"
34
 
35
 
35
   #include <math.h>
36
   #include <math.h>
48
   extern long babysteps_done;
49
   extern long babysteps_done;
49
   extern float probe_pt(const float &x, const float &y, bool, int);
50
   extern float probe_pt(const float &x, const float &y, bool, int);
50
   extern bool set_probe_deployed(bool);
51
   extern bool set_probe_deployed(bool);
52
+  extern void set_bed_leveling_enabled(bool);
51
 
53
 
52
   #define SIZE_OF_LITTLE_RAISE 1
54
   #define SIZE_OF_LITTLE_RAISE 1
53
   #define BIG_RAISE_NOT_NEEDED 0
55
   #define BIG_RAISE_NOT_NEEDED 0
325
     if (parser.seen('I')) {
327
     if (parser.seen('I')) {
326
       uint8_t cnt = 0;
328
       uint8_t cnt = 0;
327
       g29_repetition_cnt = parser.has_value() ? parser.value_int() : 1;
329
       g29_repetition_cnt = parser.has_value() ? parser.value_int() : 1;
328
-      while (g29_repetition_cnt--) {
329
-        if (cnt > 20) { cnt = 0; idle(); }
330
-        const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
331
-        if (location.x_index < 0) {
332
-          SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n");
333
-          break;            // No more invalid Mesh Points to populate
330
+      if (g29_repetition_cnt >= GRID_MAX_POINTS) {
331
+        set_all_mesh_points_to_value(NAN);
332
+      } else {
333
+        while (g29_repetition_cnt--) {
334
+          if (cnt > 20) { cnt = 0; idle(); }
335
+          const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
336
+          if (location.x_index < 0) {
337
+            // No more REACHABLE mesh points to invalidate, so we ASSUME the user
338
+            // meant to invalidate the ENTIRE mesh, which cannot be done with
339
+            // find_closest_mesh_point loop which only returns REACHABLE points.
340
+            set_all_mesh_points_to_value(NAN);
341
+            SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n");
342
+            break;            // No more invalid Mesh Points to populate
343
+          }
344
+          z_values[location.x_index][location.y_index] = NAN;
345
+          cnt++;
334
         }
346
         }
335
-        z_values[location.x_index][location.y_index] = NAN;
336
-        cnt++;
337
       }
347
       }
338
       SERIAL_PROTOCOLLNPGM("Locations invalidated.\n");
348
       SERIAL_PROTOCOLLNPGM("Locations invalidated.\n");
339
     }
349
     }
497
            *   - Specify a constant with the 'C' parameter.
507
            *   - Specify a constant with the 'C' parameter.
498
            *   - Allow 'G29 P3' to choose a 'reasonable' constant.
508
            *   - Allow 'G29 P3' to choose a 'reasonable' constant.
499
            */
509
            */
510
+
500
           if (g29_c_flag) {
511
           if (g29_c_flag) {
501
             if (g29_repetition_cnt >= GRID_MAX_POINTS) {
512
             if (g29_repetition_cnt >= GRID_MAX_POINTS) {
502
-              for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) {
503
-                for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
504
-                  z_values[x][y] = g29_constant;
505
-                }
506
-              }
513
+              set_all_mesh_points_to_value(g29_constant);
507
             }
514
             }
508
             else {
515
             else {
509
               while (g29_repetition_cnt--) {  // this only populates reachable mesh points near
516
               while (g29_repetition_cnt--) {  // this only populates reachable mesh points near
510
                 const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
517
                 const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
511
-                if (location.x_index < 0) break; // No more reachable invalid Mesh Points to populate
518
+                if (location.x_index < 0) {
519
+                  // No more REACHABLE INVALID mesh points to populate, so we ASSUME
520
+                  // user meant to populate ALL INVALID mesh points to value
521
+                  for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) {
522
+                    for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
523
+                      if ( isnan(z_values[x][y])) {
524
+                        z_values[x][y] = g29_constant;
525
+                      }
526
+                    }
527
+                  }
528
+                  break; // No more invalid Mesh Points to populate
529
+                }
512
                 z_values[location.x_index][location.y_index] = g29_constant;
530
                 z_values[location.x_index][location.y_index] = g29_constant;
513
               }
531
               }
514
             }
532
             }
999
 
1017
 
1000
       serialprintPGM(parser.seen('B') ? PSTR("Place shim & measure") : PSTR("Measure")); // TODO: Make translatable strings
1018
       serialprintPGM(parser.seen('B') ? PSTR("Place shim & measure") : PSTR("Measure")); // TODO: Make translatable strings
1001
 
1019
 
1020
+      const float z_step = 0.01;                                        // existing behavior: 0.01mm per click, occasionally step
1021
+      //const float z_step = 1.0 / planner.axis_steps_per_mm[Z_AXIS];   // approx one step each click
1022
+
1002
       while (ubl_lcd_clicked()) delay(50);             // wait for user to release encoder wheel
1023
       while (ubl_lcd_clicked()) delay(50);             // wait for user to release encoder wheel
1003
       delay(50);                                       // debounce
1024
       delay(50);                                       // debounce
1004
       while (!ubl_lcd_clicked()) {                     // we need the loop to move the nozzle based on the encoder wheel here!
1025
       while (!ubl_lcd_clicked()) {                     // we need the loop to move the nozzle based on the encoder wheel here!
1005
         idle();
1026
         idle();
1006
         if (encoder_diff) {
1027
         if (encoder_diff) {
1007
-          do_blocking_move_to_z(current_position[Z_AXIS] + float(encoder_diff) / 100.0);
1028
+          do_blocking_move_to_z(current_position[Z_AXIS] + float(encoder_diff) * z_step);
1008
           encoder_diff = 0;
1029
           encoder_diff = 0;
1009
         }
1030
         }
1010
       }
1031
       }
1115
         SERIAL_PROTOCOLLNPGM("?Can't activate and deactivate at the same time.\n");
1136
         SERIAL_PROTOCOLLNPGM("?Can't activate and deactivate at the same time.\n");
1116
         return UBL_ERR;
1137
         return UBL_ERR;
1117
       }
1138
       }
1118
-      state.active = true;
1139
+      set_bed_leveling_enabled(true);
1119
       report_state();
1140
       report_state();
1120
     }
1141
     }
1121
     else if (parser.seen('D')) {
1142
     else if (parser.seen('D')) {
1122
-      state.active = false;
1143
+      set_bed_leveling_enabled(false);
1123
       report_state();
1144
       report_state();
1124
     }
1145
     }
1125
 
1146
 
1158
       return;
1179
       return;
1159
     }
1180
     }
1160
     ubl_state_at_invocation = state.active;
1181
     ubl_state_at_invocation = state.active;
1161
-    state.active = 0;
1182
+    set_bed_leveling_enabled(false);
1162
   }
1183
   }
1163
 
1184
 
1164
   void unified_bed_leveling::restore_ubl_active_state_and_leave() {
1185
   void unified_bed_leveling::restore_ubl_active_state_and_leave() {
1168
       lcd_quick_feedback();
1189
       lcd_quick_feedback();
1169
       return;
1190
       return;
1170
     }
1191
     }
1171
-    state.active = ubl_state_at_invocation;
1192
+    set_bed_leveling_enabled(ubl_state_at_invocation);
1172
   }
1193
   }
1173
 
1194
 
1174
   /**
1195
   /**
1695
         SERIAL_EOL;
1716
         SERIAL_EOL;
1696
       }
1717
       }
1697
     #endif
1718
     #endif
1719
+
1720
+    if (do_ubl_mesh_map) display_map(g29_map_type);
1698
   }
1721
   }
1699
 
1722
 
1700
   #if ENABLED(UBL_G29_P31)
1723
   #if ENABLED(UBL_G29_P31)

+ 126
- 80
Marlin/ubl_motion.cpp Целия файл

32
 
32
 
33
   extern float destination[XYZE];
33
   extern float destination[XYZE];
34
   extern void set_current_to_destination();
34
   extern void set_current_to_destination();
35
-  extern float delta_segments_per_second;
35
+
36
+#if ENABLED(DELTA)
37
+
38
+  extern float delta[ABC],
39
+               endstop_adj[ABC];
40
+
41
+  extern float delta_radius,
42
+               delta_tower_angle_trim[2],
43
+               delta_tower[ABC][2],
44
+               delta_diagonal_rod,
45
+               delta_calibration_radius,
46
+               delta_diagonal_rod_2_tower[ABC],
47
+               delta_segments_per_second,
48
+               delta_clip_start_height;
49
+
50
+  extern float delta_safe_distance_from_top();
51
+
52
+#endif
53
+
36
 
54
 
37
   static void debug_echo_axis(const AxisEnum axis) {
55
   static void debug_echo_axis(const AxisEnum axis) {
38
     if (current_position[axis] == destination[axis])
56
     if (current_position[axis] == destination[axis])
470
     #endif
488
     #endif
471
 
489
 
472
     // We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic,
490
     // We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic,
473
-    // so we call _buffer_line directly here.  Per-segmented leveling performed first.
491
+    // so we call _buffer_line directly here.  Per-segmented leveling and kinematics performed first.
474
 
492
 
475
-    static inline void ubl_buffer_line_segment(const float ltarget[XYZE], const float &fr_mm_s, const uint8_t extruder) {
493
+    inline void _O2 ubl_buffer_segment_raw( float rx, float ry, float rz, float le, float fr ) {
476
 
494
 
477
-      #if IS_KINEMATIC
495
+      #if ENABLED(DELTA)  // apply delta inverse_kinematics
478
 
496
 
479
-        inverse_kinematics(ltarget); // this writes delta[ABC] from ltarget[XYZ] but does not modify ltarget
480
-        float feedrate = fr_mm_s;
497
+        const float delta_A = rz + sqrt( delta_diagonal_rod_2_tower[A_AXIS]
498
+                                         - HYPOT2( delta_tower[A_AXIS][X_AXIS] - rx,
499
+                                                   delta_tower[A_AXIS][Y_AXIS] - ry ));
481
 
500
 
482
-        #if IS_SCARA // scale the feed rate from mm/s to degrees/s
483
-          float adiff = abs(delta[A_AXIS] - scara_oldA),
484
-                bdiff = abs(delta[B_AXIS] - scara_oldB);
485
-          scara_oldA = delta[A_AXIS];
486
-          scara_oldB = delta[B_AXIS];
487
-          feedrate = max(adiff, bdiff) * scara_feed_factor;
488
-        #endif
501
+        const float delta_B = rz + sqrt( delta_diagonal_rod_2_tower[B_AXIS]
502
+                                         - HYPOT2( delta_tower[B_AXIS][X_AXIS] - rx,
503
+                                                   delta_tower[B_AXIS][Y_AXIS] - ry ));
504
+
505
+        const float delta_C = rz + sqrt( delta_diagonal_rod_2_tower[C_AXIS]
506
+                                         - HYPOT2( delta_tower[C_AXIS][X_AXIS] - rx,
507
+                                                   delta_tower[C_AXIS][Y_AXIS] - ry ));
508
+
509
+        planner._buffer_line(delta_A, delta_B, delta_C, le, fr, active_extruder);
510
+
511
+      #elif IS_SCARA  // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
512
+
513
+        const float lseg[XYZ] = { LOGICAL_X_POSITION(rx),
514
+                                  LOGICAL_Y_POSITION(ry),
515
+                                  LOGICAL_Z_POSITION(rz)
516
+                                };
517
+
518
+        inverse_kinematics(lseg); // this writes delta[ABC] from lseg[XYZ]
519
+                                  // should move the feedrate scaling to scara inverse_kinematics
489
 
520
 
490
-        planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], feedrate, extruder);
521
+        float adiff = abs(delta[A_AXIS] - scara_oldA),
522
+              bdiff = abs(delta[B_AXIS] - scara_oldB);
523
+        scara_oldA = delta[A_AXIS];
524
+        scara_oldB = delta[B_AXIS];
525
+        float s_feedrate = max(adiff, bdiff) * scara_feed_factor;
491
 
526
 
492
-      #else // cartesian
527
+        planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], le, s_feedrate, active_extruder);
493
 
528
 
494
-        planner._buffer_line(ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS], ltarget[E_AXIS], fr_mm_s, extruder);
529
+      #else // CARTESIAN
530
+
531
+        // Cartesian _buffer_line seems to take LOGICAL, not RAW coordinates
532
+
533
+        const float lx = LOGICAL_X_POSITION(rx),
534
+                    ly = LOGICAL_Y_POSITION(ry),
535
+                    lz = LOGICAL_Z_POSITION(rz);
536
+
537
+        planner._buffer_line(lx, ly, lz, le, fr, active_extruder);
495
 
538
 
496
       #endif
539
       #endif
540
+
497
     }
541
     }
498
 
542
 
543
+
499
     /**
544
     /**
500
-     * Prepare a linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics.
545
+     * Prepare a segmented linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics.
501
      * This calls planner._buffer_line multiple times for small incremental moves.
546
      * This calls planner._buffer_line multiple times for small incremental moves.
502
-     * Returns true if the caller did NOT update current_position, otherwise false.
547
+     * Returns true if did NOT move, false if moved (requires current_position update).
503
      */
548
      */
504
 
549
 
505
-    static bool unified_bed_leveling::prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate) {
550
+    bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float ltarget[XYZE], const float &feedrate) {
506
 
551
 
507
       if (!position_is_reachable_xy(ltarget[X_AXIS], ltarget[Y_AXIS]))  // fail if moving outside reachable boundary
552
       if (!position_is_reachable_xy(ltarget[X_AXIS], ltarget[Y_AXIS]))  // fail if moving outside reachable boundary
508
         return true; // did not move, so current_position still accurate
553
         return true; // did not move, so current_position still accurate
509
 
554
 
510
-      const float difference[XYZE] = {    // cartesian distances moved in XYZE
511
-                    ltarget[X_AXIS] - current_position[X_AXIS],
512
-                    ltarget[Y_AXIS] - current_position[Y_AXIS],
513
-                    ltarget[Z_AXIS] - current_position[Z_AXIS],
514
-                    ltarget[E_AXIS] - current_position[E_AXIS]
515
-                  };
555
+      const float tot_dx = ltarget[X_AXIS] - current_position[X_AXIS],
556
+                  tot_dy = ltarget[Y_AXIS] - current_position[Y_AXIS],
557
+                  tot_dz = ltarget[Z_AXIS] - current_position[Z_AXIS],
558
+                  tot_de = ltarget[E_AXIS] - current_position[E_AXIS];
516
 
559
 
517
-      const float cartesian_xy_mm = HYPOT(difference[X_AXIS], difference[Y_AXIS]);         // total horizontal xy distance
560
+      const float cartesian_xy_mm = HYPOT(tot_dx, tot_dy);  // total horizontal xy distance
518
 
561
 
519
       #if IS_KINEMATIC
562
       #if IS_KINEMATIC
520
         const float seconds = cartesian_xy_mm / feedrate;                                  // seconds to move xy distance at requested rate
563
         const float seconds = cartesian_xy_mm / feedrate;                                  // seconds to move xy distance at requested rate
534
         scara_oldB = stepper.get_axis_position_degrees(B_AXIS);
577
         scara_oldB = stepper.get_axis_position_degrees(B_AXIS);
535
       #endif
578
       #endif
536
 
579
 
537
-      const float segment_distance[XYZE] = {            // length for each segment
538
-                    difference[X_AXIS] * inv_segments,
539
-                    difference[Y_AXIS] * inv_segments,
540
-                    difference[Z_AXIS] * inv_segments,
541
-                    difference[E_AXIS] * inv_segments
542
-                  };
580
+      const float seg_dx = tot_dx * inv_segments,
581
+                  seg_dy = tot_dy * inv_segments,
582
+                  seg_dz = tot_dz * inv_segments,
583
+                  seg_de = tot_de * inv_segments;
543
 
584
 
544
       // Note that E segment distance could vary slightly as z mesh height
585
       // Note that E segment distance could vary slightly as z mesh height
545
       // changes for each segment, but small enough to ignore.
586
       // changes for each segment, but small enough to ignore.
546
 
587
 
588
+      float seg_rx = RAW_X_POSITION(current_position[X_AXIS]),
589
+            seg_ry = RAW_Y_POSITION(current_position[Y_AXIS]),
590
+            seg_rz = RAW_Z_POSITION(current_position[Z_AXIS]),
591
+            seg_le = current_position[E_AXIS];
592
+
547
       const bool above_fade_height = (
593
       const bool above_fade_height = (
548
         #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
594
         #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
549
           planner.z_fade_height != 0 && planner.z_fade_height < RAW_Z_POSITION(ltarget[Z_AXIS])
595
           planner.z_fade_height != 0 && planner.z_fade_height < RAW_Z_POSITION(ltarget[Z_AXIS])
558
 
604
 
559
         const float z_offset = state.active ? state.z_offset : 0.0;
605
         const float z_offset = state.active ? state.z_offset : 0.0;
560
 
606
 
561
-        float seg_dest[XYZE];                   // per-segment destination,
562
-        COPY_XYZE(seg_dest, current_position);  // starting from current position
607
+        do {
608
+
609
+          if (--segments) {     // not the last segment
610
+            seg_rx += seg_dx;
611
+            seg_ry += seg_dy;
612
+            seg_rz += seg_dz;
613
+            seg_le += seg_de;
614
+          } else {              // last segment, use exact destination
615
+            seg_rx = RAW_X_POSITION(ltarget[X_AXIS]);
616
+            seg_ry = RAW_Y_POSITION(ltarget[Y_AXIS]);
617
+            seg_rz = RAW_Z_POSITION(ltarget[Z_AXIS]);
618
+            seg_le = ltarget[E_AXIS];
619
+          }
563
 
620
 
564
-        while (--segments) {
565
-          LOOP_XYZE(i) seg_dest[i] += segment_distance[i];
566
-          float ztemp = seg_dest[Z_AXIS];
567
-          seg_dest[Z_AXIS] += z_offset;
568
-          ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
569
-          seg_dest[Z_AXIS] = ztemp;
570
-        }
621
+          ubl_buffer_segment_raw( seg_rx, seg_ry, seg_rz + z_offset, seg_le, feedrate );
622
+
623
+        } while (segments);
571
 
624
 
572
-        // Since repeated adding segment_distance accumulates small errors, final move to exact destination.
573
-        COPY_XYZE(seg_dest, ltarget);
574
-        seg_dest[Z_AXIS] += z_offset;
575
-        ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
576
         return false; // moved but did not set_current_to_destination();
625
         return false; // moved but did not set_current_to_destination();
577
       }
626
       }
578
 
627
 
582
         const float fade_scaling_factor = fade_scaling_factor_for_z(ltarget[Z_AXIS]);
631
         const float fade_scaling_factor = fade_scaling_factor_for_z(ltarget[Z_AXIS]);
583
       #endif
632
       #endif
584
 
633
 
585
-      float seg_dest[XYZE];  // per-segment destination, initialize to first segment
586
-      LOOP_XYZE(i) seg_dest[i] = current_position[i] + segment_distance[i];
587
-
588
-      const float &dx_seg = segment_distance[X_AXIS];  // alias for clarity
589
-      const float &dy_seg = segment_distance[Y_AXIS];
590
-
591
-      float rx = RAW_X_POSITION(seg_dest[X_AXIS]),  // assume raw vs logical coordinates shifted but not scaled.
592
-            ry = RAW_Y_POSITION(seg_dest[Y_AXIS]);
634
+      // increment to first segment destination
635
+      seg_rx += seg_dx;
636
+      seg_ry += seg_dy;
637
+      seg_rz += seg_dz;
638
+      seg_le += seg_de;
593
 
639
 
594
       for(;;) {  // for each mesh cell encountered during the move
640
       for(;;) {  // for each mesh cell encountered during the move
595
 
641
 
600
         // in top of loop and again re-find same adjacent cell and use it, just less efficient
646
         // in top of loop and again re-find same adjacent cell and use it, just less efficient
601
         // for mesh inset area.
647
         // for mesh inset area.
602
 
648
 
603
-        int8_t cell_xi = (rx - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
604
-               cell_yi = (ry - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
649
+        int8_t cell_xi = (seg_rx - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
650
+               cell_yi = (seg_ry - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
605
 
651
 
606
         cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
652
         cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
607
         cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);
653
         cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);
608
 
654
 
609
-        const float x0 = mesh_index_to_xpos(cell_xi),     // 64 byte table lookup avoids mul+add
610
-                    y0 = mesh_index_to_ypos(cell_yi),     // 64 byte table lookup avoids mul+add
611
-                    x1 = mesh_index_to_xpos(cell_xi + 1), // 64 byte table lookup avoids mul+add
612
-                    y1 = mesh_index_to_ypos(cell_yi + 1); // 64 byte table lookup avoids mul+add
655
+        const float x0 = mesh_index_to_xpos(cell_xi),   // 64 byte table lookup avoids mul+add
656
+                    y0 = mesh_index_to_ypos(cell_yi);
613
 
657
 
614
-        float cx = rx - x0,   // cell-relative x
615
-              cy = ry - y0,   // cell-relative y
616
-              z_x0y0 = z_values[cell_xi  ][cell_yi  ],  // z at lower left corner
658
+        float z_x0y0 = z_values[cell_xi  ][cell_yi  ],  // z at lower left corner
617
               z_x1y0 = z_values[cell_xi+1][cell_yi  ],  // z at upper left corner
659
               z_x1y0 = z_values[cell_xi+1][cell_yi  ],  // z at upper left corner
618
               z_x0y1 = z_values[cell_xi  ][cell_yi+1],  // z at lower right corner
660
               z_x0y1 = z_values[cell_xi  ][cell_yi+1],  // z at lower right corner
619
               z_x1y1 = z_values[cell_xi+1][cell_yi+1];  // z at upper right corner
661
               z_x1y1 = z_values[cell_xi+1][cell_yi+1];  // z at upper right corner
623
         if (isnan(z_x0y1)) z_x0y1 = 0;              //   in order to avoid isnan tests per cell,
665
         if (isnan(z_x0y1)) z_x0y1 = 0;              //   in order to avoid isnan tests per cell,
624
         if (isnan(z_x1y1)) z_x1y1 = 0;              //   thus guessing zero for undefined points
666
         if (isnan(z_x1y1)) z_x1y1 = 0;              //   thus guessing zero for undefined points
625
 
667
 
668
+        float cx = seg_rx - x0,   // cell-relative x and y
669
+              cy = seg_ry - y0;
670
+
626
         const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0 / (MESH_X_DIST)),   // z slope per x along y0 (lower left to lower right)
671
         const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0 / (MESH_X_DIST)),   // z slope per x along y0 (lower left to lower right)
627
                     z_xmy1 = (z_x1y1 - z_x0y1) * (1.0 / (MESH_X_DIST));   // z slope per x along y1 (upper left to upper right)
672
                     z_xmy1 = (z_x1y1 - z_x0y1) * (1.0 / (MESH_X_DIST));   // z slope per x along y1 (upper left to upper right)
628
 
673
 
629
-              float z_cxy0 = z_x0y0 + z_xmy0 * cx;            // z height along y0 at cx
674
+              float z_cxy0 = z_x0y0 + z_xmy0 * cx;            // z height along y0 at cx (changes for each cx in cell)
630
 
675
 
631
         const float z_cxy1 = z_x0y1 + z_xmy1 * cx,            // z height along y1 at cx
676
         const float z_cxy1 = z_x0y1 + z_xmy1 * cx,            // z height along y1 at cx
632
                     z_cxyd = z_cxy1 - z_cxy0;                 // z height difference along cx from y0 to y1
677
                     z_cxyd = z_cxy1 - z_cxy0;                 // z height difference along cx from y0 to y1
633
 
678
 
634
-              float z_cxym = z_cxyd * (1.0 / (MESH_Y_DIST));  // z slope per y along cx from y0 to y1
679
+              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)
635
 
680
 
636
         //    float z_cxcy = z_cxy0 + z_cxym * cy;            // interpolated mesh z height along cx at cy (do inside the segment loop)
681
         //    float z_cxcy = z_cxy0 + z_cxym * cy;            // interpolated mesh z height along cx at cy (do inside the segment loop)
637
 
682
 
639
         // and the z_cxym slope will change, both as a function of cx within the cell, and
684
         // and the z_cxym slope will change, both as a function of cx within the cell, and
640
         // each change by a constant for fixed segment lengths.
685
         // each change by a constant for fixed segment lengths.
641
 
686
 
642
-        const float z_sxy0 = z_xmy0 * dx_seg,                                     // per-segment adjustment to z_cxy0
643
-                    z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * dx_seg;  // per-segment adjustment to z_cxym
687
+        const float z_sxy0 = z_xmy0 * seg_dx,                                     // per-segment adjustment to z_cxy0
688
+                    z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * seg_dx;  // per-segment adjustment to z_cxym
644
 
689
 
645
         for(;;) {  // for all segments within this mesh cell
690
         for(;;) {  // for all segments within this mesh cell
646
 
691
 
650
             z_cxcy *= fade_scaling_factor;          // apply fade factor to interpolated mesh height
695
             z_cxcy *= fade_scaling_factor;          // apply fade factor to interpolated mesh height
651
           #endif
696
           #endif
652
 
697
 
653
-          z_cxcy += state.z_offset;             // add fixed mesh offset from G29 Z
698
+          z_cxcy += state.z_offset;                 // add fixed mesh offset from G29 Z
654
 
699
 
655
           if (--segments == 0) {                    // if this is last segment, use ltarget for exact
700
           if (--segments == 0) {                    // if this is last segment, use ltarget for exact
656
-            COPY_XYZE(seg_dest, ltarget);
657
-            seg_dest[Z_AXIS] += z_cxcy;
658
-            ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
659
-            return false;   // did not set_current_to_destination()
701
+            seg_rx = RAW_X_POSITION(ltarget[X_AXIS]);
702
+            seg_ry = RAW_Y_POSITION(ltarget[Y_AXIS]);
703
+            seg_rz = RAW_Z_POSITION(ltarget[Z_AXIS]);
704
+            seg_le = ltarget[E_AXIS];
660
           }
705
           }
661
 
706
 
662
-          const float z_orig = seg_dest[Z_AXIS];  // remember the pre-leveled segment z value
663
-          seg_dest[Z_AXIS] = z_orig + z_cxcy;     // adjust segment z height per mesh leveling
664
-          ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
665
-          seg_dest[Z_AXIS] = z_orig;              // restore pre-leveled z before incrementing
707
+          ubl_buffer_segment_raw( seg_rx, seg_ry, seg_rz + z_cxcy, seg_le, feedrate );
708
+
709
+          if (segments == 0 )                       // done with last segment
710
+            return false;                           // did not set_current_to_destination()
666
 
711
 
667
-          LOOP_XYZE(i) seg_dest[i] += segment_distance[i];  // adjust seg_dest for next segment
712
+          seg_rx += seg_dx;
713
+          seg_ry += seg_dy;
714
+          seg_rz += seg_dz;
715
+          seg_le += seg_de;
668
 
716
 
669
-          cx += dx_seg;
670
-          cy += dy_seg;
717
+          cx += seg_dx;
718
+          cy += seg_dy;
671
 
719
 
672
           if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) {  // done within this cell, break to next
720
           if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) {  // done within this cell, break to next
673
-            rx = RAW_X_POSITION(seg_dest[X_AXIS]);
674
-            ry = RAW_Y_POSITION(seg_dest[Y_AXIS]);
675
             break;
721
             break;
676
           }
722
           }
677
 
723
 

Loading…
Отказ
Запис