Feature file updates

Marlin/src/feature/I2CPositionEncoder.h

@@ -23,88 +23,85 @@
 #ifndef I2CPOSENC_H
 #define I2CPOSENC_H
 
-#include "MarlinConfig.h"
+#include "../inc/MarlinConfig.h"
 
-#if ENABLED(I2C_POSITION_ENCODERS)
+#include "../module/planner.h"
 
-  #include "enum.h"
-  #include "macros.h"
-  #include "types.h"
-  #include <Wire.h>
+#include <Wire.h>
 
-  //=========== Advanced / Less-Common Encoder Configuration Settings ==========
+//=========== Advanced / Less-Common Encoder Configuration Settings ==========
 
-  #define I2CPE_EC_THRESH_PROPORTIONAL                    // if enabled adjusts the error correction threshold
-                                                          // proportional to the current speed of the axis allows
-                                                          // for very small error margin at low speeds without
-                                                          // stuttering due to reading latency at high speeds
+#define I2CPE_EC_THRESH_PROPORTIONAL                    // if enabled adjusts the error correction threshold
+                                                        // proportional to the current speed of the axis allows
+                                                        // for very small error margin at low speeds without
+                                                        // stuttering due to reading latency at high speeds
 
-  #define I2CPE_DEBUG                                     // enable encoder-related debug serial echos
+#define I2CPE_DEBUG                                     // enable encoder-related debug serial echos
 
-  #define I2CPE_REBOOT_TIME             5000              // time we wait for an encoder module to reboot
-                                                          // after changing address.
+#define I2CPE_REBOOT_TIME             5000              // time we wait for an encoder module to reboot
+                                                        // after changing address.
 
-  #define I2CPE_MAG_SIG_GOOD            0
-  #define I2CPE_MAG_SIG_MID             1
-  #define I2CPE_MAG_SIG_BAD             2
-  #define I2CPE_MAG_SIG_NF              255
+#define I2CPE_MAG_SIG_GOOD            0
+#define I2CPE_MAG_SIG_MID             1
+#define I2CPE_MAG_SIG_BAD             2
+#define I2CPE_MAG_SIG_NF              255
 
-  #define I2CPE_REQ_REPORT              0
-  #define I2CPE_RESET_COUNT             1
-  #define I2CPE_SET_ADDR                2
-  #define I2CPE_SET_REPORT_MODE         3
-  #define I2CPE_CLEAR_EEPROM            4
+#define I2CPE_REQ_REPORT              0
+#define I2CPE_RESET_COUNT             1
+#define I2CPE_SET_ADDR                2
+#define I2CPE_SET_REPORT_MODE         3
+#define I2CPE_CLEAR_EEPROM            4
 
-  #define I2CPE_LED_PAR_MODE            10
-  #define I2CPE_LED_PAR_BRT             11
-  #define I2CPE_LED_PAR_RATE            14
+#define I2CPE_LED_PAR_MODE            10
+#define I2CPE_LED_PAR_BRT             11
+#define I2CPE_LED_PAR_RATE            14
 
-  #define I2CPE_REPORT_DISTANCE         0
-  #define I2CPE_REPORT_STRENGTH         1
-  #define I2CPE_REPORT_VERSION          2
+#define I2CPE_REPORT_DISTANCE         0
+#define I2CPE_REPORT_STRENGTH         1
+#define I2CPE_REPORT_VERSION          2
 
-  // Default I2C addresses
-  #define I2CPE_PRESET_ADDR_X           30
-  #define I2CPE_PRESET_ADDR_Y           31
-  #define I2CPE_PRESET_ADDR_Z           32
-  #define I2CPE_PRESET_ADDR_E           33
+// Default I2C addresses
+#define I2CPE_PRESET_ADDR_X           30
+#define I2CPE_PRESET_ADDR_Y           31
+#define I2CPE_PRESET_ADDR_Z           32
+#define I2CPE_PRESET_ADDR_E           33
 
-  #define I2CPE_DEF_AXIS                X_AXIS
-  #define I2CPE_DEF_ADDR                I2CPE_PRESET_ADDR_X
+#define I2CPE_DEF_AXIS                X_AXIS
+#define I2CPE_DEF_ADDR                I2CPE_PRESET_ADDR_X
 
-  // Error event counter; tracks how many times there is an error exceeding a certain threshold
-  #define I2CPE_ERR_CNT_THRESH          3.00
-  #define I2CPE_ERR_CNT_DEBOUNCE_MS     2000
+// Error event counter; tracks how many times there is an error exceeding a certain threshold
+#define I2CPE_ERR_CNT_THRESH          3.00
+#define I2CPE_ERR_CNT_DEBOUNCE_MS     2000
 
-  #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
-    #define I2CPE_ERR_ARRAY_SIZE        32
-  #endif
+#if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
+  #define I2CPE_ERR_ARRAY_SIZE        32
+#endif
 
-  // Error Correction Methods
-  #define I2CPE_ECM_NONE                0
-  #define I2CPE_ECM_MICROSTEP           1
-  #define I2CPE_ECM_PLANNER             2
-  #define I2CPE_ECM_STALLDETECT         3
+// Error Correction Methods
+#define I2CPE_ECM_NONE                0
+#define I2CPE_ECM_MICROSTEP           1
+#define I2CPE_ECM_PLANNER             2
+#define I2CPE_ECM_STALLDETECT         3
 
-  // Encoder types
-  #define I2CPE_ENC_TYPE_ROTARY         0
-  #define I2CPE_ENC_TYPE_LINEAR         1
+// Encoder types
+#define I2CPE_ENC_TYPE_ROTARY         0
+#define I2CPE_ENC_TYPE_LINEAR         1
 
-  // Parser
-  #define I2CPE_PARSE_ERR               1
-  #define I2CPE_PARSE_OK                0
+// Parser
+#define I2CPE_PARSE_ERR               1
+#define I2CPE_PARSE_OK                0
 
-  #define LOOP_PE(VAR) LOOP_L_N(VAR, I2CPE_ENCODER_CNT)
-  #define CHECK_IDX() do{ if (!WITHIN(idx, 0, I2CPE_ENCODER_CNT - 1)) return; }while(0)
+#define LOOP_PE(VAR) LOOP_L_N(VAR, I2CPE_ENCODER_CNT)
+#define CHECK_IDX() do{ if (!WITHIN(idx, 0, I2CPE_ENCODER_CNT - 1)) return; }while(0)
 
-  extern const char axis_codes[XYZE];
+extern const char axis_codes[XYZE];
 
-  typedef union {
-    volatile int32_t val = 0;
-    uint8_t          bval[4];
-  } i2cLong;
+typedef union {
+  volatile int32_t val = 0;
+  uint8_t          bval[4];
+} i2cLong;
 
-  class I2CPositionEncoder {
+class I2CPositionEncoder {
   private:
     AxisEnum  encoderAxis         = I2CPE_DEF_AXIS;
 
@@ -229,9 +226,9 @@
     FORCE_INLINE void set_current_position(const float newPositionMm) {
       set_axis_offset(get_position_mm() - newPositionMm + axisOffset);
     }
-  };
+};
 
-  class I2CPositionEncodersMgr {
+class I2CPositionEncodersMgr {
   private:
     static bool I2CPE_anyaxis;
     static uint8_t I2CPE_addr, I2CPE_idx;
@@ -252,7 +249,7 @@
 
     static void report_status(const int8_t idx) {
       CHECK_IDX();
-      SERIAL_ECHOPAIR("Encoder ",idx);
+      SERIAL_ECHOPAIR("Encoder ", idx);
       SERIAL_ECHOPGM(": ");
       encoders[idx].get_raw_count();
       encoders[idx].passes_test(true);
@@ -340,20 +337,19 @@
     static void M869();
 
     static I2CPositionEncoder encoders[I2CPE_ENCODER_CNT];
-  };
-
-  extern I2CPositionEncodersMgr I2CPEM;
-
-  FORCE_INLINE static void gcode_M860() { I2CPEM.M860(); }
-  FORCE_INLINE static void gcode_M861() { I2CPEM.M861(); }
-  FORCE_INLINE static void gcode_M862() { I2CPEM.M862(); }
-  FORCE_INLINE static void gcode_M863() { I2CPEM.M863(); }
-  FORCE_INLINE static void gcode_M864() { I2CPEM.M864(); }
-  FORCE_INLINE static void gcode_M865() { I2CPEM.M865(); }
-  FORCE_INLINE static void gcode_M866() { I2CPEM.M866(); }
-  FORCE_INLINE static void gcode_M867() { I2CPEM.M867(); }
-  FORCE_INLINE static void gcode_M868() { I2CPEM.M868(); }
-  FORCE_INLINE static void gcode_M869() { I2CPEM.M869(); }
-
-#endif //I2C_POSITION_ENCODERS
+};
+
+extern I2CPositionEncodersMgr I2CPEM;
+
+FORCE_INLINE static void gcode_M860() { I2CPEM.M860(); }
+FORCE_INLINE static void gcode_M861() { I2CPEM.M861(); }
+FORCE_INLINE static void gcode_M862() { I2CPEM.M862(); }
+FORCE_INLINE static void gcode_M863() { I2CPEM.M863(); }
+FORCE_INLINE static void gcode_M864() { I2CPEM.M864(); }
+FORCE_INLINE static void gcode_M865() { I2CPEM.M865(); }
+FORCE_INLINE static void gcode_M866() { I2CPEM.M866(); }
+FORCE_INLINE static void gcode_M867() { I2CPEM.M867(); }
+FORCE_INLINE static void gcode_M868() { I2CPEM.M868(); }
+FORCE_INLINE static void gcode_M869() { I2CPEM.M869(); }
+
 #endif //I2CPOSENC_H

Marlin/src/feature/dac/dac_dac084s085.cpp

@@ -3,106 +3,110 @@
  * External DAC for Alligator Board
  *
  ****************************************************************/
-#include "Marlin.h"
 
-#if MB(ALLIGATOR)
-  #include "stepper.h"
-  #include "dac_dac084s085.h"
+#include "../../inc/MarlinConfig.h"
 
-  dac084s085::dac084s085() {
-    return ;
-  }
+#if MB(ALLIGATOR)
 
-  void dac084s085::begin() {
-    uint8_t externalDac_buf[2] = {0x20,0x00};//all off
-
-    // All SPI chip-select HIGH
-    pinMode(DAC0_SYNC, OUTPUT);
-    digitalWrite( DAC0_SYNC , HIGH );
-    #if EXTRUDERS > 1
-      pinMode(DAC1_SYNC, OUTPUT);
-      digitalWrite( DAC1_SYNC , HIGH );
-    #endif
-    digitalWrite( SPI_EEPROM1_CS , HIGH );
-    digitalWrite( SPI_EEPROM2_CS , HIGH );
-    digitalWrite( SPI_FLASH_CS , HIGH );
-    digitalWrite( SS_PIN , HIGH );
-    spiBegin();
-
-    //init onboard DAC
+#include "dac_dac084s085.h"
+
+#include "../../Marlin.h"
+#include "../../module/stepper.h"
+
+dac084s085::dac084s085() {
+  return ;
+}
+
+void dac084s085::begin() {
+  uint8_t externalDac_buf[2] = {0x20,0x00};//all off
+
+  // All SPI chip-select HIGH
+  pinMode(DAC0_SYNC, OUTPUT);
+  digitalWrite( DAC0_SYNC , HIGH );
+  #if EXTRUDERS > 1
+    pinMode(DAC1_SYNC, OUTPUT);
+    digitalWrite( DAC1_SYNC , HIGH );
+  #endif
+  digitalWrite( SPI_EEPROM1_CS , HIGH );
+  digitalWrite( SPI_EEPROM2_CS , HIGH );
+  digitalWrite( SPI_FLASH_CS , HIGH );
+  digitalWrite( SS_PIN , HIGH );
+  spiBegin();
+
+  //init onboard DAC
+  delayMicroseconds(2U);
+  digitalWrite( DAC0_SYNC , LOW );
+  delayMicroseconds(2U);
+  digitalWrite( DAC0_SYNC , HIGH );
+  delayMicroseconds(2U);
+  digitalWrite( DAC0_SYNC , LOW );
+
+  spiSend(SPI_CHAN_DAC,externalDac_buf , 2);
+  digitalWrite( DAC0_SYNC , HIGH );
+
+  #if EXTRUDERS > 1
+    //init Piggy DAC
     delayMicroseconds(2U);
-    digitalWrite( DAC0_SYNC , LOW );
+    digitalWrite( DAC1_SYNC , LOW );
     delayMicroseconds(2U);
-    digitalWrite( DAC0_SYNC , HIGH );
+    digitalWrite( DAC1_SYNC , HIGH );
     delayMicroseconds(2U);
-    digitalWrite( DAC0_SYNC , LOW );
+    digitalWrite( DAC1_SYNC , LOW );
 
     spiSend(SPI_CHAN_DAC,externalDac_buf , 2);
-    digitalWrite( DAC0_SYNC , HIGH );
+    digitalWrite( DAC1_SYNC , HIGH );
+  #endif
 
-    #if EXTRUDERS > 1
-      //init Piggy DAC
-      delayMicroseconds(2U);
-      digitalWrite( DAC1_SYNC , LOW );
-      delayMicroseconds(2U);
-      digitalWrite( DAC1_SYNC , HIGH );
-      delayMicroseconds(2U);
-      digitalWrite( DAC1_SYNC , LOW );
-
-      spiSend(SPI_CHAN_DAC,externalDac_buf , 2);
-      digitalWrite( DAC1_SYNC , HIGH );
-    #endif
+  return;
+}
 
+void dac084s085::setValue(uint8_t channel, uint8_t value) {
+  if(channel >= 7) // max channel (X,Y,Z,E0,E1,E2,E3)
     return;
+  if(value > 255) value = 255;
+
+  uint8_t externalDac_buf[2] = {0x10,0x00};
+
+  if(channel > 3)
+    externalDac_buf[0] |= (7 - channel << 6);
+  else
+    externalDac_buf[0] |= (3 - channel << 6);
+
+  externalDac_buf[0] |= (value >> 4);
+  externalDac_buf[1] |= (value << 4);
+  
+  // All SPI chip-select HIGH
+  digitalWrite( DAC0_SYNC , HIGH );
+  #if EXTRUDERS > 1
+    digitalWrite( DAC1_SYNC , HIGH );
+  #endif
+  digitalWrite( SPI_EEPROM1_CS , HIGH );
+  digitalWrite( SPI_EEPROM2_CS , HIGH );
+  digitalWrite( SPI_FLASH_CS , HIGH );
+  digitalWrite( SS_PIN , HIGH );
+
+  if(channel > 3) { // DAC Piggy E1,E2,E3
+
+    digitalWrite(DAC1_SYNC , LOW);
+    delayMicroseconds(2U);
+    digitalWrite(DAC1_SYNC , HIGH);
+    delayMicroseconds(2U);
+    digitalWrite(DAC1_SYNC , LOW);
   }
 
-  void dac084s085::setValue(uint8_t channel, uint8_t value) {
-    if(channel >= 7) // max channel (X,Y,Z,E0,E1,E2,E3)
-      return;
-    if(value > 255) value = 255;
-
-    uint8_t externalDac_buf[2] = {0x10,0x00};
-
-    if(channel > 3)
-      externalDac_buf[0] |= (7 - channel << 6);
-    else
-      externalDac_buf[0] |= (3 - channel << 6);
-
-    externalDac_buf[0] |= (value >> 4);
-    externalDac_buf[1] |= (value << 4);
-    
-    // All SPI chip-select HIGH
-    digitalWrite( DAC0_SYNC , HIGH );
-    #if EXTRUDERS > 1
-      digitalWrite( DAC1_SYNC , HIGH );
-    #endif
-    digitalWrite( SPI_EEPROM1_CS , HIGH );
-    digitalWrite( SPI_EEPROM2_CS , HIGH );
-    digitalWrite( SPI_FLASH_CS , HIGH );
-    digitalWrite( SS_PIN , HIGH );
-
-    if(channel > 3) { // DAC Piggy E1,E2,E3
-
-      digitalWrite(DAC1_SYNC , LOW);
-      delayMicroseconds(2U);
-      digitalWrite(DAC1_SYNC , HIGH);
-      delayMicroseconds(2U);
-      digitalWrite(DAC1_SYNC , LOW);
-    }
-
-    else { // DAC onboard X,Y,Z,E0
-
-      digitalWrite(DAC0_SYNC , LOW);
-      delayMicroseconds(2U);
-      digitalWrite(DAC0_SYNC , HIGH);
-      delayMicroseconds(2U);
-      digitalWrite(DAC0_SYNC , LOW);
-    }
+  else { // DAC onboard X,Y,Z,E0
 
+    digitalWrite(DAC0_SYNC , LOW);
     delayMicroseconds(2U);
-    spiSend(SPI_CHAN_DAC,externalDac_buf , 2);
-
-    return;
+    digitalWrite(DAC0_SYNC , HIGH);
+    delayMicroseconds(2U);
+    digitalWrite(DAC0_SYNC , LOW);
   }
 
-#endif
+  delayMicroseconds(2U);
+  spiSend(SPI_CHAN_DAC,externalDac_buf , 2);
+
+  return;
+}
+
+#endif // MB(ALLIGATOR)

Marlin/src/feature/dac/dac_dac084s085.h

@@ -1,5 +1,5 @@
-#ifndef dac084s085_h
-#define dac084s085_h
+#ifndef DAC084S085_H
+#define DAC084S085_H
 
 class dac084s085 {
   public:
@@ -8,4 +8,4 @@ class dac084s085 {
     static void setValue(uint8_t channel, uint8_t value);
 };
 
-#endif //dac084s085_h
+#endif // DAC084S085_H

Marlin/src/feature/dac/dac_mcp4728.cpp

@@ -30,11 +30,12 @@
  * http://arduino.cc/forum/index.php/topic,51842.0.html
  */
 
-#include "dac_mcp4728.h"
-#include "enum.h"
+#include "../../inc/MarlinConfig.h"
 
 #if ENABLED(DAC_STEPPER_CURRENT)
 
+#include "dac_mcp4728.h"
+
 uint16_t mcp4728_values[XYZE];
 
 /**

Marlin/src/feature/dac/dac_mcp4728.h

@@ -27,10 +27,7 @@
 #ifndef DAC_MCP4728_H
 #define DAC_MCP4728_H
 
-#include "MarlinConfig.h"
-
-#if ENABLED(DAC_STEPPER_CURRENT)
-#include "Wire.h"
+#include <Wire.h>
 
 #define defaultVDD     DAC_STEPPER_MAX //was 5000 but differs with internal Vref
 #define BASE_ADDR      0x60
@@ -50,7 +47,6 @@
 // DAC_OR_ADDRESS defined in pins_BOARD.h  file
 #define DAC_DEV_ADDRESS (BASE_ADDR | DAC_OR_ADDRESS)
 
-
 void mcp4728_init();
 uint8_t mcp4728_analogWrite(uint8_t channel, uint16_t value);
 uint8_t mcp4728_eepromWrite();
@@ -62,5 +58,4 @@ uint8_t mcp4728_simpleCommand(byte simpleCommand);
 uint8_t mcp4728_getDrvPct(uint8_t channel);
 void mcp4728_setDrvPct(uint8_t pct[XYZE]);
 
-#endif
 #endif // DAC_MCP4728_H

Marlin/src/feature/dac/stepper_dac.cpp

@@ -41,85 +41,85 @@
   along with Marlin.  If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "Marlin.h"
+#include "../../inc/MarlinConfig.h"
 
 #if ENABLED(DAC_STEPPER_CURRENT)
 
-  #include "stepper_dac.h"
+#include "stepper_dac.h"
 
-  bool dac_present = false;
-  const uint8_t dac_order[NUM_AXIS] = DAC_STEPPER_ORDER;
-  uint8_t dac_channel_pct[XYZE] = DAC_MOTOR_CURRENT_DEFAULT;
+bool dac_present = false;
+const uint8_t dac_order[NUM_AXIS] = DAC_STEPPER_ORDER;
+uint8_t dac_channel_pct[XYZE] = DAC_MOTOR_CURRENT_DEFAULT;
 
-  int dac_init() {
-    #if PIN_EXISTS(DAC_DISABLE)
-      OUT_WRITE(DAC_DISABLE_PIN, LOW);  // set pin low to enable DAC
-    #endif
+int dac_init() {
+  #if PIN_EXISTS(DAC_DISABLE)
+    OUT_WRITE(DAC_DISABLE_PIN, LOW);  // set pin low to enable DAC
+  #endif
 
-    mcp4728_init();
+  mcp4728_init();
 
-    if (mcp4728_simpleCommand(RESET)) return -1;
+  if (mcp4728_simpleCommand(RESET)) return -1;
 
-    dac_present = true;
+  dac_present = true;
 
-    mcp4728_setVref_all(DAC_STEPPER_VREF);
-    mcp4728_setGain_all(DAC_STEPPER_GAIN);
+  mcp4728_setVref_all(DAC_STEPPER_VREF);
+  mcp4728_setGain_all(DAC_STEPPER_GAIN);
 
-    if (mcp4728_getDrvPct(0) < 1 || mcp4728_getDrvPct(1) < 1 || mcp4728_getDrvPct(2) < 1 || mcp4728_getDrvPct(3) < 1 ) {
-      mcp4728_setDrvPct(dac_channel_pct);
-      mcp4728_eepromWrite();
-    }
-
-    return 0;
-  }
-
-  void dac_current_percent(uint8_t channel, float val) {
-    if (!dac_present) return;
-
-    NOMORE(val, 100);
-
-    mcp4728_analogWrite(dac_order[channel], val * 0.01 * (DAC_STEPPER_MAX));
-    mcp4728_simpleCommand(UPDATE);
-  }
-
-  void dac_current_raw(uint8_t channel, uint16_t val) {
-    if (!dac_present) return;
-
-    NOMORE(val, DAC_STEPPER_MAX);
-
-    mcp4728_analogWrite(dac_order[channel], val);
-    mcp4728_simpleCommand(UPDATE);
-  }
-
-  static float dac_perc(int8_t n) { return 100.0 * mcp4728_getValue(dac_order[n]) * (1.0 / (DAC_STEPPER_MAX)); }
-  static float dac_amps(int8_t n) { return mcp4728_getDrvPct(dac_order[n]) * (DAC_STEPPER_MAX) * 0.125 * (1.0 / (DAC_STEPPER_SENSE)); }
-
-  uint8_t dac_current_get_percent(AxisEnum axis) { return mcp4728_getDrvPct(dac_order[axis]); }
-  void dac_current_set_percents(const uint8_t pct[XYZE]) {
-    LOOP_XYZE(i) dac_channel_pct[i] = pct[dac_order[i]];
+  if (mcp4728_getDrvPct(0) < 1 || mcp4728_getDrvPct(1) < 1 || mcp4728_getDrvPct(2) < 1 || mcp4728_getDrvPct(3) < 1 ) {
     mcp4728_setDrvPct(dac_channel_pct);
-  }
-
-  void dac_print_values() {
-    if (!dac_present) return;
-
-    SERIAL_ECHO_START();
-    SERIAL_ECHOLNPGM("Stepper current values in % (Amps):");
-    SERIAL_ECHO_START();
-    SERIAL_ECHOPAIR(" X:",  dac_perc(X_AXIS));
-    SERIAL_ECHOPAIR(" (",   dac_amps(X_AXIS));
-    SERIAL_ECHOPAIR(") Y:", dac_perc(Y_AXIS));
-    SERIAL_ECHOPAIR(" (",   dac_amps(Y_AXIS));
-    SERIAL_ECHOPAIR(") Z:", dac_perc(Z_AXIS));
-    SERIAL_ECHOPAIR(" (",   dac_amps(Z_AXIS));
-    SERIAL_ECHOPAIR(") E:", dac_perc(E_AXIS));
-    SERIAL_ECHOPAIR(" (",   dac_amps(E_AXIS));
-    SERIAL_ECHOLN(")");
-  }
-
-  void dac_commit_eeprom() {
-    if (!dac_present) return;
     mcp4728_eepromWrite();
   }
 
+  return 0;
+}
+
+void dac_current_percent(uint8_t channel, float val) {
+  if (!dac_present) return;
+
+  NOMORE(val, 100);
+
+  mcp4728_analogWrite(dac_order[channel], val * 0.01 * (DAC_STEPPER_MAX));
+  mcp4728_simpleCommand(UPDATE);
+}
+
+void dac_current_raw(uint8_t channel, uint16_t val) {
+  if (!dac_present) return;
+
+  NOMORE(val, DAC_STEPPER_MAX);
+
+  mcp4728_analogWrite(dac_order[channel], val);
+  mcp4728_simpleCommand(UPDATE);
+}
+
+static float dac_perc(int8_t n) { return 100.0 * mcp4728_getValue(dac_order[n]) * (1.0 / (DAC_STEPPER_MAX)); }
+static float dac_amps(int8_t n) { return mcp4728_getDrvPct(dac_order[n]) * (DAC_STEPPER_MAX) * 0.125 * (1.0 / (DAC_STEPPER_SENSE)); }
+
+uint8_t dac_current_get_percent(AxisEnum axis) { return mcp4728_getDrvPct(dac_order[axis]); }
+void dac_current_set_percents(const uint8_t pct[XYZE]) {
+  LOOP_XYZE(i) dac_channel_pct[i] = pct[dac_order[i]];
+  mcp4728_setDrvPct(dac_channel_pct);
+}
+
+void dac_print_values() {
+  if (!dac_present) return;
+
+  SERIAL_ECHO_START();
+  SERIAL_ECHOLNPGM("Stepper current values in % (Amps):");
+  SERIAL_ECHO_START();
+  SERIAL_ECHOPAIR(" X:",  dac_perc(X_AXIS));
+  SERIAL_ECHOPAIR(" (",   dac_amps(X_AXIS));
+  SERIAL_ECHOPAIR(") Y:", dac_perc(Y_AXIS));
+  SERIAL_ECHOPAIR(" (",   dac_amps(Y_AXIS));
+  SERIAL_ECHOPAIR(") Z:", dac_perc(Z_AXIS));
+  SERIAL_ECHOPAIR(" (",   dac_amps(Z_AXIS));
+  SERIAL_ECHOPAIR(") E:", dac_perc(E_AXIS));
+  SERIAL_ECHOPAIR(" (",   dac_amps(E_AXIS));
+  SERIAL_ECHOLN(")");
+}
+
+void dac_commit_eeprom() {
+  if (!dac_present) return;
+  mcp4728_eepromWrite();
+}
+
 #endif // DAC_STEPPER_CURRENT

Marlin/src/feature/digipot_mcp4018.cpp

@@ -20,11 +20,11 @@
  *
  */
 
-#include "MarlinConfig.h"
+#include "../inc/MarlinConfig.h"
 
 #if ENABLED(DIGIPOT_I2C) && ENABLED(DIGIPOT_MCP4018)
 
-#include "enum.h"
+#include "../core/enum.h"
 #include "Stream.h"
 #include "utility/twi.h"
 #include <SlowSoftI2CMaster.h>  //https://github.com/stawel/SlowSoftI2CMaster

Marlin/src/feature/digipot_mcp4451.cpp

@@ -20,7 +20,7 @@
  *
  */
 
-#include "MarlinConfig.h"
+#include "../inc/MarlinConfig.h"
 
 #if ENABLED(DIGIPOT_I2C) && DISABLED(DIGIPOT_MCP4018)
 

Marlin/src/feature/leds/Max7219_Debug_LEDs.cpp

@@ -49,15 +49,16 @@
  * void Max7219_idle_tasks();
  */
 
-#include "MarlinConfig.h"
+#include "../../inc/MarlinConfig.h"
 
 #if ENABLED(MAX7219_DEBUG)
 
-  #include "Marlin.h"
-  #include "planner.h"
-  #include "stepper.h"
   #include "Max7219_Debug_LEDs.h"
 
+  #include "../../module/planner.h"
+  #include "../../module/stepper.h"
+  #include "../../Marlin.h"
+
   static uint8_t LEDs[8] = { 0 };
 
   void Max7219_PutByte(uint8_t data) {

Marlin/src/feature/leds/blinkm.cpp

@@ -25,7 +25,7 @@
  * Created by Tim Koster, August 21 2013.
  */
 
-#include "Marlin.h"
+#include "../../Marlin.h"
 
 #if ENABLED(BLINKM)
 

Marlin/src/feature/leds/pca9632.cpp

@@ -20,12 +20,12 @@
  *
  */
 
-/*
+/**
  * Driver for the Philips PCA9632 LED driver.
  * Written by Robert Mendon Feb 2017.
  */
 
-#include "MarlinConfig.h"
+#include "../../inc/MarlinConfig.h"
 
 #if ENABLED(PCA9632)
 

Marlin/src/feature/mbl/mesh_bed_leveling.cpp

@@ -20,10 +20,12 @@
  *
  */
 
-#include "mesh_bed_leveling.h"
+#include "../../inc/MarlinConfig.h"
 
 #if ENABLED(MESH_BED_LEVELING)
 
+  #include "mesh_bed_leveling.h"
+
   mesh_bed_leveling mbl;
 
   uint8_t mesh_bed_leveling::status;

Marlin/src/feature/mbl/mesh_bed_leveling.h

@@ -20,103 +20,104 @@
  *
  */
 
-#include "Marlin.h"
-
-#if ENABLED(MESH_BED_LEVELING)
-
-  enum MeshLevelingState {
-    MeshReport,
-    MeshStart,
-    MeshNext,
-    MeshSet,
-    MeshSetZOffset,
-    MeshReset
-  };
-
-  enum MBLStatus {
-    MBL_STATUS_NONE = 0,
-    MBL_STATUS_HAS_MESH_BIT = 0,
-    MBL_STATUS_ACTIVE_BIT = 1
-  };
-
-  #define MESH_X_DIST ((MESH_MAX_X - (MESH_MIN_X)) / (GRID_MAX_POINTS_X - 1))
-  #define MESH_Y_DIST ((MESH_MAX_Y - (MESH_MIN_Y)) / (GRID_MAX_POINTS_Y - 1))
-
-  class mesh_bed_leveling {
-  public:
-    static uint8_t status; // Has Mesh and Is Active bits
-    static float z_offset,
-                 z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y],
-                 index_to_xpos[GRID_MAX_POINTS_X],
-                 index_to_ypos[GRID_MAX_POINTS_Y];
-
-    mesh_bed_leveling();
-
-    static void reset();
-
-    static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; }
-
-    static bool active()                       { return TEST(status, MBL_STATUS_ACTIVE_BIT); }
-    static void set_active(const bool onOff)   { onOff ? SBI(status, MBL_STATUS_ACTIVE_BIT) : CBI(status, MBL_STATUS_ACTIVE_BIT); }
-    static bool has_mesh()                     { return TEST(status, MBL_STATUS_HAS_MESH_BIT); }
-    static void set_has_mesh(const bool onOff) { onOff ? SBI(status, MBL_STATUS_HAS_MESH_BIT) : CBI(status, MBL_STATUS_HAS_MESH_BIT); }
-
-    static inline void zigzag(const int8_t index, int8_t &px, int8_t &py) {
-      px = index % (GRID_MAX_POINTS_X);
-      py = index / (GRID_MAX_POINTS_X);
-      if (py & 1) px = (GRID_MAX_POINTS_X - 1) - px; // Zig zag
-    }
-
-    static void set_zigzag_z(const int8_t index, const float &z) {
-      int8_t px, py;
-      zigzag(index, px, py);
-      set_z(px, py, z);
-    }
-
-    static int8_t cell_index_x(const float &x) {
-      int8_t cx = (x - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST));
-      return constrain(cx, 0, (GRID_MAX_POINTS_X) - 2);
-    }
-
-    static int8_t cell_index_y(const float &y) {
-      int8_t cy = (y - (MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST));
-      return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 2);
-    }
-
-    static int8_t probe_index_x(const float &x) {
-      int8_t px = (x - (MESH_MIN_X) + 0.5 * (MESH_X_DIST)) * (1.0 / (MESH_X_DIST));
-      return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1;
-    }
-
-    static int8_t probe_index_y(const float &y) {
-      int8_t py = (y - (MESH_MIN_Y) + 0.5 * (MESH_Y_DIST)) * (1.0 / (MESH_Y_DIST));
-      return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1;
-    }
-
-    static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
-      const float delta_z = (z2 - z1) / (a2 - a1),
-                  delta_a = a0 - a1;
-      return z1 + delta_a * delta_z;
-    }
-
-    static float get_z(const float &x0, const float &y0
+#ifndef _MESH_BED_LEVELING_H_
+#define _MESH_BED_LEVELING_H_
+
+#include "../../Marlin.h"
+
+enum MeshLevelingState {
+  MeshReport,
+  MeshStart,
+  MeshNext,
+  MeshSet,
+  MeshSetZOffset,
+  MeshReset
+};
+
+enum MBLStatus {
+  MBL_STATUS_NONE = 0,
+  MBL_STATUS_HAS_MESH_BIT = 0,
+  MBL_STATUS_ACTIVE_BIT = 1
+};
+
+#define MESH_X_DIST ((MESH_MAX_X - (MESH_MIN_X)) / (GRID_MAX_POINTS_X - 1))
+#define MESH_Y_DIST ((MESH_MAX_Y - (MESH_MIN_Y)) / (GRID_MAX_POINTS_Y - 1))
+
+class mesh_bed_leveling {
+public:
+  static uint8_t status; // Has Mesh and Is Active bits
+  static float z_offset,
+               z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y],
+               index_to_xpos[GRID_MAX_POINTS_X],
+               index_to_ypos[GRID_MAX_POINTS_Y];
+
+  mesh_bed_leveling();
+
+  static void reset();
+
+  static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; }
+
+  static bool active()                       { return TEST(status, MBL_STATUS_ACTIVE_BIT); }
+  static void set_active(const bool onOff)   { onOff ? SBI(status, MBL_STATUS_ACTIVE_BIT) : CBI(status, MBL_STATUS_ACTIVE_BIT); }
+  static bool has_mesh()                     { return TEST(status, MBL_STATUS_HAS_MESH_BIT); }
+  static void set_has_mesh(const bool onOff) { onOff ? SBI(status, MBL_STATUS_HAS_MESH_BIT) : CBI(status, MBL_STATUS_HAS_MESH_BIT); }
+
+  static inline void zigzag(const int8_t index, int8_t &px, int8_t &py) {
+    px = index % (GRID_MAX_POINTS_X);
+    py = index / (GRID_MAX_POINTS_X);
+    if (py & 1) px = (GRID_MAX_POINTS_X - 1) - px; // Zig zag
+  }
+
+  static void set_zigzag_z(const int8_t index, const float &z) {
+    int8_t px, py;
+    zigzag(index, px, py);
+    set_z(px, py, z);
+  }
+
+  static int8_t cell_index_x(const float &x) {
+    int8_t cx = (x - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST));
+    return constrain(cx, 0, (GRID_MAX_POINTS_X) - 2);
+  }
+
+  static int8_t cell_index_y(const float &y) {
+    int8_t cy = (y - (MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST));
+    return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 2);
+  }
+
+  static int8_t probe_index_x(const float &x) {
+    int8_t px = (x - (MESH_MIN_X) + 0.5 * (MESH_X_DIST)) * (1.0 / (MESH_X_DIST));
+    return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1;
+  }
+
+  static int8_t probe_index_y(const float &y) {
+    int8_t py = (y - (MESH_MIN_Y) + 0.5 * (MESH_Y_DIST)) * (1.0 / (MESH_Y_DIST));
+    return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1;
+  }
+
+  static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
+    const float delta_z = (z2 - z1) / (a2 - a1),
+                delta_a = a0 - a1;
+    return z1 + delta_a * delta_z;
+  }
+
+  static float get_z(const float &x0, const float &y0
+    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+      , const float &factor
+    #endif
+  ) {
+    const int8_t cx = cell_index_x(x0), cy = cell_index_y(y0);
+    const float z1 = calc_z0(x0, index_to_xpos[cx], z_values[cx][cy], index_to_xpos[cx + 1], z_values[cx + 1][cy]),
+                z2 = calc_z0(x0, index_to_xpos[cx], z_values[cx][cy + 1], index_to_xpos[cx + 1], z_values[cx + 1][cy + 1]),
+                z0 = calc_z0(y0, index_to_ypos[cy], z1, index_to_ypos[cy + 1], z2);
+
+    return z_offset + z0
       #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
-        , const float &factor
+        * factor
       #endif
-    ) {
-      const int8_t cx = cell_index_x(x0), cy = cell_index_y(y0);
-      const float z1 = calc_z0(x0, index_to_xpos[cx], z_values[cx][cy], index_to_xpos[cx + 1], z_values[cx + 1][cy]),
-                  z2 = calc_z0(x0, index_to_xpos[cx], z_values[cx][cy + 1], index_to_xpos[cx + 1], z_values[cx + 1][cy + 1]),
-                  z0 = calc_z0(y0, index_to_ypos[cy], z1, index_to_ypos[cy + 1], z2);
-
-      return z_offset + z0
-        #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
-          * factor
-        #endif
-      ;
-    }
-  };
-
-  extern mesh_bed_leveling mbl;
-
-#endif // MESH_BED_LEVELING
+    ;
+  }
+};
+
+extern mesh_bed_leveling mbl;
+
+#endif // _MESH_BED_LEVELING_H_

Marlin/src/feature/tmc2130.cpp

@@ -0,0 +1,157 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(HAVE_TMC2130)
+
+#include "tmc2130.h"
+#include "../Marlin.h"
+
+#include "../libs/duration_t.h"
+#include "../module/stepper_indirection.h"
+
+#include <TMC2130Stepper.h>
+
+#ifdef AUTOMATIC_CURRENT_CONTROL
+  bool auto_current_control = 0;
+#endif
+
+void automatic_current_control(TMC2130Stepper &st, String axisID) {
+  // Check otpw even if we don't use automatic control. Allows for flag inspection.
+  const bool is_otpw = st.checkOT();
+
+  // Report if a warning was triggered
+  static bool previous_otpw = false;
+  if (is_otpw && !previous_otpw) {
+    char timestamp[10];
+    duration_t elapsed = print_job_timer.duration();
+    const bool has_days = (elapsed.value > 60*60*24L);
+    (void)elapsed.toDigital(timestamp, has_days);
+    SERIAL_ECHO(timestamp);
+    SERIAL_ECHOPGM(": ");
+    SERIAL_ECHO(axisID);
+    SERIAL_ECHOLNPGM(" driver overtemperature warning!");
+  }
+  previous_otpw = is_otpw;
+
+  #if ENABLED(AUTOMATIC_CURRENT_CONTROL) && CURRENT_STEP > 0
+    // Return if user has not enabled current control start with M906 S1.
+    if (!auto_current_control) return;
+
+    /**
+     * Decrease current if is_otpw is true.
+     * Bail out if driver is disabled.
+     * Increase current if OTPW has not been triggered yet.
+     */
+    uint16_t current = st.getCurrent();
+    if (is_otpw) {
+      st.setCurrent(current - CURRENT_STEP, R_SENSE, HOLD_MULTIPLIER);
+      #if ENABLED(REPORT_CURRENT_CHANGE)
+        SERIAL_ECHO(axisID);
+        SERIAL_ECHOPAIR(" current decreased to ", st.getCurrent());
+      #endif
+    }
+
+    else if (!st.isEnabled())
+      return;
+
+    else if (!is_otpw && !st.getOTPW()) {
+      current += CURRENT_STEP;
+      if (current <= AUTO_ADJUST_MAX) {
+        st.setCurrent(current, R_SENSE, HOLD_MULTIPLIER);
+        #if ENABLED(REPORT_CURRENT_CHANGE)
+          SERIAL_ECHO(axisID);
+          SERIAL_ECHOPAIR(" current increased to ", st.getCurrent());
+        #endif
+      }
+    }
+    SERIAL_EOL();
+  #endif
+}
+
+void tmc2130_checkOverTemp(void) {
+  static millis_t next_cOT = 0;
+  if (ELAPSED(millis(), next_cOT)) {
+    next_cOT = millis() + 5000;
+    #if ENABLED(X_IS_TMC2130)
+      automatic_current_control(stepperX, "X");
+    #endif
+    #if ENABLED(Y_IS_TMC2130)
+      automatic_current_control(stepperY, "Y");
+    #endif
+    #if ENABLED(Z_IS_TMC2130)
+      automatic_current_control(stepperZ, "Z");
+    #endif
+    #if ENABLED(X2_IS_TMC2130)
+      automatic_current_control(stepperX2, "X2");
+    #endif
+    #if ENABLED(Y2_IS_TMC2130)
+      automatic_current_control(stepperY2, "Y2");
+    #endif
+    #if ENABLED(Z2_IS_TMC2130)
+      automatic_current_control(stepperZ2, "Z2");
+    #endif
+    #if ENABLED(E0_IS_TMC2130)
+      automatic_current_control(stepperE0, "E0");
+    #endif
+    #if ENABLED(E1_IS_TMC2130)
+      automatic_current_control(stepperE1, "E1");
+    #endif
+    #if ENABLED(E2_IS_TMC2130)
+      automatic_current_control(stepperE2, "E2");
+    #endif
+    #if ENABLED(E3_IS_TMC2130)
+      automatic_current_control(stepperE3, "E3");
+    #endif
+    #if ENABLED(E4_IS_TMC2130)
+      automatic_current_control(stepperE4, "E4");
+    #endif
+    #if ENABLED(E4_IS_TMC2130)
+      automatic_current_control(stepperE4);
+    #endif
+  }
+}
+
+/**
+ * TMC2130 specific sensorless homing using stallGuard2.
+ * stallGuard2 only works when in spreadCycle mode.
+ * spreadCycle and stealthChop are mutually exclusive.
+ */
+#if ENABLED(SENSORLESS_HOMING)
+  void tmc2130_sensorless_homing(TMC2130Stepper &st, bool enable/*=true*/) {
+    #if ENABLED(STEALTHCHOP)
+      if (enable) {
+        st.coolstep_min_speed(1024UL * 1024UL - 1UL);
+        st.stealthChop(0);
+      }
+      else {
+        st.coolstep_min_speed(0);
+        st.stealthChop(1);
+      }
+    #endif
+
+    st.diag1_stall(enable ? 1 : 0);
+  }
+#endif // SENSORLESS_HOMING
+
+#endif // HAVE_TMC2130

Marlin/src/feature/tmc2130.h

@@ -0,0 +1,30 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef _TMC2130_H_
+#define _TMC2130_H_
+
+extern bool auto_current_control;
+
+void tmc2130_checkOverTemp(void);
+
+#endif // _TMC2130_H_

Marlin/src/feature/twibus.cpp

@@ -20,11 +20,12 @@
  *
  */
 
-#include "Marlin.h"
+#include "../inc/MarlinConfig.h"
 
 #if ENABLED(EXPERIMENTAL_I2CBUS)
 
 #include "twibus.h"
+
 #include <Wire.h>
 
 TWIBus::TWIBus() {

Marlin/src/feature/twibus.h

@@ -23,7 +23,7 @@
 #ifndef TWIBUS_H
 #define TWIBUS_H
 
-#include "macros.h"
+#include "../core/macros.h"
 
 #include <Wire.h>
 

Marlin/src/feature/ubl/G26_Mesh_Validation_Tool.cpp

@@ -24,17 +24,18 @@
  * Marlin Firmware -- G26 - Mesh Validation Tool
  */
 
-#include "MarlinConfig.h"
+#include "../../inc/MarlinConfig.h"
 
 #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_G26_MESH_VALIDATION)
 
   #include "ubl.h"
-  #include "Marlin.h"
-  #include "planner.h"
-  #include "stepper.h"
-  #include "temperature.h"
-  #include "ultralcd.h"
-  #include "gcode.h"
+
+  #include "../../Marlin.h"
+  #include "../../module/planner.h"
+  #include "../../module/stepper.h"
+  #include "../../module/temperature.h"
+  #include "../../lcd/ultralcd.h"
+  #include "../../gcode/parser.h"
 
   #define EXTRUSION_MULTIPLIER 1.0
   #define RETRACTION_MULTIPLIER 1.0
@@ -140,8 +141,8 @@
     inline void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); }
     inline void set_current_to_destination() { COPY(current_position, destination); }
   #else
-    extern void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); }
-    extern void set_current_to_destination() { COPY(current_position, destination); }
+    extern void sync_plan_position_e();
+    extern void set_current_to_destination();
   #endif
   #if ENABLED(NEWPANEL)
     void lcd_setstatusPGM(const char* const message, const int8_t level);

Marlin/src/feature/ubl/ubl.cpp

@@ -20,16 +20,18 @@
  *
  */
 
-#include "Marlin.h"
-#include "math.h"
+#include "../../inc/MarlinConfig.h"
 
 #if ENABLED(AUTO_BED_LEVELING_UBL)
 
   #include "ubl.h"
-  #include "hex_print_routines.h"
-  #include "temperature.h"
+  unified_bed_leveling ubl;
 
-  extern Planner planner;
+  #include "../../module/configuration_store.h"
+  #include "../../core/serial.h"
+  #include "../../module/planner.h"
+
+  #include "math.h"
 
   /**
    * These support functions allow the use of large bit arrays of flags that take very
@@ -37,9 +39,9 @@
    * to unsigned long will allow us to go to 32x32 if higher resolution Mesh's are needed
    * in the future.
    */
-  void bit_clear(uint16_t bits[16], uint8_t x, uint8_t y) { CBI(bits[y], x); }
-  void bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { SBI(bits[y], x); }
-  bool is_bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { return TEST(bits[y], x); }
+  void bit_clear(uint16_t bits[16], const uint8_t x, const uint8_t y) { CBI(bits[y], x); }
+  void bit_set(uint16_t bits[16], const uint8_t x, const uint8_t y) { SBI(bits[y], x); }
+  bool is_bit_set(uint16_t bits[16], const uint8_t x, const uint8_t y) { return TEST(bits[y], x); }
 
   uint8_t ubl_cnt = 0;
 

Marlin/src/feature/ubl/ubl.h

@@ -23,387 +23,375 @@
 #ifndef UNIFIED_BED_LEVELING_H
 #define UNIFIED_BED_LEVELING_H
 
-#include "MarlinConfig.h"
-
-#if ENABLED(AUTO_BED_LEVELING_UBL)
-  #include "Marlin.h"
-  #include "planner.h"
-  #include "math.h"
-  #include "vector_3.h"
-  #include "configuration_store.h"
-
-  #define UBL_VERSION "1.01"
-  #define UBL_OK false
-  #define UBL_ERR true
-
-  #define USE_NOZZLE_AS_REFERENCE 0
-  #define USE_PROBE_AS_REFERENCE 1
-
-  typedef struct {
-    int8_t x_index, y_index;
-    float distance; // When populated, the distance from the search location
-  } mesh_index_pair;
-
-  // ubl.cpp
-
-  void bit_clear(uint16_t bits[16], uint8_t x, uint8_t y);
-  void bit_set(uint16_t bits[16], uint8_t x, uint8_t y);
-  bool is_bit_set(uint16_t bits[16], uint8_t x, uint8_t y);
-
-  // ubl_motion.cpp
-
-  void debug_current_and_destination(const char * const title);
-
-  // ubl_G29.cpp
-
-  enum MeshPointType { INVALID, REAL, SET_IN_BITMAP };
-
-  // External references
-
-  char *ftostr43sign(const float&, char);
-  bool ubl_lcd_clicked();
-  void home_all_axes();
-
-  extern uint8_t ubl_cnt;
-
-  ///////////////////////////////////////////////////////////////////////////////////////////////////////
-
-  #if ENABLED(ULTRA_LCD)
-    extern char lcd_status_message[];
-    void lcd_quick_feedback();
-  #endif
-
-  #define MESH_X_DIST (float(UBL_MESH_MAX_X - (UBL_MESH_MIN_X)) / float(GRID_MAX_POINTS_X - 1))
-  #define MESH_Y_DIST (float(UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y)) / float(GRID_MAX_POINTS_Y - 1))
-
-  typedef struct {
-    bool active = false;
-    float z_offset = 0.0;
-    int8_t storage_slot = -1;
-  } ubl_state;
-
-  class unified_bed_leveling {
-    private:
-
-      static float last_specified_z;
-
-      static int    g29_verbose_level,
-                    g29_phase_value,
-                    g29_repetition_cnt,
-                    g29_storage_slot,
-                    g29_map_type,
-                    g29_grid_size;
-      static bool   g29_c_flag, g29_x_flag, g29_y_flag;
-      static float  g29_x_pos, g29_y_pos,
-                    g29_card_thickness,
-                    g29_constant;
-
-      #if ENABLED(UBL_G26_MESH_VALIDATION)
-        static float   g26_extrusion_multiplier,
-                       g26_retraction_multiplier,
-                       g26_nozzle,
-                       g26_filament_diameter,
-                       g26_prime_length,
-                       g26_x_pos, g26_y_pos,
-                       g26_ooze_amount,
-                       g26_layer_height;
-        static int16_t g26_bed_temp,
-                       g26_hotend_temp,
-                       g26_repeats;
-        static int8_t  g26_prime_flag;
-        static bool    g26_continue_with_closest, g26_keep_heaters_on;
-      #endif
-
-      static float measure_point_with_encoder();
-      static float measure_business_card_thickness(float);
-      static bool g29_parameter_parsing();
-      static void find_mean_mesh_height();
-      static void shift_mesh_height();
-      static void probe_entire_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map, const bool stow_probe, bool do_furthest);
-      static void manually_probe_remaining_mesh(const float&, const float&, const float&, const float&, const bool);
-      static void tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3);
-      static void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map);
-      static void g29_what_command();
-      static void g29_eeprom_dump();
-      static void g29_compare_current_mesh_to_stored_mesh();
-      static void fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map);
-      static bool smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir);
-      static void smart_fill_mesh();
-
-      #if ENABLED(UBL_G26_MESH_VALIDATION)
-        static bool exit_from_g26();
-        static bool parse_G26_parameters();
-        static void G26_line_to_destination(const float &feed_rate);
-        static mesh_index_pair find_closest_circle_to_print(const float&, const float&);
-        static bool look_for_lines_to_connect();
-        static bool turn_on_heaters();
-        static bool prime_nozzle();
-        static void retract_filament(const float where[XYZE]);
-        static void recover_filament(const float where[XYZE]);
-        static void print_line_from_here_to_there(const float&, const float&, const float&, const float&, const float&, const float&);
-        static void move_to(const float&, const float&, const float&, const float&);
-        inline static void move_to(const float where[XYZE], const float &de) { move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], de); }
-      #endif
-
-    public:
-
-      static void echo_name();
-      static void report_state();
-      static void save_ubl_active_state_and_disable();
-      static void restore_ubl_active_state_and_leave();
-      static void display_map(const int);
-      static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, uint16_t[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
-      static void smart_fill_wlsf(const float &) _O2; // O2 gives smaller code than Os on A2560
-
-      #if ENABLED(UBL_G26_MESH_VALIDATION)
-        static void G26();
-      #endif
-
-      static ubl_state state;
-
-      static float z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
-
-      // 15 is the maximum nubmer of grid points supported + 1 safety margin for now,
-      // until determinism prevails
-      constexpr static float _mesh_index_to_xpos[16] PROGMEM = {
-                                UBL_MESH_MIN_X +  0 * (MESH_X_DIST), UBL_MESH_MIN_X +  1 * (MESH_X_DIST),
-                                UBL_MESH_MIN_X +  2 * (MESH_X_DIST), UBL_MESH_MIN_X +  3 * (MESH_X_DIST),
-                                UBL_MESH_MIN_X +  4 * (MESH_X_DIST), UBL_MESH_MIN_X +  5 * (MESH_X_DIST),
-                                UBL_MESH_MIN_X +  6 * (MESH_X_DIST), UBL_MESH_MIN_X +  7 * (MESH_X_DIST),
-                                UBL_MESH_MIN_X +  8 * (MESH_X_DIST), UBL_MESH_MIN_X +  9 * (MESH_X_DIST),
-                                UBL_MESH_MIN_X + 10 * (MESH_X_DIST), UBL_MESH_MIN_X + 11 * (MESH_X_DIST),
-                                UBL_MESH_MIN_X + 12 * (MESH_X_DIST), UBL_MESH_MIN_X + 13 * (MESH_X_DIST),
-                                UBL_MESH_MIN_X + 14 * (MESH_X_DIST), UBL_MESH_MIN_X + 15 * (MESH_X_DIST)
-                              };
-
-      constexpr static float _mesh_index_to_ypos[16] PROGMEM = {
-                                UBL_MESH_MIN_Y +  0 * (MESH_Y_DIST), UBL_MESH_MIN_Y +  1 * (MESH_Y_DIST),
-                                UBL_MESH_MIN_Y +  2 * (MESH_Y_DIST), UBL_MESH_MIN_Y +  3 * (MESH_Y_DIST),
-                                UBL_MESH_MIN_Y +  4 * (MESH_Y_DIST), UBL_MESH_MIN_Y +  5 * (MESH_Y_DIST),
-                                UBL_MESH_MIN_Y +  6 * (MESH_Y_DIST), UBL_MESH_MIN_Y +  7 * (MESH_Y_DIST),
-                                UBL_MESH_MIN_Y +  8 * (MESH_Y_DIST), UBL_MESH_MIN_Y +  9 * (MESH_Y_DIST),
-                                UBL_MESH_MIN_Y + 10 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 11 * (MESH_Y_DIST),
-                                UBL_MESH_MIN_Y + 12 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 13 * (MESH_Y_DIST),
-                                UBL_MESH_MIN_Y + 14 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 15 * (MESH_Y_DIST)
-                              };
-
-      static bool g26_debug_flag, has_control_of_lcd_panel;
-
-      static volatile int encoder_diff; // Volatile because it's changed at interrupt time.
-
-      unified_bed_leveling();
-
-      FORCE_INLINE static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; }
-
-      static int8_t get_cell_index_x(const float &x) {
-        const int8_t cx = (x - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST));
-        return constrain(cx, 0, (GRID_MAX_POINTS_X) - 1);   // -1 is appropriate if we want all movement to the X_MAX
-      }                                                     // position. But with this defined this way, it is possible
-                                                            // to extrapolate off of this point even further out. Probably
-                                                            // that is OK because something else should be keeping that from
-                                                            // happening and should not be worried about at this level.
-      static int8_t get_cell_index_y(const float &y) {
-        const int8_t cy = (y - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST));
-        return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 1);   // -1 is appropriate if we want all movement to the Y_MAX
-      }                                                     // position. But with this defined this way, it is possible
-                                                            // to extrapolate off of this point even further out. Probably
-                                                            // that is OK because something else should be keeping that from
-                                                            // happening and should not be worried about at this level.
-
-      static int8_t find_closest_x_index(const float &x) {
-        const int8_t px = (x - (UBL_MESH_MIN_X) + (MESH_X_DIST) * 0.5) * (1.0 / (MESH_X_DIST));
-        return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1;
+#include "../../Marlin.h"
+#include "../../core/serial.h"
+#include "../../module/planner.h"
+
+#define UBL_VERSION "1.01"
+#define UBL_OK false
+#define UBL_ERR true
+
+#define USE_NOZZLE_AS_REFERENCE 0
+#define USE_PROBE_AS_REFERENCE 1
+
+typedef struct {
+  int8_t x_index, y_index;
+  float distance; // When populated, the distance from the search location
+} mesh_index_pair;
+
+// ubl.cpp
+
+void bit_clear(uint16_t bits[16], const uint8_t x, const uint8_t y);
+void bit_set(uint16_t bits[16], const uint8_t x, const uint8_t y);
+bool is_bit_set(uint16_t bits[16], const uint8_t x, const uint8_t y);
+
+// ubl_motion.cpp
+
+void debug_current_and_destination(const char * const title);
+
+// ubl_G29.cpp
+
+enum MeshPointType { INVALID, REAL, SET_IN_BITMAP };
+
+// External references
+
+char *ftostr43sign(const float&, char);
+bool ubl_lcd_clicked();
+void home_all_axes();
+
+extern uint8_t ubl_cnt;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#if ENABLED(ULTRA_LCD)
+  extern char lcd_status_message[];
+  void lcd_quick_feedback();
+#endif
+
+#define MESH_X_DIST (float(UBL_MESH_MAX_X - (UBL_MESH_MIN_X)) / float(GRID_MAX_POINTS_X - 1))
+#define MESH_Y_DIST (float(UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y)) / float(GRID_MAX_POINTS_Y - 1))
+
+typedef struct {
+  bool active = false;
+  float z_offset = 0.0;
+  int8_t storage_slot = -1;
+} ubl_state;
+
+class unified_bed_leveling {
+  private:
+
+    static float last_specified_z;
+
+    static int    g29_verbose_level,
+                  g29_phase_value,
+                  g29_repetition_cnt,
+                  g29_storage_slot,
+                  g29_map_type,
+                  g29_grid_size;
+    static bool   g29_c_flag, g29_x_flag, g29_y_flag;
+    static float  g29_x_pos, g29_y_pos,
+                  g29_card_thickness,
+                  g29_constant;
+
+    #if ENABLED(UBL_G26_MESH_VALIDATION)
+      static float   g26_extrusion_multiplier,
+                     g26_retraction_multiplier,
+                     g26_nozzle,
+                     g26_filament_diameter,
+                     g26_prime_length,
+                     g26_x_pos, g26_y_pos,
+                     g26_ooze_amount,
+                     g26_layer_height;
+      static int16_t g26_bed_temp,
+                     g26_hotend_temp,
+                     g26_repeats;
+      static int8_t  g26_prime_flag;
+      static bool    g26_continue_with_closest, g26_keep_heaters_on;
+    #endif
+
+    static float measure_point_with_encoder();
+    static float measure_business_card_thickness(float);
+    static bool g29_parameter_parsing();
+    static void find_mean_mesh_height();
+    static void shift_mesh_height();
+    static void probe_entire_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map, const bool stow_probe, bool do_furthest);
+    static void manually_probe_remaining_mesh(const float&, const float&, const float&, const float&, const bool);
+    static void tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3);
+    static void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map);
+    static void g29_what_command();
+    static void g29_eeprom_dump();
+    static void g29_compare_current_mesh_to_stored_mesh();
+    static void fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map);
+    static bool smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir);
+    static void smart_fill_mesh();
+
+    #if ENABLED(UBL_G26_MESH_VALIDATION)
+      static bool exit_from_g26();
+      static bool parse_G26_parameters();
+      static void G26_line_to_destination(const float &feed_rate);
+      static mesh_index_pair find_closest_circle_to_print(const float&, const float&);
+      static bool look_for_lines_to_connect();
+      static bool turn_on_heaters();
+      static bool prime_nozzle();
+      static void retract_filament(const float where[XYZE]);
+      static void recover_filament(const float where[XYZE]);
+      static void print_line_from_here_to_there(const float&, const float&, const float&, const float&, const float&, const float&);
+      static void move_to(const float&, const float&, const float&, const float&);
+      inline static void move_to(const float where[XYZE], const float &de) { move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], de); }
+    #endif
+
+  public:
+
+    static void echo_name();
+    static void report_state();
+    static void save_ubl_active_state_and_disable();
+    static void restore_ubl_active_state_and_leave();
+    static void display_map(const int);
+    static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, uint16_t[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
+    static void smart_fill_wlsf(const float &) _O2; // O2 gives smaller code than Os on A2560
+
+    #if ENABLED(UBL_G26_MESH_VALIDATION)
+      static void G26();
+    #endif
+
+    static ubl_state state;
+
+    static float z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
+
+    // 15 is the maximum nubmer of grid points supported + 1 safety margin for now,
+    // until determinism prevails
+    constexpr static float _mesh_index_to_xpos[16] PROGMEM = {
+                              UBL_MESH_MIN_X +  0 * (MESH_X_DIST), UBL_MESH_MIN_X +  1 * (MESH_X_DIST),
+                              UBL_MESH_MIN_X +  2 * (MESH_X_DIST), UBL_MESH_MIN_X +  3 * (MESH_X_DIST),
+                              UBL_MESH_MIN_X +  4 * (MESH_X_DIST), UBL_MESH_MIN_X +  5 * (MESH_X_DIST),
+                              UBL_MESH_MIN_X +  6 * (MESH_X_DIST), UBL_MESH_MIN_X +  7 * (MESH_X_DIST),
+                              UBL_MESH_MIN_X +  8 * (MESH_X_DIST), UBL_MESH_MIN_X +  9 * (MESH_X_DIST),
+                              UBL_MESH_MIN_X + 10 * (MESH_X_DIST), UBL_MESH_MIN_X + 11 * (MESH_X_DIST),
+                              UBL_MESH_MIN_X + 12 * (MESH_X_DIST), UBL_MESH_MIN_X + 13 * (MESH_X_DIST),
+                              UBL_MESH_MIN_X + 14 * (MESH_X_DIST), UBL_MESH_MIN_X + 15 * (MESH_X_DIST)
+                            };
+
+    constexpr static float _mesh_index_to_ypos[16] PROGMEM = {
+                              UBL_MESH_MIN_Y +  0 * (MESH_Y_DIST), UBL_MESH_MIN_Y +  1 * (MESH_Y_DIST),
+                              UBL_MESH_MIN_Y +  2 * (MESH_Y_DIST), UBL_MESH_MIN_Y +  3 * (MESH_Y_DIST),
+                              UBL_MESH_MIN_Y +  4 * (MESH_Y_DIST), UBL_MESH_MIN_Y +  5 * (MESH_Y_DIST),
+                              UBL_MESH_MIN_Y +  6 * (MESH_Y_DIST), UBL_MESH_MIN_Y +  7 * (MESH_Y_DIST),
+                              UBL_MESH_MIN_Y +  8 * (MESH_Y_DIST), UBL_MESH_MIN_Y +  9 * (MESH_Y_DIST),
+                              UBL_MESH_MIN_Y + 10 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 11 * (MESH_Y_DIST),
+                              UBL_MESH_MIN_Y + 12 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 13 * (MESH_Y_DIST),
+                              UBL_MESH_MIN_Y + 14 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 15 * (MESH_Y_DIST)
+                            };
+
+    static bool g26_debug_flag, has_control_of_lcd_panel;
+
+    static volatile int encoder_diff; // Volatile because it's changed at interrupt time.
+
+    unified_bed_leveling();
+
+    FORCE_INLINE static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; }
+
+    static int8_t get_cell_index_x(const float &x) {
+      const int8_t cx = (x - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST));
+      return constrain(cx, 0, (GRID_MAX_POINTS_X) - 1);   // -1 is appropriate if we want all movement to the X_MAX
+    }                                                     // position. But with this defined this way, it is possible
+                                                          // to extrapolate off of this point even further out. Probably
+                                                          // that is OK because something else should be keeping that from
+                                                          // happening and should not be worried about at this level.
+    static int8_t get_cell_index_y(const float &y) {
+      const int8_t cy = (y - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST));
+      return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 1);   // -1 is appropriate if we want all movement to the Y_MAX
+    }                                                     // position. But with this defined this way, it is possible
+                                                          // to extrapolate off of this point even further out. Probably
+                                                          // that is OK because something else should be keeping that from
+                                                          // happening and should not be worried about at this level.
+
+    static int8_t find_closest_x_index(const float &x) {
+      const int8_t px = (x - (UBL_MESH_MIN_X) + (MESH_X_DIST) * 0.5) * (1.0 / (MESH_X_DIST));
+      return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1;
+    }
+
+    static int8_t find_closest_y_index(const float &y) {
+      const int8_t py = (y - (UBL_MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * (1.0 / (MESH_Y_DIST));
+      return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1;
+    }
+
+    /**
+     *                           z2   --|
+     *                 z0        |      |
+     *                  |        |      + (z2-z1)
+     *   z1             |        |      |
+     * ---+-------------+--------+--  --|
+     *   a1            a0        a2
+     *    |<---delta_a---------->|
+     *
+     *  calc_z0 is the basis for all the Mesh Based correction. It is used to
+     *  find the expected Z Height at a position between two known Z-Height locations.
+     *
+     *  It is fairly expensive with its 4 floating point additions and 2 floating point
+     *  multiplications.
+     */
+    FORCE_INLINE static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
+      return z1 + (z2 - z1) * (a0 - a1) / (a2 - a1);
+    }
+
+    /**
+     * z_correction_for_x_on_horizontal_mesh_line is an optimization for
+     * the case where the printer is making a vertical line that only crosses horizontal mesh lines.
+     */
+    inline static float z_correction_for_x_on_horizontal_mesh_line(const float &lx0, const int x1_i, const int yi) {
+      if (!WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 2) || !WITHIN(yi, 0, GRID_MAX_POINTS_Y - 1)) {
+        serialprintPGM( !WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) ? PSTR("x1l_i") : PSTR("yi") );
+        SERIAL_ECHOPAIR(" out of bounds in z_correction_for_x_on_horizontal_mesh_line(lx0=", lx0);
+        SERIAL_ECHOPAIR(",x1_i=", x1_i);
+        SERIAL_ECHOPAIR(",yi=", yi);
+        SERIAL_CHAR(')');
+        SERIAL_EOL();
+        return NAN;
       }
 
-      static int8_t find_closest_y_index(const float &y) {
-        const int8_t py = (y - (UBL_MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * (1.0 / (MESH_Y_DIST));
-        return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1;
+      const float xratio = (RAW_X_POSITION(lx0) - mesh_index_to_xpos(x1_i)) * (1.0 / (MESH_X_DIST)),
+                  z1 = z_values[x1_i][yi];
+
+      return z1 + xratio * (z_values[x1_i + 1][yi] - z1);
+    }
+
+    //
+    // See comments above for z_correction_for_x_on_horizontal_mesh_line
+    //
+    inline static float z_correction_for_y_on_vertical_mesh_line(const float &ly0, const int xi, const int y1_i) {
+      if (!WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(y1_i, 0, GRID_MAX_POINTS_Y - 2)) {
+        serialprintPGM( !WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) ? PSTR("xi") : PSTR("yl_i") );
+        SERIAL_ECHOPAIR(" out of bounds in z_correction_for_y_on_vertical_mesh_line(ly0=", ly0);
+        SERIAL_ECHOPAIR(", xi=", xi);
+        SERIAL_ECHOPAIR(", y1_i=", y1_i);
+        SERIAL_CHAR(')');
+        SERIAL_EOL();
+        return NAN;
       }
 
-      /**
-       *                           z2   --|
-       *                 z0        |      |
-       *                  |        |      + (z2-z1)
-       *   z1             |        |      |
-       * ---+-------------+--------+--  --|
-       *   a1            a0        a2
-       *    |<---delta_a---------->|
-       *
-       *  calc_z0 is the basis for all the Mesh Based correction. It is used to
-       *  find the expected Z Height at a position between two known Z-Height locations.
-       *
-       *  It is fairly expensive with its 4 floating point additions and 2 floating point
-       *  multiplications.
-       */
-      FORCE_INLINE static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
-        return z1 + (z2 - z1) * (a0 - a1) / (a2 - a1);
-      }
+      const float yratio = (RAW_Y_POSITION(ly0) - mesh_index_to_ypos(y1_i)) * (1.0 / (MESH_Y_DIST)),
+                  z1 = z_values[xi][y1_i];
 
-      /**
-       * z_correction_for_x_on_horizontal_mesh_line is an optimization for
-       * the case where the printer is making a vertical line that only crosses horizontal mesh lines.
-       */
-      inline static float z_correction_for_x_on_horizontal_mesh_line(const float &lx0, const int x1_i, const int yi) {
-        if (!WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 2) || !WITHIN(yi, 0, GRID_MAX_POINTS_Y - 1)) {
-          serialprintPGM( !WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) ? PSTR("x1l_i") : PSTR("yi") );
-          SERIAL_ECHOPAIR(" out of bounds in z_correction_for_x_on_horizontal_mesh_line(lx0=", lx0);
-          SERIAL_ECHOPAIR(",x1_i=", x1_i);
-          SERIAL_ECHOPAIR(",yi=", yi);
-          SERIAL_CHAR(')');
-          SERIAL_EOL();
-          return NAN;
-        }
+      return z1 + yratio * (z_values[xi][y1_i + 1] - z1);
+    }
 
-        const float xratio = (RAW_X_POSITION(lx0) - mesh_index_to_xpos(x1_i)) * (1.0 / (MESH_X_DIST)),
-                    z1 = z_values[x1_i][yi];
+    /**
+     * This is the generic Z-Correction. It works anywhere within a Mesh Cell. It first
+     * does a linear interpolation along both of the bounding X-Mesh-Lines to find the
+     * Z-Height at both ends. Then it does a linear interpolation of these heights based
+     * on the Y position within the cell.
+     */
+    static float get_z_correction(const float &lx0, const float &ly0) {
+      const int8_t cx = get_cell_index_x(RAW_X_POSITION(lx0)),
+                   cy = get_cell_index_y(RAW_Y_POSITION(ly0));
 
-        return z1 + xratio * (z_values[x1_i + 1][yi] - z1);
-      }
+      if (!WITHIN(cx, 0, GRID_MAX_POINTS_X - 2) || !WITHIN(cy, 0, GRID_MAX_POINTS_Y - 2)) {
 
-      //
-      // See comments above for z_correction_for_x_on_horizontal_mesh_line
-      //
-      inline static float z_correction_for_y_on_vertical_mesh_line(const float &ly0, const int xi, const int y1_i) {
-        if (!WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(y1_i, 0, GRID_MAX_POINTS_Y - 2)) {
-          serialprintPGM( !WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) ? PSTR("xi") : PSTR("yl_i") );
-          SERIAL_ECHOPAIR(" out of bounds in z_correction_for_y_on_vertical_mesh_line(ly0=", ly0);
-          SERIAL_ECHOPAIR(", xi=", xi);
-          SERIAL_ECHOPAIR(", y1_i=", y1_i);
-          SERIAL_CHAR(')');
-          SERIAL_EOL();
-          return NAN;
-        }
+        SERIAL_ECHOPAIR("? in get_z_correction(lx0=", lx0);
+        SERIAL_ECHOPAIR(", ly0=", ly0);
+        SERIAL_CHAR(')');
+        SERIAL_EOL();
 
-        const float yratio = (RAW_Y_POSITION(ly0) - mesh_index_to_ypos(y1_i)) * (1.0 / (MESH_Y_DIST)),
-                    z1 = z_values[xi][y1_i];
-
-        return z1 + yratio * (z_values[xi][y1_i + 1] - z1);
+        #if ENABLED(ULTRA_LCD)
+          strcpy(lcd_status_message, "get_z_correction() indexes out of range.");
+          lcd_quick_feedback();
+        #endif
+        return NAN; // this used to return state.z_offset
       }
 
-      /**
-       * This is the generic Z-Correction. It works anywhere within a Mesh Cell. It first
-       * does a linear interpolation along both of the bounding X-Mesh-Lines to find the
-       * Z-Height at both ends. Then it does a linear interpolation of these heights based
-       * on the Y position within the cell.
-       */
-      static float get_z_correction(const float &lx0, const float &ly0) {
-        const int8_t cx = get_cell_index_x(RAW_X_POSITION(lx0)),
-                     cy = get_cell_index_y(RAW_Y_POSITION(ly0));
-
-        if (!WITHIN(cx, 0, GRID_MAX_POINTS_X - 2) || !WITHIN(cy, 0, GRID_MAX_POINTS_Y - 2)) {
-
-          SERIAL_ECHOPAIR("? in get_z_correction(lx0=", lx0);
-          SERIAL_ECHOPAIR(", ly0=", ly0);
-          SERIAL_CHAR(')');
-          SERIAL_EOL();
-
-          #if ENABLED(ULTRA_LCD)
-            strcpy(lcd_status_message, "get_z_correction() indexes out of range.");
-            lcd_quick_feedback();
-          #endif
-          return NAN; // this used to return state.z_offset
+      const float z1 = calc_z0(RAW_X_POSITION(lx0),
+                               mesh_index_to_xpos(cx), z_values[cx][cy],
+                               mesh_index_to_xpos(cx + 1), z_values[cx + 1][cy]);
+
+      const float z2 = calc_z0(RAW_X_POSITION(lx0),
+                               mesh_index_to_xpos(cx), z_values[cx][cy + 1],
+                               mesh_index_to_xpos(cx + 1), z_values[cx + 1][cy + 1]);
+
+      float z0 = calc_z0(RAW_Y_POSITION(ly0),
+                         mesh_index_to_ypos(cy), z1,
+                         mesh_index_to_ypos(cy + 1), z2);
+
+      #if ENABLED(DEBUG_LEVELING_FEATURE)
+        if (DEBUGGING(MESH_ADJUST)) {
+          SERIAL_ECHOPAIR(" raw get_z_correction(", lx0);
+          SERIAL_CHAR(',');
+          SERIAL_ECHO(ly0);
+          SERIAL_ECHOPGM(") = ");
+          SERIAL_ECHO_F(z0, 6);
         }
+      #endif
 
-        const float z1 = calc_z0(RAW_X_POSITION(lx0),
-                                 mesh_index_to_xpos(cx), z_values[cx][cy],
-                                 mesh_index_to_xpos(cx + 1), z_values[cx + 1][cy]);
-
-        const float z2 = calc_z0(RAW_X_POSITION(lx0),
-                                 mesh_index_to_xpos(cx), z_values[cx][cy + 1],
-                                 mesh_index_to_xpos(cx + 1), z_values[cx + 1][cy + 1]);
+      #if ENABLED(DEBUG_LEVELING_FEATURE)
+        if (DEBUGGING(MESH_ADJUST)) {
+          SERIAL_ECHOPGM(" >>>---> ");
+          SERIAL_ECHO_F(z0, 6);
+          SERIAL_EOL();
+        }
+      #endif
 
-        float z0 = calc_z0(RAW_Y_POSITION(ly0),
-                           mesh_index_to_ypos(cy), z1,
-                           mesh_index_to_ypos(cy + 1), z2);
+      if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN
+        z0 = 0.0;      // in ubl.z_values[][] and propagate through the
+                       // calculations. If our correction is NAN, we throw it out
+                       // because part of the Mesh is undefined and we don't have the
+                       // information we need to complete the height correction.
 
         #if ENABLED(DEBUG_LEVELING_FEATURE)
           if (DEBUGGING(MESH_ADJUST)) {
-            SERIAL_ECHOPAIR(" raw get_z_correction(", lx0);
+            SERIAL_ECHOPAIR("??? Yikes!  NAN in get_z_correction(", lx0);
             SERIAL_CHAR(',');
             SERIAL_ECHO(ly0);
-            SERIAL_ECHOPGM(") = ");
-            SERIAL_ECHO_F(z0, 6);
-          }
-        #endif
-
-        #if ENABLED(DEBUG_LEVELING_FEATURE)
-          if (DEBUGGING(MESH_ADJUST)) {
-            SERIAL_ECHOPGM(" >>>---> ");
-            SERIAL_ECHO_F(z0, 6);
+            SERIAL_CHAR(')');
             SERIAL_EOL();
           }
         #endif
-
-        if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN
-          z0 = 0.0;      // in ubl.z_values[][] and propagate through the
-                         // calculations. If our correction is NAN, we throw it out
-                         // because part of the Mesh is undefined and we don't have the
-                         // information we need to complete the height correction.
-
-          #if ENABLED(DEBUG_LEVELING_FEATURE)
-            if (DEBUGGING(MESH_ADJUST)) {
-              SERIAL_ECHOPAIR("??? Yikes!  NAN in get_z_correction(", lx0);
-              SERIAL_CHAR(',');
-              SERIAL_ECHO(ly0);
-              SERIAL_CHAR(')');
-              SERIAL_EOL();
-            }
-          #endif
-        }
-        return z0; // there used to be a +state.z_offset on this line
       }
-
-      /**
-       * This function sets the Z leveling fade factor based on the given Z height,
-       * only re-calculating when necessary.
-       *
-       *  Returns 1.0 if planner.z_fade_height is 0.0.
-       *  Returns 0.0 if Z is past the specified 'Fade Height'.
-       */
-      #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
-        static inline float fade_scaling_factor_for_z(const float &lz) {
-          if (planner.z_fade_height == 0.0) return 1.0;
-          static float fade_scaling_factor = 1.0;
-          const float rz = RAW_Z_POSITION(lz);
-          if (last_specified_z != rz) {
-            last_specified_z = rz;
-            fade_scaling_factor =
-              rz < planner.z_fade_height
-                ? 1.0 - (rz * planner.inverse_z_fade_height)
-                : 0.0;
-          }
-          return fade_scaling_factor;
+      return z0; // there used to be a +state.z_offset on this line
+    }
+
+    /**
+     * This function sets the Z leveling fade factor based on the given Z height,
+     * only re-calculating when necessary.
+     *
+     *  Returns 1.0 if planner.z_fade_height is 0.0.
+     *  Returns 0.0 if Z is past the specified 'Fade Height'.
+     */
+    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+      static inline float fade_scaling_factor_for_z(const float &lz) {
+        if (planner.z_fade_height == 0.0) return 1.0;
+        static float fade_scaling_factor = 1.0;
+        const float rz = RAW_Z_POSITION(lz);
+        if (last_specified_z != rz) {
+          last_specified_z = rz;
+          fade_scaling_factor =
+            rz < planner.z_fade_height
+              ? 1.0 - (rz * planner.inverse_z_fade_height)
+              : 0.0;
         }
-      #else
-        FORCE_INLINE static float fade_scaling_factor_for_z(const float &lz) { return 1.0; }
-      #endif
-
-      FORCE_INLINE static float mesh_index_to_xpos(const uint8_t i) {
-        return i < GRID_MAX_POINTS_X ? pgm_read_float(&_mesh_index_to_xpos[i]) : UBL_MESH_MIN_X + i * (MESH_X_DIST);
-      }
-
-      FORCE_INLINE static float mesh_index_to_ypos(const uint8_t i) {
-        return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : UBL_MESH_MIN_Y + i * (MESH_Y_DIST);
+        return fade_scaling_factor;
       }
+    #else
+      FORCE_INLINE static float fade_scaling_factor_for_z(const float &lz) { return 1.0; }
+    #endif
 
-      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);
+    FORCE_INLINE static float mesh_index_to_xpos(const uint8_t i) {
+      return i < GRID_MAX_POINTS_X ? pgm_read_float(&_mesh_index_to_xpos[i]) : UBL_MESH_MIN_X + i * (MESH_X_DIST);
+    }
 
-  }; // class unified_bed_leveling
+    FORCE_INLINE static float mesh_index_to_ypos(const uint8_t i) {
+      return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : UBL_MESH_MIN_Y + i * (MESH_Y_DIST);
+    }
 
-  extern unified_bed_leveling ubl;
+    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);
 
-  #if ENABLED(UBL_G26_MESH_VALIDATION)
-    FORCE_INLINE void gcode_G26() { ubl.G26(); }
-  #endif
+}; // class unified_bed_leveling
 
-  FORCE_INLINE void gcode_G29() { ubl.G29(); }
+extern unified_bed_leveling ubl;
 
-#endif // AUTO_BED_LEVELING_UBL
 #endif // UNIFIED_BED_LEVELING_H

Marlin/src/feature/ubl/ubl_G29.cpp

@@ -20,21 +20,22 @@
  *
  */
 
-#include "MarlinConfig.h"
+#include "../../inc/MarlinConfig.h"
 
 #if ENABLED(AUTO_BED_LEVELING_UBL)
 
   #include "ubl.h"
-  #include "Marlin.h"
-  #include "hex_print_routines.h"
-  #include "configuration_store.h"
-  #include "ultralcd.h"
-  #include "stepper.h"
-  #include "planner.h"
-  #include "gcode.h"
+
+  #include "../../Marlin.h"
+  #include "../../libs/hex_print_routines.h"
+  #include "../../module/configuration_store.h"
+  #include "../../lcd/ultralcd.h"
+  #include "../../module/stepper.h"
+  #include "../../module/planner.h"
+  #include "../../gcode/parser.h"
+  #include "../../libs/least_squares_fit.h"
 
   #include <math.h>
-  #include "least_squares_fit.h"
 
   #define UBL_G29_P31
 

Marlin/src/feature/ubl/ubl_motion.cpp

@@ -19,14 +19,16 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  *
  */
-#include "MarlinConfig.h"
+#include "../../inc/MarlinConfig.h"
 
 #if ENABLED(AUTO_BED_LEVELING_UBL)
 
-  #include "Marlin.h"
   #include "ubl.h"
-  #include "planner.h"
-  #include "stepper.h"
+
+  #include "../../Marlin.h"
+  #include "../../module/planner.h"
+  #include "../../module/stepper.h"
+
   #include <math.h>
 
   extern float destination[XYZE];