Browse Source

New feature: BED_SKEW_CORRECTION

Scott Lahteine 7 years ago
parent
commit
172ce67344

+ 1
- 1
.travis.yml View File

@@ -89,7 +89,7 @@ script:
89 89
   - opt_enable PIDTEMPBED FIX_MOUNTED_PROBE Z_SAFE_HOMING ARC_P_CIRCLES CNC_WORKSPACE_PLANES CNC_COORDINATE_SYSTEMS
90 90
   - opt_enable REPRAP_DISCOUNT_SMART_CONTROLLER SDSUPPORT EEPROM_SETTINGS
91 91
   - opt_enable BLINKM PCA9632 RGB_LED NEOPIXEL_LED
92
-  - opt_enable AUTO_BED_LEVELING_LINEAR Z_MIN_PROBE_REPEATABILITY_TEST DEBUG_LEVELING_FEATURE
92
+  - opt_enable AUTO_BED_LEVELING_LINEAR Z_MIN_PROBE_REPEATABILITY_TEST DEBUG_LEVELING_FEATURE SKEW_CORRECTION SKEW_CORRECTION_FOR_Z SKEW_CORRECTION_GCODE
93 93
   - opt_enable_adv FWRETRACT MAX7219_DEBUG LED_CONTROL_MENU
94 94
   - opt_set ABL_GRID_POINTS_X 16
95 95
   - opt_set ABL_GRID_POINTS_Y 16

+ 44
- 1
Marlin/Conditionals_post.h View File

@@ -845,6 +845,49 @@
845 845
   #endif
846 846
 
847 847
   /**
848
+   * XYZ Bed Skew Correction
849
+   */
850
+  #if ENABLED(SKEW_CORRECTION)
851
+    #define SKEW_FACTOR_MIN -1
852
+    #define SKEW_FACTOR_MAX 1
853
+
854
+    #define _GET_SIDE(a,b,c) (SQRT(2*sq(a)+2*sq(b)-4*sq(c))*0.5)
855
+    #define _SKEW_SIDE(a,b,c) tan(M_PI*0.5-acos((sq(a)-sq(b)-sq(c))/(2*c*b)))
856
+    #define _SKEW_FACTOR(a,b,c) _SKEW_SIDE(a,_GET_SIDE(a,b,c),c)
857
+
858
+    #ifndef XY_SKEW_FACTOR
859
+      constexpr float XY_SKEW_FACTOR = (
860
+        #if defined(XY_DIAG_AC) && defined(XY_DIAG_BD) && defined(XY_SIDE_AD)
861
+          _SKEW_FACTOR(XY_DIAG_AC, XY_DIAG_BD, XY_SIDE_AD)
862
+        #else
863
+          0.0
864
+        #endif
865
+      );
866
+    #endif
867
+    #ifndef XZ_SKEW_FACTOR
868
+      #if defined(XY_SIDE_AD) && !defined(XZ_SIDE_AD)
869
+        #define XZ_SIDE_AD XY_SIDE_AD
870
+      #endif
871
+      constexpr float XZ_SKEW_FACTOR = (
872
+        #if defined(XZ_DIAG_AC) && defined(XZ_DIAG_BD) && defined(XZ_SIDE_AD)
873
+          _SKEW_FACTOR(XZ_DIAG_AC, XZ_DIAG_BD, XZ_SIDE_AD)
874
+        #else
875
+          0.0
876
+        #endif
877
+      );
878
+    #endif
879
+    #ifndef YZ_SKEW_FACTOR
880
+      constexpr float YZ_SKEW_FACTOR = (
881
+        #if defined(YZ_DIAG_AC) && defined(YZ_DIAG_BD) && defined(YZ_SIDE_AD)
882
+          _SKEW_FACTOR(YZ_DIAG_AC, YZ_DIAG_BD, YZ_SIDE_AD)
883
+        #else
884
+          0.0
885
+        #endif
886
+      );
887
+    #endif
888
+  #endif // SKEW_CORRECTION
889
+
890
+  /**
848 891
    * Heater & Fan Pausing
849 892
    */
850 893
   #if FAN_COUNT == 0
@@ -898,7 +941,7 @@
898 941
   #define HAS_LEVELING          (HAS_ABL || ENABLED(MESH_BED_LEVELING))
899 942
   #define HAS_AUTOLEVEL         (HAS_ABL && DISABLED(PROBE_MANUALLY))
900 943
   #define HAS_MESH              (ENABLED(AUTO_BED_LEVELING_BILINEAR) || ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(MESH_BED_LEVELING))
901
-  #define PLANNER_LEVELING      (OLDSCHOOL_ABL || ENABLED(MESH_BED_LEVELING) || UBL_DELTA)
944
+  #define PLANNER_LEVELING      (OLDSCHOOL_ABL || ENABLED(MESH_BED_LEVELING) || UBL_DELTA || ENABLED(SKEW_CORRECTION))
902 945
   #define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST))
903 946
   #if HAS_PROBING_PROCEDURE
904 947
     #define PROBE_BED_WIDTH abs(RIGHT_PROBE_BED_POSITION - (LEFT_PROBE_BED_POSITION))

+ 58
- 1
Marlin/Configuration.h View File

@@ -832,7 +832,7 @@
832 832
 //===========================================================================
833 833
 //=============================== Bed Leveling ==============================
834 834
 //===========================================================================
835
-// @section bedlevel
835
+// @section calibrate
836 836
 
837 837
 /**
838 838
  * Choose one of the options below to enable G29 Bed Leveling. The parameters
@@ -1044,6 +1044,63 @@
1044 1044
 #define HOMING_FEEDRATE_XY (50*60)
1045 1045
 #define HOMING_FEEDRATE_Z  (4*60)
1046 1046
 
1047
+// @section calibrate
1048
+
1049
+/**
1050
+ * Bed Skew Compensation
1051
+ *
1052
+ * This feature corrects for misalignment in the XYZ axes.
1053
+ *
1054
+ * Take the following steps to get the bed skew in the XY plane:
1055
+ *  1. Print a test square (e.g., https://www.thingiverse.com/thing:2563185)
1056
+ *  2. For XY_DIAG_AC measure the diagonal A to C
1057
+ *  3. For XY_DIAG_BD measure the diagonal B to D
1058
+ *  4. For XY_SIDE_AD measure the edge A to D
1059
+ *
1060
+ * Marlin automatically computes skew factors from these measurements.
1061
+ * Skew factors may also be computed and set manually:
1062
+ *
1063
+ *  - Compute AB     : SQRT(2*AC*AC+2*BD*BD-4*AD*AD)/2
1064
+ *  - XY_SKEW_FACTOR : TAN(PI/2-ACOS((AC*AC-AB*AB-AD*AD)/(2*AB*AD)))
1065
+ *
1066
+ * If desired, follow the same procedure for XZ and YZ.
1067
+ * Use these diagrams for reference:
1068
+ *
1069
+ *    Y                     Z                     Z
1070
+ *    ^     B-------C       ^     B-------C       ^     B-------C
1071
+ *    |    /       /        |    /       /        |    /       /
1072
+ *    |   /       /         |   /       /         |   /       /
1073
+ *    |  A-------D          |  A-------D          |  A-------D
1074
+ *    +-------------->X     +-------------->X     +-------------->Y
1075
+ *     XY_SKEW_FACTOR        XZ_SKEW_FACTOR        YZ_SKEW_FACTOR
1076
+ */
1077
+//#define SKEW_CORRECTION
1078
+
1079
+#if ENABLED(SKEW_CORRECTION)
1080
+  // Input all length measurements here:
1081
+  #define XY_DIAG_AC 282.8427124746
1082
+  #define XY_DIAG_BD 282.8427124746
1083
+  #define XY_SIDE_AD 200
1084
+
1085
+  // Or, set the default skew factors directly here
1086
+  // to override the above measurements:
1087
+  #define XY_SKEW_FACTOR 0.0
1088
+
1089
+  //#define SKEW_CORRECTION_FOR_Z
1090
+  #if ENABLED(SKEW_CORRECTION_FOR_Z)
1091
+    #define XZ_DIAG_AC 282.8427124746
1092
+    #define XZ_DIAG_BD 282.8427124746
1093
+    #define YZ_DIAG_AC 282.8427124746
1094
+    #define YZ_DIAG_BD 282.8427124746
1095
+    #define YZ_SIDE_AD 200
1096
+    #define XZ_SKEW_FACTOR 0.0
1097
+    #define YZ_SKEW_FACTOR 0.0
1098
+  #endif
1099
+
1100
+  // Enable this option for M852 to set skew at runtime
1101
+  //#define SKEW_CORRECTION_GCODE
1102
+#endif
1103
+
1047 1104
 //=============================================================================
1048 1105
 //============================= Additional Features ===========================
1049 1106
 //=============================================================================

+ 70
- 0
Marlin/Marlin_main.cpp View File

@@ -205,6 +205,7 @@
205 205
  * M666 - Set delta endstop adjustment. (Requires DELTA)
206 206
  * M605 - Set dual x-carriage movement mode: "M605 S<mode> [X<x_offset>] [R<temp_offset>]". (Requires DUAL_X_CARRIAGE)
207 207
  * M851 - Set Z probe's Z offset in current units. (Negative = below the nozzle.)
208
+ * M852 - Set skew factors: "M852 [I<xy>] [J<xz>] [K<yz>]". (Requires SKEW_CORRECTION_GCODE, and SKEW_CORRECTION_FOR_Z for IJ)
208 209
  * M860 - Report the position of position encoder modules.
209 210
  * M861 - Report the status of position encoder modules.
210 211
  * M862 - Perform an axis continuity test for position encoder modules.
@@ -9942,6 +9943,69 @@ inline void gcode_M502() {
9942 9943
 
9943 9944
 #endif // HAS_BED_PROBE
9944 9945
 
9946
+#if ENABLED(SKEW_CORRECTION_GCODE)
9947
+
9948
+  /**
9949
+   * M852: Get or set the machine skew factors. Reports current values with no arguments.
9950
+   *
9951
+   *  S[xy_factor] - Alias for 'I'
9952
+   *  I[xy_factor] - New XY skew factor
9953
+   *  J[xz_factor] - New XZ skew factor
9954
+   *  K[yz_factor] - New YZ skew factor
9955
+   */
9956
+  inline void gcode_M852() {
9957
+    const bool ijk = parser.seen('I') || parser.seen('S')
9958
+      #if ENABLED(SKEW_CORRECTION_FOR_Z)
9959
+        || parser.seen('J') || parser.seen('K')
9960
+      #endif
9961
+    ;
9962
+    bool badval = false;
9963
+
9964
+    if (parser.seen('I') || parser.seen('S')) {
9965
+      const float value = parser.value_linear_units();
9966
+      if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX))
9967
+        planner.xy_skew_factor = value;
9968
+      else
9969
+        badval = true;
9970
+    }
9971
+
9972
+    #if ENABLED(SKEW_CORRECTION_FOR_Z)
9973
+
9974
+      if (parser.seen('J')) {
9975
+        const float value = parser.value_linear_units();
9976
+        if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX))
9977
+          planner.xz_skew_factor = value;
9978
+        else
9979
+          badval = true;
9980
+      }
9981
+
9982
+      if (parser.seen('K')) {
9983
+        const float value = parser.value_linear_units();
9984
+        if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX))
9985
+          planner.yz_skew_factor = value;
9986
+        else
9987
+          badval = true;
9988
+      }
9989
+
9990
+    #endif
9991
+
9992
+    if (badval)
9993
+      SERIAL_ECHOLNPGM(MSG_SKEW_MIN " " STRINGIFY(SKEW_FACTOR_MIN) " " MSG_SKEW_MAX " " STRINGIFY(SKEW_FACTOR_MAX));
9994
+
9995
+    if (!ijk) {
9996
+      SERIAL_ECHO_START();
9997
+      SERIAL_ECHOPAIR(MSG_SKEW_FACTOR " XY: ", planner.xy_skew_factor);
9998
+      #if ENABLED(SKEW_CORRECTION_FOR_Z)
9999
+        SERIAL_ECHOPAIR(" XZ: ", planner.xz_skew_factor);
10000
+        SERIAL_ECHOLNPAIR(" YZ: ", planner.yz_skew_factor);
10001
+      #else
10002
+        SERIAL_EOL();
10003
+      #endif
10004
+    }
10005
+  }
10006
+
10007
+#endif // SKEW_CORRECTION_GCODE
10008
+
9945 10009
 #if ENABLED(ADVANCED_PAUSE_FEATURE)
9946 10010
 
9947 10011
   /**
@@ -11787,6 +11851,12 @@ void process_parsed_command() {
11787 11851
           break;
11788 11852
       #endif // HAS_BED_PROBE
11789 11853
 
11854
+      #if ENABLED(SKEW_CORRECTION_GCODE)
11855
+        case 852: // M852: Set Skew factors
11856
+          gcode_M852();
11857
+          break;
11858
+      #endif
11859
+
11790 11860
       #if ENABLED(ADVANCED_PAUSE_FEATURE)
11791 11861
         case 600: // M600: Pause for filament change
11792 11862
           gcode_M600();

+ 14
- 0
Marlin/SanityCheck.h View File

@@ -1518,3 +1518,17 @@ static_assert(COUNT(sanity_arr_3) <= XYZE_N, "DEFAULT_MAX_ACCELERATION has too m
1518 1518
 #if ENABLED(LED_CONTROL_MENU) && DISABLED(ULTIPANEL)
1519 1519
   #error "LED_CONTROL_MENU requires an LCD controller."
1520 1520
 #endif
1521
+
1522
+#if ENABLED(SKEW_CORRECTION)
1523
+  #if !defined(XY_SKEW_FACTOR) && !(defined(XY_DIAG_AC) && defined(XY_DIAG_BD) && defined(XY_SIDE_AD))
1524
+    #error "SKEW_CORRECTION requires XY_SKEW_FACTOR or XY_DIAG_AC, XY_DIAG_BD, XY_SIDE_AD."
1525
+  #endif
1526
+  #if ENABLED(SKEW_CORRECTION_FOR_Z)
1527
+    #if !defined(XZ_SKEW_FACTOR) && !(defined(XZ_DIAG_AC) && defined(XZ_DIAG_BD) && defined(XZ_SIDE_AD))
1528
+      #error "SKEW_CORRECTION requires XZ_SKEW_FACTOR or XZ_DIAG_AC, XZ_DIAG_BD, XZ_SIDE_AD."
1529
+    #endif
1530
+    #if !defined(YZ_SKEW_FACTOR) && !(defined(YZ_DIAG_AC) && defined(YZ_DIAG_BD) && defined(YZ_SIDE_AD))
1531
+      #error "SKEW_CORRECTION requires YZ_SKEW_FACTOR or YZ_DIAG_AC, YZ_DIAG_BD, YZ_SIDE_AD."
1532
+    #endif
1533
+  #endif
1534
+#endif

+ 69
- 4
Marlin/configuration_store.cpp View File

@@ -36,13 +36,13 @@
36 36
  *
37 37
  */
38 38
 
39
-#define EEPROM_VERSION "V45"
39
+#define EEPROM_VERSION "V46"
40 40
 
41 41
 // Change EEPROM version if these are changed:
42 42
 #define EEPROM_OFFSET 100
43 43
 
44 44
 /**
45
- * V45 EEPROM Layout:
45
+ * V46 EEPROM Layout:
46 46
  *
47 47
  *  100  Version                                    (char x4)
48 48
  *  104  EEPROM CRC16                               (uint16_t)
@@ -166,8 +166,13 @@
166 166
  * CNC_COORDINATE_SYSTEMS                           108 bytes
167 167
  *  602  G54-G59.3 coordinate_system                (float x 27)
168 168
  *
169
- *  710                                   Minimum end-point
170
- * 2239 (710 + 208 + 36 + 9 + 288 + 988)  Maximum end-point
169
+ * SKEW_CORRECTION:                                 12 bytes
170
+ *  710  M852 I    planner.xy_skew_factor           (float)
171
+ *  714  M852 J    planner.xz_skew_factor           (float)
172
+ *  718  M852 K    planner.yz_skew_factor           (float)
173
+ *
174
+ *  722                                   Minimum end-point
175
+ * 2251 (722 + 208 + 36 + 9 + 288 + 988)  Maximum end-point
171 176
  *
172 177
  * ========================================================================
173 178
  * meshes_begin (between max and min end-point, directly above)
@@ -664,6 +669,10 @@ void MarlinSettings::postprocess() {
664 669
       for (uint8_t q = 3; q--;) EEPROM_WRITE(dummyui32);
665 670
     #endif
666 671
 
672
+    //
673
+    // CNC Coordinate Systems
674
+    //
675
+
667 676
     #if ENABLED(CNC_COORDINATE_SYSTEMS)
668 677
       EEPROM_WRITE(coordinate_system); // 27 floats
669 678
     #else
@@ -671,6 +680,19 @@ void MarlinSettings::postprocess() {
671 680
       for (uint8_t q = 27; q--;) EEPROM_WRITE(dummy);
672 681
     #endif
673 682
 
683
+    //
684
+    // Skew correction factors
685
+    //
686
+
687
+    #if ENABLED(SKEW_CORRECTION)
688
+      EEPROM_WRITE(planner.xy_skew_factor);
689
+      EEPROM_WRITE(planner.xz_skew_factor);
690
+      EEPROM_WRITE(planner.yz_skew_factor);
691
+    #else
692
+      dummy = 0.0f;
693
+      for (uint8_t q = 3; q--;) EEPROM_WRITE(dummy);
694
+    #endif
695
+
674 696
     if (!eeprom_error) {
675 697
       const int eeprom_size = eeprom_index;
676 698
 
@@ -1105,6 +1127,23 @@ void MarlinSettings::postprocess() {
1105 1127
         for (uint8_t q = 27; q--;) EEPROM_READ(dummy);
1106 1128
       #endif
1107 1129
 
1130
+      //
1131
+      // Skew correction factors
1132
+      //
1133
+
1134
+      #if ENABLED(SKEW_CORRECTION_GCODE)
1135
+        EEPROM_READ(planner.xy_skew_factor);
1136
+        #if ENABLED(SKEW_CORRECTION_FOR_Z)
1137
+          EEPROM_READ(planner.xz_skew_factor);
1138
+          EEPROM_READ(planner.yz_skew_factor);
1139
+        #else
1140
+          EEPROM_READ(dummy);
1141
+          EEPROM_READ(dummy);
1142
+        #endif
1143
+      #else
1144
+        for (uint8_t q = 3; q--;) EEPROM_READ(dummy);
1145
+      #endif
1146
+
1108 1147
       if (working_crc == stored_crc) {
1109 1148
         postprocess();
1110 1149
         #if ENABLED(EEPROM_CHITCHAT)
@@ -1483,6 +1522,14 @@ void MarlinSettings::reset() {
1483 1522
     ubl.reset();
1484 1523
   #endif
1485 1524
 
1525
+  #if ENABLED(SKEW_CORRECTION_GCODE)
1526
+    planner.xy_skew_factor = XY_SKEW_FACTOR;
1527
+    #if ENABLED(SKEW_CORRECTION_FOR_Z)
1528
+      planner.xz_skew_factor = XZ_SKEW_FACTOR;
1529
+      planner.yz_skew_factor = YZ_SKEW_FACTOR;
1530
+    #endif
1531
+  #endif
1532
+
1486 1533
   postprocess();
1487 1534
 
1488 1535
   #if ENABLED(EEPROM_CHITCHAT)
@@ -1913,6 +1960,24 @@ void MarlinSettings::reset() {
1913 1960
     #endif
1914 1961
 
1915 1962
     /**
1963
+     * Bed Skew Correction
1964
+     */
1965
+    #if ENABLED(SKEW_CORRECTION_GCODE)
1966
+      if (!forReplay) {
1967
+        CONFIG_ECHO_START;
1968
+        SERIAL_ECHOLNPGM("Skew Factor: ");
1969
+      }
1970
+      CONFIG_ECHO_START;
1971
+      #if ENABLED(SKEW_CORRECTION_FOR_Z)
1972
+        SERIAL_ECHOPAIR("  M852 I", LINEAR_UNIT(planner.xy_skew_factor));
1973
+        SERIAL_ECHOPAIR(" J", LINEAR_UNIT(planner.xz_skew_factor));
1974
+        SERIAL_ECHOLNPAIR(" K", LINEAR_UNIT(planner.yz_skew_factor));
1975
+      #else
1976
+        SERIAL_ECHOLNPAIR("  M852 S", LINEAR_UNIT(planner.xy_skew_factor));
1977
+      #endif
1978
+    #endif
1979
+
1980
+    /**
1916 1981
      * TMC2130 stepper driver current
1917 1982
      */
1918 1983
     #if ENABLED(HAVE_TMC2130)

+ 3
- 0
Marlin/language.h View File

@@ -162,6 +162,9 @@
162 162
 #define MSG_Z2_MAX                          "z2_max: "
163 163
 #define MSG_Z_PROBE                         "z_probe: "
164 164
 #define MSG_PROBE_Z_OFFSET                  "Probe Z Offset"
165
+#define MSG_SKEW_MIN                        "min_skew_factor: "
166
+#define MSG_SKEW_MAX                        "max_skew_factor: "
167
+#define MSG_SKEW_WARN                       "WARNING: Skew compensation disabled (outside MIN/MAX limits)"
165 168
 #define MSG_FILAMENT_RUNOUT_SENSOR          "filament: "
166 169
 #define MSG_ERR_MATERIAL_INDEX              "M145 S<index> out of range (0-1)"
167 170
 #define MSG_ERR_M355_NONE                   "No case light"

+ 3
- 0
Marlin/language_en.h View File

@@ -699,6 +699,9 @@
699 699
 #ifndef MSG_ZPROBE_OUT
700 700
   #define MSG_ZPROBE_OUT                      _UxGT("Z probe out. bed")
701 701
 #endif
702
+#ifndef MSG_SKEW_FACTOR
703
+  #define MSG_SKEW_FACTOR                     _UxGT("Skew Factor")
704
+#endif
702 705
 #ifndef MSG_BLTOUCH
703 706
   #define MSG_BLTOUCH                         _UxGT("BLTouch")
704 707
 #endif

+ 68
- 30
Marlin/planner.cpp View File

@@ -124,6 +124,20 @@ float Planner::min_feedrate_mm_s,
124 124
   #endif
125 125
 #endif
126 126
 
127
+#if ENABLED(SKEW_CORRECTION)
128
+  #if ENABLED(SKEW_CORRECTION_GCODE)
129
+    // Initialized by settings.load()
130
+    float Planner::xy_skew_factor;
131
+    #if ENABLED(SKEW_CORRECTION_FOR_Z)
132
+      float Planner::xz_skew_factor, Planner::yz_skew_factor;
133
+    #else
134
+      constexpr float Planner::xz_skew_factor, Planner::yz_skew_factor;
135
+    #endif
136
+  #else
137
+    constexpr float Planner::xy_skew_factor, Planner::xz_skew_factor, Planner::yz_skew_factor;
138
+  #endif
139
+#endif
140
+
127 141
 #if ENABLED(AUTOTEMP)
128 142
   float Planner::autotemp_max = 250,
129 143
         Planner::autotemp_min = 210,
@@ -554,6 +568,19 @@ void Planner::calculate_volumetric_multipliers() {
554 568
    */
555 569
   void Planner::apply_leveling(float &rx, float &ry, float &rz) {
556 570
 
571
+    #if ENABLED(SKEW_CORRECTION)
572
+      if (WITHIN(rx, X_MIN_POS + 1, X_MAX_POS) && WITHIN(ry, Y_MIN_POS + 1, Y_MAX_POS)) {
573
+        const float tempry = ry - (rz * planner.yz_skew_factor),
574
+                    temprx = rx - (ry * planner.xy_skew_factor) - (rz * (planner.xz_skew_factor - (planner.xy_skew_factor * planner.yz_skew_factor)));
575
+        if (WITHIN(temprx, X_MIN_POS, X_MAX_POS) && WITHIN(tempry, Y_MIN_POS, Y_MAX_POS)) {
576
+          rx = temprx;
577
+          ry = tempry;
578
+        }
579
+        else
580
+          SERIAL_ECHOLN(MSG_SKEW_WARN);
581
+      }
582
+    #endif
583
+
557 584
     if (!leveling_active) return;
558 585
 
559 586
     #if ABL_PLANAR
@@ -600,45 +627,56 @@ void Planner::calculate_volumetric_multipliers() {
600 627
 
601 628
   void Planner::unapply_leveling(float raw[XYZ]) {
602 629
 
603
-    if (!leveling_active) return;
630
+    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
631
+      const float fade_scaling_factor = fade_scaling_factor_for_z(raw[Z_AXIS]);
632
+    #else
633
+      constexpr float fade_scaling_factor = 1.0;
634
+    #endif
604 635
 
605
-    #if ABL_PLANAR
636
+    if (leveling_active && fade_scaling_factor) {
606 637
 
607
-      matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
638
+      #if ABL_PLANAR
608 639
 
609
-      float dx = raw[X_AXIS] - (X_TILT_FULCRUM),
610
-            dy = raw[Y_AXIS] - (Y_TILT_FULCRUM);
640
+        matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
611 641
 
612
-      apply_rotation_xyz(inverse, dx, dy, raw[Z_AXIS]);
642
+        float dx = raw[X_AXIS] - (X_TILT_FULCRUM),
643
+              dy = raw[Y_AXIS] - (Y_TILT_FULCRUM);
613 644
 
614
-      raw[X_AXIS] = dx + X_TILT_FULCRUM;
615
-      raw[Y_AXIS] = dy + Y_TILT_FULCRUM;
645
+        apply_rotation_xyz(inverse, dx, dy, raw[Z_AXIS]);
616 646
 
617
-    #else
647
+        raw[X_AXIS] = dx + X_TILT_FULCRUM;
648
+        raw[Y_AXIS] = dy + Y_TILT_FULCRUM;
618 649
 
619
-      #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
620
-        const float fade_scaling_factor = fade_scaling_factor_for_z(raw[Z_AXIS]);
621
-        if (!fade_scaling_factor) return;
622
-      #elif HAS_MESH
623
-        constexpr float fade_scaling_factor = 1.0;
624
-      #endif
650
+      #else // !ABL_PLANAR
625 651
 
626
-      raw[Z_AXIS] -= (
627
-        #if ENABLED(AUTO_BED_LEVELING_UBL)
628
-          ubl.get_z_correction(raw[X_AXIS], raw[Y_AXIS]) * fade_scaling_factor
629
-        #elif ENABLED(MESH_BED_LEVELING)
630
-          mbl.get_z(raw[X_AXIS], raw[Y_AXIS]
631
-            #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
632
-              , fade_scaling_factor
633
-            #endif
634
-          )
635
-        #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
636
-          bilinear_z_offset(raw) * fade_scaling_factor
637
-        #else
638
-          0
639
-        #endif
640
-      );
652
+        raw[Z_AXIS] -= (
653
+          #if ENABLED(AUTO_BED_LEVELING_UBL)
654
+            ubl.get_z_correction(raw[X_AXIS], raw[Y_AXIS]) * fade_scaling_factor
655
+          #elif ENABLED(MESH_BED_LEVELING)
656
+            mbl.get_z(raw[X_AXIS], raw[Y_AXIS]
657
+              #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
658
+                , fade_scaling_factor
659
+              #endif
660
+            )
661
+          #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
662
+            bilinear_z_offset(raw) * fade_scaling_factor
663
+          #else
664
+            0
665
+          #endif
666
+        );
641 667
 
668
+      #endif // !ABL_PLANAR
669
+    }
670
+
671
+    #if ENABLED(SKEW_CORRECTION)
672
+      if (WITHIN(raw[X_AXIS], X_MIN_POS, X_MAX_POS) && WITHIN(raw[Y_AXIS], Y_MIN_POS, Y_MAX_POS)) {
673
+        const float temprx = raw[X_AXIS] + raw[Y_AXIS] * planner.xy_skew_factor + raw[Z_AXIS] * planner.xz_skew_factor,
674
+                    tempry = raw[Y_AXIS] + raw[Z_AXIS] * planner.yz_skew_factor;
675
+        if (WITHIN(temprx, X_MIN_POS, X_MAX_POS) && WITHIN(tempry, Y_MIN_POS, Y_MAX_POS)) {
676
+          raw[X_AXIS] = temprx;
677
+          raw[Y_AXIS] = tempry;
678
+        }
679
+      }
642 680
     #endif
643 681
   }
644 682
 

+ 17
- 0
Marlin/planner.h View File

@@ -176,6 +176,23 @@ class Planner {
176 176
       static float extruder_advance_k, advance_ed_ratio;
177 177
     #endif
178 178
 
179
+    #if ENABLED(SKEW_CORRECTION)
180
+      #if ENABLED(SKEW_CORRECTION_GCODE)
181
+        static float xy_skew_factor;
182
+      #else
183
+        static constexpr float xy_skew_factor = XY_SKEW_FACTOR;
184
+      #endif
185
+      #if ENABLED(SKEW_CORRECTION_FOR_Z)
186
+        #if ENABLED(SKEW_CORRECTION_GCODE)
187
+          static float xz_skew_factor, yz_skew_factor;
188
+        #else
189
+          static constexpr float xz_skew_factor = XZ_SKEW_FACTOR, yz_skew_factor = YZ_SKEW_FACTOR;
190
+        #endif
191
+      #else
192
+        static constexpr float xz_skew_factor = 0, yz_skew_factor = 0;
193
+      #endif
194
+    #endif
195
+
179 196
   private:
180 197
 
181 198
     /**

Loading…
Cancel
Save