Merge branch 'dev'

MK/Configuration_Cartesian.h

@@ -15,8 +15,8 @@
  * - Disables axis
  * - Travel limits
  * - Axis relative mode
- * - MBL or ABL
- * - Auto bed levelling
+ * - Mesh Bed Leveling (MBL)
+ * - Auto Bed Leveling (ABL)
  * - Z probe endstop
  * - Safe Z homing
  * - Manual home positions
@@ -28,6 +28,7 @@
  * - Cartesian Correction
  *
  * Basic-settings can be found in Configuration_Basic.h
+ * Temperature-settings can be found in Configuration_Temperature.h
  * Feature-settings can be found in Configuration_Feature.h
  * Pins-settings can be found in "Configuration_Pins.h"
  */
@@ -229,24 +230,7 @@
 
 
 /*****************************************************************************************
- ************************************** MBL or ABL ***************************************
- *****************************************************************************************
- *                                                                                       *
- * Manual Bed Leveling (MBL) or Auto Bed Leveling (ABL) settings                         *
- * Set the rectangle in which to probe in MBL or ABL.                                    *
- *                                                                                       *
- *****************************************************************************************/
-#define LEFT_PROBE_BED_POSITION 20
-#define RIGHT_PROBE_BED_POSITION 180
-#define FRONT_PROBE_BED_POSITION 20
-#define BACK_PROBE_BED_POSITION 180
-
-#define XY_TRAVEL_SPEED 10000     // X and Y axis travel speed between probes, in mm/min
-/*****************************************************************************************/
-
-
-/*****************************************************************************************
- ******************************* Mesh bed leveling ***************************************
+ ******************************* Mesh Bed Leveling ***************************************
  *****************************************************************************************/
 //#define MESH_BED_LEVELING
 
@@ -266,7 +250,7 @@
 
 
 /*****************************************************************************************
- ******************************* Auto bed leveling ***************************************
+ ******************************* Auto Bed Leveling ***************************************
  *****************************************************************************************
  *                                                                                       *
  * There are 2 different ways to specify probing locations                               *
@@ -281,7 +265,7 @@
  *   You specify the XY coordinates of all 3 points.                                     *
  *                                                                                       *
  *                                                                                       *
- * Uncomment AUTO_BED_LEVELING_FEATURE to enable                                         *
+ * Uncomment AUTO BED LEVELING FEATURE to enable                                         *
  *                                                                                       *
  *****************************************************************************************/
 //#define AUTO_BED_LEVELING_FEATURE
@@ -291,14 +275,22 @@
 // Note: this feature generates 10KB extra code size
 #define AUTO_BED_LEVELING_GRID
 
-// yes AUTO_BED_LEVELING_GRID
-#define MIN_PROBE_EDGE 10 // The probe square sides can be no smaller than this
+// START yes AUTO BED LEVELING GRID
+#define LEFT_PROBE_BED_POSITION 20
+#define RIGHT_PROBE_BED_POSITION 180
+#define FRONT_PROBE_BED_POSITION 20
+#define BACK_PROBE_BED_POSITION 180
+
+// The probe square sides can be no smaller than this
+#define MIN_PROBE_EDGE 10
+
 // Set the number of grid points per dimension
 // You probably don't need more than 3 (squared=9)
 #define AUTO_BED_LEVELING_GRID_POINTS 2
-// yes AUTO_BED_LEVELING_GRID
+// END yes AUTO BED LEVELING GRID
+
 
-// no AUTO_BED_LEVELING_GRID
+// START no AUTO BED LEVELING GRID
 // Arbitrary points to probe. A simple cross-product
 // is used to estimate the plane of the bed.
 #define ABL_PROBE_PT_1_X 15
@@ -307,7 +299,7 @@
 #define ABL_PROBE_PT_2_Y 15
 #define ABL_PROBE_PT_3_X 180
 #define ABL_PROBE_PT_3_Y 15
-// no AUTO_BED_LEVELING_GRID
+// END no AUTO BED LEVELING GRID
 
 // Offsets to the probe relative to the extruder tip (Hotend - Probe)
 // X and Y offsets MUST be INTEGERS
@@ -321,10 +313,13 @@
 //    |  P (-)    | T <-- probe (-10,-10)
 //    |           |
 //    O-- FRONT --+
+//  (0,0)
 #define X_PROBE_OFFSET_FROM_EXTRUDER  0     // X offset: -left  [of the nozzle] +right
 #define Y_PROBE_OFFSET_FROM_EXTRUDER  0     // Y offset: -front [of the nozzle] +behind
 #define Z_PROBE_OFFSET_FROM_EXTRUDER -1     // Z offset: -below [of the nozzle] (always negative!)
 
+#define XY_TRAVEL_SPEED 10000               // X and Y axis travel speed between probes, in mm/min
+
 #define Z_RAISE_BEFORE_PROBING       10     //How much the extruder will be raised before travelling to the first probing point.
 #define Z_RAISE_BETWEEN_PROBINGS      5     //How much the extruder will be raised when travelling from between next probing points
 #define Z_RAISE_AFTER_PROBING         5     //How much the extruder will be raised after the last probing point.

MK/Configuration_Core.h

@@ -16,8 +16,8 @@
  * - Disables axis
  * - Travel limits
  * - Axis relative mode
- * - MBL or ABL
- * - Auto bed levelling
+ * - Mesh Bed Leveling (MBL)
+ * - Auto Bed Leveling (ABL)
  * - Z probe endstop
  * - Safe Z homing
  * - Manual home positions
@@ -28,6 +28,7 @@
  * - Hotend offset
  *
  * Basic-settings can be found in Configuration_Basic.h
+ * Temperature-settings can be found in Configuration_Temperature.h
  * Feature-settings can be found in Configuration_Feature.h
  * Pins-settings can be found in "Configuration_Pins.h"
  */
@@ -251,24 +252,7 @@
 
 
 /*****************************************************************************************
- ************************************** MBL or ABL ***************************************
- *****************************************************************************************
- *                                                                                       *
- * Manual Bed Leveling (MBL) or Auto Bed Leveling (ABL) settings                         *
- * Set the rectangle in which to probe in MBL or ABL.                                    *
- *                                                                                       *
- *****************************************************************************************/
-#define LEFT_PROBE_BED_POSITION 20
-#define RIGHT_PROBE_BED_POSITION 180
-#define FRONT_PROBE_BED_POSITION 20
-#define BACK_PROBE_BED_POSITION 180
-
-#define XY_TRAVEL_SPEED 10000     // X and Y axis travel speed between probes, in mm/min
-/*****************************************************************************************/
-
-
-/*****************************************************************************************
- ******************************* Mesh bed leveling ***************************************
+ ******************************* Mesh Bed Leveling ***************************************
  *****************************************************************************************/
 //#define MESH_BED_LEVELING
 
@@ -288,7 +272,7 @@
 
 
 /*****************************************************************************************
- ******************************* Auto bed leveling ***************************************
+ ******************************* Auto Bed Leveling ***************************************
  *****************************************************************************************
  *                                                                                       *
  * There are 2 different ways to specify probing locations                               *
@@ -303,7 +287,7 @@
  *   You specify the XY coordinates of all 3 points.                                     *
  *                                                                                       *
  *                                                                                       *
- * Uncomment AUTO_BED_LEVELING_FEATURE to enable                                         *
+ * Uncomment AUTO BED LEVELING FEATURE to enable                                         *
  *                                                                                       *
  *****************************************************************************************/
 //#define AUTO_BED_LEVELING_FEATURE
@@ -313,14 +297,22 @@
 // Note: this feature generates 10KB extra code size
 #define AUTO_BED_LEVELING_GRID
 
-// yes AUTO_BED_LEVELING_GRID
-#define MIN_PROBE_EDGE 10 // The probe square sides can be no smaller than this
+// START yes AUTO BED LEVELING GRID
+#define LEFT_PROBE_BED_POSITION 20
+#define RIGHT_PROBE_BED_POSITION 180
+#define FRONT_PROBE_BED_POSITION 20
+#define BACK_PROBE_BED_POSITION 180
+
+// The probe square sides can be no smaller than this
+#define MIN_PROBE_EDGE 10
+
 // Set the number of grid points per dimension
 // You probably don't need more than 3 (squared=9)
 #define AUTO_BED_LEVELING_GRID_POINTS 2
-// yes AUTO_BED_LEVELING_GRID
+// END yes AUTO BED LEVELING GRID
+
 
-// no AUTO_BED_LEVELING_GRID
+// START no AUTO BED LEVELING GRID
 // Arbitrary points to probe. A simple cross-product
 // is used to estimate the plane of the bed.
 #define ABL_PROBE_PT_1_X 15
@@ -329,7 +321,7 @@
 #define ABL_PROBE_PT_2_Y 15
 #define ABL_PROBE_PT_3_X 180
 #define ABL_PROBE_PT_3_Y 15
-// no AUTO_BED_LEVELING_GRID
+// END no AUTO BED LEVELING GRID
 
 // Offsets to the probe relative to the extruder tip (Hotend - Probe)
 // X and Y offsets MUST be INTEGERS
@@ -343,10 +335,13 @@
 //    |  P (-)    | T <-- probe (-10,-10)
 //    |           |
 //    O-- FRONT --+
+//  (0,0)
 #define X_PROBE_OFFSET_FROM_EXTRUDER  0     // X offset: -left  [of the nozzle] +right
 #define Y_PROBE_OFFSET_FROM_EXTRUDER  0     // Y offset: -front [of the nozzle] +behind
 #define Z_PROBE_OFFSET_FROM_EXTRUDER -1     // Z offset: -below [of the nozzle] (always negative!)
 
+#define XY_TRAVEL_SPEED 10000               // X and Y axis travel speed between probes, in mm/min
+
 #define Z_RAISE_BEFORE_PROBING       10     //How much the extruder will be raised before travelling to the first probing point.
 #define Z_RAISE_BETWEEN_PROBINGS      5     //How much the extruder will be raised when travelling from between next probing points
 #define Z_RAISE_AFTER_PROBING         5     //How much the extruder will be raised after the last probing point.

MK/Configuration_Scara.h

@@ -9,6 +9,7 @@
  * - Endstop pullup resistors
  * - Endstops logic
  * - Endstops min or max
+ * - Min Z height for homing
  * - Stepper enable logic
  * - Stepper step logic
  * - Stepper direction
@@ -24,6 +25,7 @@
  * - Hotend offset
  *
  * Basic-settings can be found in Configuration_Basic.h
+ * Temperature-settings can be found in Configuration_Temperature.h
  * Feature-settings can be found in Configuration_Feature.h
  * Pins-settings can be found in "Configuration_Pins.h"
  */
@@ -124,6 +126,18 @@
 /*****************************************************************************************/
 
 
+/*****************************************************************************************
+ ***************************** MIN Z HEIGHT FOR HOMING **********************************
+ *****************************************************************************************
+ *                                                                                       *
+ * (in mm) Minimal z height before homing (G28) for Z clearance above the bed, clamps,   *
+ * Be sure you have this distance over your Z_MAX_POS in case.                           *
+ *                                                                                       *
+ *****************************************************************************************/
+#define MIN_Z_HEIGHT_FOR_HOMING   0
+/*****************************************************************************************/
+
+
 /*****************************************************************************************
  ********************************* Stepper enable logic **********************************
  *****************************************************************************************

MK/base.h

@@ -61,12 +61,10 @@
 #include "module/language/language.h"
 #include "module/printcounter/printcounter.h"
 #include "module/MK_Main.h"
-#include "module/motion/planner.h"
+#include "module/planner/planner.h"
+#include "module/endstop/endstops.h"
 #include "module/motion/stepper_indirection.h"
 #include "module/motion/stepper.h"
-#include "module/motion/endstops.h"
-#include "module/motion/vector_3.h"
-#include "module/motion/qr_solve.h"
 #include "module/motion/cartesian_correction.h"
 #include "module/temperature/temperature.h"
 #include "module/sensor/flowmeter.h"

MK/module/MK_Main.h

@@ -79,8 +79,12 @@ void prepare_move();
 void kill(const char *);
 void Stop();
 
+#if !MECH(DELTA) && !MECH(SCARA)
+  void set_current_position_from_planner();
+#endif
+
 #if ENABLED(FILAMENT_RUNOUT_SENSOR)
-  void filrunout();
+  void handle_filament_runout();
 #endif
 
 /**

MK/module/conditionals.h

@@ -444,6 +444,9 @@
     #define MIN_PROBE_Y (max(Y_MIN_POS, Y_MIN_POS + Y_PROBE_OFFSET_FROM_EXTRUDER))
     #define MAX_PROBE_Y (min(Y_MAX_POS, Y_MAX_POS + Y_PROBE_OFFSET_FROM_EXTRUDER))
 
+    #define HAS_Z_ENDSTOP_SERVO (ENABLED(Z_ENDSTOP_SERVO_NR) && Z_ENDSTOP_SERVO_NR >= 0)
+    #define SERVO_LEVELING (ENABLED(AUTO_BED_LEVELING_FEATURE) && HAS_Z_ENDSTOP_SERVO)
+
     // Make sure probing points are reachable
     #if LEFT_PROBE_BED_POSITION < MIN_PROBE_X
       #undef LEFT_PROBE_BED_POSITION

MK/module/digipot/digipot_mcp4451.cpp

+/**
+ * MK & MK4due 3D Printer Firmware
+ *
+ * Based on Marlin, Sprinter and grbl
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ * Copyright (C) 2013 - 2016 Alberto Cotronei @MagoKimbra
+ *
+ * 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 "../../base.h"
 
 #if ENABLED(DIGIPOT_I2C)

MK/module/digipot/digipot_mcp4451.h

+/**
+ * MK & MK4due 3D Printer Firmware
+ *
+ * Based on Marlin, Sprinter and grbl
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ * Copyright (C) 2013 - 2016 Alberto Cotronei @MagoKimbra
+ *
+ * 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 DIGIPOT_MCP4451_H
 #define DIGIPOT_MCP4451_H
 

MK/module/motion/endstops.cpp → MK/module/endstop/endstops.cpp

@@ -25,7 +25,7 @@
  */
 
 #include "../../base.h"
-#include "endstops.h"
+//#include "endstops.h"
 
 // TEST_ENDSTOP: test the old and the current status of an endstop
 #define TEST_ENDSTOP(ENDSTOP) (TEST(current_endstop_bits & old_endstop_bits, ENDSTOP))
@@ -189,6 +189,9 @@ void Endstops::report_state() {
         card.sdprinting = false;
         card.closeFile();
         quickStop();
+        #if NOMECH(DELTA) && NOMECH(SCARA)
+          set_current_position_from_planner();
+        #endif
         disable_all_heaters(); // switch off all heaters.
         disable_all_coolers();
       }

MK/module/laser/laser.cpp

-
 /**
  * MK & MK4due 3D Printer Firmware
  *
@@ -24,6 +23,7 @@
 /**
  * laser.cpp - Laser control library for Arduino using 16 bit timers- Version 1
  * Copyright (c) 2013 Timothy Schmidt.  All right reserved.
+ * Copyright (c) 2016 Franco (nextime) Lanza
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
@@ -335,7 +335,6 @@
         }
       }
     }
-
   #endif // LASER_PERIPHERALS
 
 #endif // LASERBEAM

MK/module/laser/laser.h

@@ -23,6 +23,7 @@
 /**
  * laser.cpp - Laser control library for Arduino using 16 bit timers- Version 1
  * Copyright (c) 2013 Timothy Schmidt.  All right reserved.
+ * Copyright (c) 2016 Franco (nextime) Lanza
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public

MK/module/mbl/mesh_bed_leveling.cpp

@@ -29,7 +29,7 @@
   mesh_bed_leveling::mesh_bed_leveling() { reset(); }
 
   void mesh_bed_leveling::reset() {
-    active = 0;
+    status = MBL_STATUS_NONE;
     z_offset = 0;
     for (int8_t y = MESH_NUM_Y_POINTS; y--;)
       for (int8_t x = MESH_NUM_X_POINTS; x--;)

MK/module/mbl/mesh_bed_leveling.h

@@ -22,12 +22,14 @@
 
 #if ENABLED(MESH_BED_LEVELING)
 
+  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))/(MESH_NUM_X_POINTS - 1))
   #define MESH_Y_DIST ((MESH_MAX_Y - (MESH_MIN_Y))/(MESH_NUM_Y_POINTS - 1))
 
   class mesh_bed_leveling {
   public:
-    bool active;
+    uint8_t status; // Has Mesh and Is Active bits
     float z_offset;
     float z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS];
 
@@ -39,6 +41,11 @@
     static FORCE_INLINE float get_probe_y(int8_t i) { return MESH_MIN_Y + (MESH_Y_DIST) * i; }
     void set_z(const int8_t px, const int8_t py, const float z) { z_values[py][px] = z; }
 
+    bool active()                 { return TEST(status, MBL_STATUS_ACTIVE_BIT); }
+    void set_active(bool onOff)   { if (onOff) SBI(status, MBL_STATUS_ACTIVE_BIT); else CBI(status, MBL_STATUS_ACTIVE_BIT); }
+    bool has_mesh()               { return TEST(status, MBL_STATUS_HAS_MESH_BIT); }
+    void set_has_mesh(bool onOff) { if (onOff) SBI(status, MBL_STATUS_HAS_MESH_BIT); else CBI(status, MBL_STATUS_HAS_MESH_BIT); }
+
     inline void zigzag(int8_t index, int8_t &px, int8_t &py) {
       px = index % (MESH_NUM_X_POINTS);
       py = index / (MESH_NUM_X_POINTS);

MK/module/mechanics.h

@@ -34,5 +34,6 @@
   #define MECH_SCARA       4
 
   #define MECH(mech)    (MECHANISM == MECH_##mech)
+  #define NOMECH(mech)  (MECHANISM != MECH_##mech)
 
 #endif
\ No newline at end of file

MK/module/motion/cartesian_correction.cpp

@@ -22,7 +22,7 @@
 
 /**
  * cartesian_correction.cpp
- * A class that manages hysteresis by inserting extra plan_buffer_line when necessary
+ * A class that manages hysteresis by inserting extra planner.buffer_line when necessary
  * A class that manages ZWobble
  *
  * Copyright (c) 2012 Neil James Martin
@@ -86,7 +86,7 @@
   //===========================================================================
   void Hysteresis::calcSteps() {
     for (uint8_t i = 0; i < NUM_AXIS; i++)
-      m_hysteresis_steps[i] = (long)(m_hysteresis_mm[i] * axis_steps_per_unit[i]);
+      m_hysteresis_steps[i] = (long)(m_hysteresis_mm[i] * planner.axis_steps_per_unit[i]);
   }
 
   //===========================================================================
@@ -135,9 +135,9 @@
   }
 
   //===========================================================================
-  // insert a plan_buffer_line if required to handle any hysteresis
+  // insert a planner.buffer_line if required to handle any hysteresis
   void Hysteresis::InsertCorrection(const float x, const float y, const float z, const float e) {
-    long destination[NUM_AXIS] = {x * axis_steps_per_unit[X_AXIS], y * axis_steps_per_unit[Y_AXIS], z * axis_steps_per_unit[Z_AXIS], e * axis_steps_per_unit[E_AXIS + active_extruder]};
+    long destination[NUM_AXIS] = {x * planner.axis_steps_per_unit[X_AXIS], y * planner.axis_steps_per_unit[Y_AXIS], z * planner.axis_steps_per_unit[Z_AXIS], e * planner.axis_steps_per_unit[E_AXIS + active_extruder]};
     uint8_t direction_bits = calc_direction_bits(position, destination);
     uint8_t move_bits = calc_move_bits(position, destination);
 
@@ -459,12 +459,12 @@
   }
 
   //===========================================================================
-  // insert a plan_buffer_line if required to handle any hysteresis
+  // insert a planner.buffer_line if required to handle any hysteresis
   void ZWobble::InsertCorrection(const float targetZ) {
 
     if (!m_consistent) return; // don't go through consistency checks all the time; just check one bool
 
-    float originZ = (float)position[Z_AXIS] / axis_steps_per_unit[Z_AXIS];
+    float originZ = (float)position[Z_AXIS] / planner.axis_steps_per_unit[Z_AXIS];
 
     if (originZ < ZWOBBLE_MIN_Z || targetZ < ZWOBBLE_MIN_Z) return;
 
@@ -492,7 +492,7 @@
       ECHO_MV(" Target Rod: ", targetZRod);
 
     // difference in steps between the correct movement (originZRod->targetZRod) and the planned movement
-    long stepDiff = lround((targetZRod - originZRod) * axis_steps_per_unit[Z_AXIS]) - (lround(targetZ * axis_steps_per_unit[Z_AXIS]) - position[Z_AXIS]);
+    long stepDiff = lround((targetZRod - originZRod) * planner.axis_steps_per_unit[Z_AXIS]) - (lround(targetZ * planner.axis_steps_per_unit[Z_AXIS]) - position[Z_AXIS]);
 
     if (DEBUGGING(DEBUG))
       ECHO_EMV(" stepDiff: ", stepDiff);

MK/module/motion/cartesian_correction.h

@@ -22,7 +22,7 @@
 
 /**
  * cartesian_correction.h
- * A class that manages hysteresis by inserting extra plan_buffer_line when necessary
+ * A class that manages hysteresis by inserting extra planner.buffer_line when necessary
  * A class that manages ZWobble
  *
  * Copyright (c) 2012 Neil James Martin

MK/module/motion/stepper.cpp

@@ -417,7 +417,7 @@ ISR(TIMER1_COMPA_vect) {
 
   if (cleaning_buffer_counter) {
     current_block = NULL;
-    plan_discard_current_block();
+    planner.discard_current_block();
     #if ENABLED(SD_FINISHED_RELEASECOMMAND)
       if ((cleaning_buffer_counter == 1) && (SD_FINISHED_STEPPERRELEASE)) enqueue_and_echo_commands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
     #endif
@@ -438,7 +438,7 @@ ISR(TIMER1_COMPA_vect) {
   // If there is no current block, attempt to pop one from the buffer
   if (!current_block) {
     // Anything in the buffer?
-    current_block = plan_get_current_block();
+    current_block = planner.get_current_block();
     if (current_block) {
       current_block->busy = true;
       trapezoid_generator_reset();
@@ -651,12 +651,6 @@ ISR(TIMER1_COMPA_vect) {
         #endif
       #endif // LASERBEAM
 
-      // safe check for erroneous calculated events count
-      if(current_block->step_event_count >= MAX_EVENTS_COUNT) {
-         kill_current_block();
-         break;
-      }
-
       step_events_completed++;
       if (step_events_completed >= current_block->step_event_count) break;
     }
@@ -710,7 +704,6 @@ ISR(TIMER1_COMPA_vect) {
 
       if (step_rate <= acc_step_rate) {
         step_rate = acc_step_rate - step_rate; // Decelerate from acceleration end point.
-        // lower limit
         NOLESS(step_rate, current_block->final_rate);
       }
       else {
@@ -763,7 +756,7 @@ ISR(TIMER1_COMPA_vect) {
     // If current block is finished, reset pointer
     if (step_events_completed >= current_block->step_event_count) {
       current_block = NULL;
-      plan_discard_current_block();
+      planner.discard_current_block();
       #if ENABLED(LASERBEAM) && ENABLED(LASER_PULSE_METHOD)
         if (current_block->laser_mode == CONTINUOUS && current_block->laser_status == LASER_ON)
           laser_extinguish();
@@ -1043,7 +1036,7 @@ void st_init() {
 /**
  * Block until all buffered steps are executed
  */
-void st_synchronize() { while (blocks_queued()) idle(); }
+void st_synchronize() { while (planner.blocks_queued()) idle(); }
 
 /**
  * Set the stepper positions directly in steps
@@ -1119,7 +1112,7 @@ float st_get_axis_position_mm(AxisEnum axis) {
     axis_pos = st_get_position(axis);
   #endif
 
-  return axis_pos / axis_steps_per_unit[axis];
+  return axis_pos / planner.axis_steps_per_unit[axis];
 }
 
 void enable_all_steppers() {
@@ -1150,7 +1143,7 @@ void finishAndDisableSteppers() {
 void quickStop() {
   cleaning_buffer_counter = 5000;
   DISABLE_STEPPER_DRIVER_INTERRUPT();
-  while (blocks_queued()) plan_discard_current_block();
+  while (planner.blocks_queued()) planner.discard_current_block();
   current_block = NULL;
   ENABLE_STEPPER_DRIVER_INTERRUPT();
 }
@@ -1217,7 +1210,7 @@ void kill_current_block() {
 }
 
 float triggered_position_mm(AxisEnum axis) {
-  return endstops_trigsteps[axis] / axis_steps_per_unit[axis];
+  return endstops_trigsteps[axis] / planner.axis_steps_per_unit[axis];
 }
 
 bool motor_direction(AxisEnum axis) { return TEST(last_direction_bits, axis); }
@@ -1456,7 +1449,7 @@ void microstep_readings() {
 }
 
 #if ENABLED(Z_DUAL_ENDSTOPS)
-  void In_Homing_Process(bool state) { performing_homing = state; }
-  void Lock_z_motor(bool state) { locked_z_motor = state; }
-  void Lock_z2_motor(bool state) { locked_z2_motor = state; }
+  void set_homing_flag(bool state) { performing_homing = state; }
+  void set_z_lock(bool state) { locked_z_motor = state; }
+  void set_z2_lock(bool state) { locked_z2_motor = state; }
 #endif

MK/module/motion/stepper.h

@@ -22,6 +22,22 @@
 
 /**
  * stepper.h - stepper motor driver: executes motion plans of planner.c using the stepper motors
+ * Part of Grbl
+ *
+ * Copyright (c) 2009-2011 Simen Svale Skogsrud
+ *
+ * Grbl 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.
+ *
+ * Grbl 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 Grbl. If not, see <http://www.gnu.org/licenses/>.
  */
 
 #ifndef STEPPER_H
@@ -107,9 +123,9 @@
   void kill_current_block();
 
   #if ENABLED(Z_DUAL_ENDSTOPS)
-    void In_Homing_Process(bool state);
-    void Lock_z_motor(bool state);
-    void Lock_z2_motor(bool state);
+    void set_homing_flag(bool state);
+    void set_z_lock(bool state);
+    void set_z2_lock(bool state);
   #endif
 
   #if ENABLED(BABYSTEPPING)

MK/module/motion/stepper_indirection.h

@@ -45,17 +45,19 @@
 #define X_ENABLE_READ READ(X_ENABLE_PIN)
 
 // X2 motor
-#define X2_STEP_INIT SET_OUTPUT(X2_STEP_PIN)
-#define X2_STEP_WRITE(STATE) WRITE(X2_STEP_PIN,STATE)
-#define X2_STEP_READ READ(X2_STEP_PIN)
+#if ENABLED(DUAL_X_CARRIAGE)
+  #define X2_STEP_INIT SET_OUTPUT(X2_STEP_PIN)
+  #define X2_STEP_WRITE(STATE) WRITE(X2_STEP_PIN,STATE)
+  #define X2_STEP_READ READ(X2_STEP_PIN)
 
-#define X2_DIR_INIT SET_OUTPUT(X2_DIR_PIN)
-#define X2_DIR_WRITE(STATE) WRITE(X2_DIR_PIN,STATE)
-#define X2_DIR_READ READ(X_DIR_PIN)
+  #define X2_DIR_INIT SET_OUTPUT(X2_DIR_PIN)
+  #define X2_DIR_WRITE(STATE) WRITE(X2_DIR_PIN,STATE)
+  #define X2_DIR_READ READ(X_DIR_PIN)
 
-#define X2_ENABLE_INIT SET_OUTPUT(X2_ENABLE_PIN)
-#define X2_ENABLE_WRITE(STATE) WRITE(X2_ENABLE_PIN,STATE)
-#define X2_ENABLE_READ READ(X_ENABLE_PIN)
+  #define X2_ENABLE_INIT SET_OUTPUT(X2_ENABLE_PIN)
+  #define X2_ENABLE_WRITE(STATE) WRITE(X2_ENABLE_PIN,STATE)
+  #define X2_ENABLE_READ READ(X_ENABLE_PIN)
+#endif // DUAL_X_CARRIAGE
 
 // Y motor
 #define Y_STEP_INIT SET_OUTPUT(Y_STEP_PIN)
@@ -71,17 +73,19 @@
 #define Y_ENABLE_READ READ(Y_ENABLE_PIN)
 
 // Y2 motor
-#define Y2_STEP_INIT SET_OUTPUT(Y2_STEP_PIN)
-#define Y2_STEP_WRITE(STATE) WRITE(Y2_STEP_PIN,STATE)
-#define Y2_STEP_READ READ(Y2_STEP_PIN)
+#if ENABLED(Y_DUAL_STEPPER_DRIVERS)
+  #define Y2_STEP_INIT SET_OUTPUT(Y2_STEP_PIN)
+  #define Y2_STEP_WRITE(STATE) WRITE(Y2_STEP_PIN,STATE)
+  #define Y2_STEP_READ READ(Y2_STEP_PIN)
 
-#define Y2_DIR_INIT SET_OUTPUT(Y2_DIR_PIN)
-#define Y2_DIR_WRITE(STATE) WRITE(Y2_DIR_PIN,STATE)
-#define Y2_DIR_READ READ(Y2_DIR_PIN)
+  #define Y2_DIR_INIT SET_OUTPUT(Y2_DIR_PIN)
+  #define Y2_DIR_WRITE(STATE) WRITE(Y2_DIR_PIN,STATE)
+  #define Y2_DIR_READ READ(Y2_DIR_PIN)
 
-#define Y2_ENABLE_INIT SET_OUTPUT(Y2_ENABLE_PIN)
-#define Y2_ENABLE_WRITE(STATE) WRITE(Y2_ENABLE_PIN,STATE)
-#define Y2_ENABLE_READ READ(Y2_ENABLE_PIN)
+  #define Y2_ENABLE_INIT SET_OUTPUT(Y2_ENABLE_PIN)
+  #define Y2_ENABLE_WRITE(STATE) WRITE(Y2_ENABLE_PIN,STATE)
+  #define Y2_ENABLE_READ READ(Y2_ENABLE_PIN)
+#endif // Y_DUAL_STEPPER_DRIVERS
 
 // Z motor
 #define Z_STEP_INIT SET_OUTPUT(Z_STEP_PIN)
@@ -97,17 +101,19 @@
 #define Z_ENABLE_READ READ(Z_ENABLE_PIN)
 
 // Z2 motor
-#define Z2_STEP_INIT SET_OUTPUT(Z2_STEP_PIN)
-#define Z2_STEP_WRITE(STATE) WRITE(Z2_STEP_PIN,STATE)
-#define Z2_STEP_READ READ(Z2_STEP_PIN)
+#if ENABLED(Z_DUAL_STEPPER_DRIVERS)
+  #define Z2_STEP_INIT SET_OUTPUT(Z2_STEP_PIN)
+  #define Z2_STEP_WRITE(STATE) WRITE(Z2_STEP_PIN,STATE)
+  #define Z2_STEP_READ READ(Z2_STEP_PIN)
 
-#define Z2_DIR_INIT SET_OUTPUT(Z2_DIR_PIN)
-#define Z2_DIR_WRITE(STATE) WRITE(Z2_DIR_PIN,STATE)
-#define Z2_DIR_READ READ(Z2_DIR_PIN)
+  #define Z2_DIR_INIT SET_OUTPUT(Z2_DIR_PIN)
+  #define Z2_DIR_WRITE(STATE) WRITE(Z2_DIR_PIN,STATE)
+  #define Z2_DIR_READ READ(Z2_DIR_PIN)
 
-#define Z2_ENABLE_INIT SET_OUTPUT(Z2_ENABLE_PIN)
-#define Z2_ENABLE_WRITE(STATE) WRITE(Z2_ENABLE_PIN,STATE)
-#define Z2_ENABLE_READ READ(Z2_ENABLE_PIN)
+  #define Z2_ENABLE_INIT SET_OUTPUT(Z2_ENABLE_PIN)
+  #define Z2_ENABLE_WRITE(STATE) WRITE(Z2_ENABLE_PIN,STATE)
+  #define Z2_ENABLE_READ READ(Z2_ENABLE_PIN)
+#endif // Z_DUAL_STEPPER_DRIVERS
 
 // E0 motor
 #define E0_STEP_INIT SET_OUTPUT(E0_STEP_PIN)
@@ -374,11 +380,11 @@
 // TMC26X drivers have step and dir on normal pins, but everything else via SPI
 //////////////////////////////////
 #if ENABLED(HAVE_TMCDRIVER)
-#include <SPI.h>
-#include <TMC26XStepper.h>
+  #include <SPI.h>
+  #include <TMC26XStepper.h>
 
   void tmc_init();
-#if ENABLED(X_IS_TMC)
+  #if ENABLED(X_IS_TMC)
     extern TMC26XStepper stepperX;
     #undef X_ENABLE_INIT
     #define X_ENABLE_INIT ((void)0)
@@ -389,8 +395,8 @@
     #undef X_ENABLE_READ
     #define X_ENABLE_READ stepperX.isEnabled()
 
-#endif
-#if ENABLED(X2_IS_TMC)
+  #endif
+  #if ENABLED(X2_IS_TMC)
     extern TMC26XStepper stepperX2;
     #undef X2_ENABLE_INIT
     #define X2_ENABLE_INIT ((void)0)
@@ -400,8 +406,8 @@
 
     #undef X2_ENABLE_READ
     #define X2_ENABLE_READ stepperX2.isEnabled()
-#endif
-#if ENABLED(Y_IS_TMC)
+  #endif
+  #if ENABLED(Y_IS_TMC)
     extern TMC26XStepper stepperY;
     #undef Y_ENABLE_INIT
     #define Y_ENABLE_INIT ((void)0)
@@ -411,8 +417,8 @@
 
     #undef Y_ENABLE_READ
     #define Y_ENABLE_READ stepperY.isEnabled()
-#endif
-#if ENABLED(Y2_IS_TMC)
+  #endif
+  #if ENABLED(Y2_IS_TMC)
     extern TMC26XStepper stepperY2;
     #undef Y2_ENABLE_INIT
     #define Y2_ENABLE_INIT ((void)0)
@@ -422,8 +428,8 @@
 
     #undef Y2_ENABLE_READ
     #define Y2_ENABLE_READ stepperY2.isEnabled()
-#endif
-#if ENABLED(Z_IS_TMC)
+  #endif
+  #if ENABLED(Z_IS_TMC)
     extern TMC26XStepper stepperZ;
     #undef Z_ENABLE_INIT
     #define Z_ENABLE_INIT ((void)0)
@@ -433,8 +439,8 @@
 
     #undef Z_ENABLE_READ
     #define Z_ENABLE_READ stepperZ.isEnabled()
-#endif
-#if ENABLED(Z2_IS_TMC)
+  #endif
+  #if ENABLED(Z2_IS_TMC)
     extern TMC26XStepper stepperZ2;
     #undef Z2_ENABLE_INIT
     #define Z2_ENABLE_INIT ((void)0)
@@ -444,8 +450,8 @@
 
     #undef Z2_ENABLE_READ
     #define Z2_ENABLE_READ stepperZ2.isEnabled()
-#endif
-#if ENABLED(E0_IS_TMC)
+  #endif
+  #if ENABLED(E0_IS_TMC)
     extern TMC26XStepper stepperE0;
     #undef E0_ENABLE_INIT
     #define E0_ENABLE_INIT ((void)0)
@@ -455,8 +461,8 @@
 
     #undef E0_ENABLE_READ
     #define E0_ENABLE_READ stepperE0.isEnabled()
-#endif
-#if ENABLED(E1_IS_TMC)
+  #endif
+  #if ENABLED(E1_IS_TMC)
     extern TMC26XStepper stepperE1;
     #undef E1_ENABLE_INIT
     #define E1_ENABLE_INIT ((void)0)
@@ -466,8 +472,8 @@
 
     #undef E1_ENABLE_READ
     #define E1_ENABLE_READ stepperE1.isEnabled()
-#endif
-#if ENABLED(E2_IS_TMC)
+  #endif
+  #if ENABLED(E2_IS_TMC)
     extern TMC26XStepper stepperE2;
     #undef E2_ENABLE_INIT
     #define E2_ENABLE_INIT ((void)0)
@@ -477,8 +483,8 @@
 
     #undef E2_ENABLE_READ
     #define E2_ENABLE_READ stepperE2.isEnabled()
-#endif
-#if ENABLED(E3_IS_TMC)
+  #endif
+  #if ENABLED(E3_IS_TMC)
     extern TMC26XStepper stepperE3;
     #undef E3_ENABLE_INIT
     #define E3_ENABLE_INIT ((void)0)
@@ -488,7 +494,7 @@
 
     #undef E3_ENABLE_READ
     #define E3_ENABLE_READ stepperE3.isEnabled()
-#endif
+  #endif
 
 #endif  // HAVE_TMCDRIVER
 
@@ -498,11 +504,11 @@
 //////////////////////////////////
 #if ENABLED(HAVE_L6470DRIVER)
 
-#include <SPI.h>
-#include <L6470.h>
+  #include <SPI.h>
+  #include <L6470.h>
 
   void L6470_init();
-#if ENABLED(X_IS_L6470)
+  #if ENABLED(X_IS_L6470)
     extern L6470 stepperX;
     #undef X_ENABLE_INIT
     #define X_ENABLE_INIT ((void)0)
@@ -522,8 +528,8 @@
     #undef X_DIR_READ
     #define X_DIR_READ (stepperX.getStatus() & STATUS_DIR)
 
-#endif
-#if ENABLED(X2_IS_L6470)
+  #endif
+  #if ENABLED(X2_IS_L6470)
     extern L6470 stepperX2;
     #undef X2_ENABLE_INIT
     #define X2_ENABLE_INIT ((void)0)
@@ -542,8 +548,8 @@
 
     #undef X2_DIR_READ
     #define X2_DIR_READ (stepperX2.getStatus() & STATUS_DIR)
-#endif
-#if ENABLED(Y_IS_L6470)
+  #endif
+  #if ENABLED(Y_IS_L6470)
     extern L6470 stepperY;
     #undef Y_ENABLE_INIT
     #define Y_ENABLE_INIT ((void)0)
@@ -562,8 +568,8 @@
 
     #undef Y_DIR_READ
     #define Y_DIR_READ (stepperY.getStatus() & STATUS_DIR)
-#endif
-#if ENABLED(Y2_IS_L6470)
+  #endif
+  #if ENABLED(Y2_IS_L6470)
     extern L6470 stepperY2;
     #undef Y2_ENABLE_INIT
     #define Y2_ENABLE_INIT ((void)0)
@@ -582,8 +588,8 @@
 
     #undef Y2_DIR_READ
     #define Y2_DIR_READ (stepperY2.getStatus() & STATUS_DIR)
-#endif
-#if ENABLED(Z_IS_L6470)
+  #endif
+  #if ENABLED(Z_IS_L6470)
     extern L6470 stepperZ;
     #undef Z_ENABLE_INIT
     #define Z_ENABLE_INIT ((void)0)
@@ -602,8 +608,8 @@
 
     #undef Y_DIR_READ
     #define Y_DIR_READ (stepperZ.getStatus() & STATUS_DIR)
-#endif
-#if ENABLED(Z2_IS_L6470)
+  #endif
+  #if ENABLED(Z2_IS_L6470)
     extern L6470 stepperZ2;
     #undef Z2_ENABLE_INIT
     #define Z2_ENABLE_INIT ((void)0)
@@ -622,8 +628,8 @@
 
     #undef Y2_DIR_READ
     #define Y2_DIR_READ (stepperZ2.getStatus() & STATUS_DIR)
-#endif
-#if ENABLED(E0_IS_L6470)
+  #endif
+  #if ENABLED(E0_IS_L6470)
     extern L6470 stepperE0;
     #undef E0_ENABLE_INIT
     #define E0_ENABLE_INIT ((void)0)
@@ -642,8 +648,8 @@
 
     #undef E0_DIR_READ
     #define E0_DIR_READ (stepperE0.getStatus() & STATUS_DIR)
-#endif
-#if ENABLED(E1_IS_L6470)
+  #endif
+  #if ENABLED(E1_IS_L6470)
     extern L6470 stepperE1;
     #undef E1_ENABLE_INIT
     #define E1_ENABLE_INIT ((void)0)
@@ -662,8 +668,8 @@
 
     #undef E1_DIR_READ
     #define E1_DIR_READ (stepperE1.getStatus() & STATUS_DIR)
-#endif
-#if ENABLED(E2_IS_L6470)
+  #endif
+  #if ENABLED(E2_IS_L6470)
     extern L6470 stepperE2;
     #undef E2_ENABLE_INIT
     #define E2_ENABLE_INIT ((void)0)
@@ -682,8 +688,8 @@
 
     #undef E2_DIR_READ
     #define E2_DIR_READ (stepperE2.getStatus() & STATUS_DIR)
-#endif
-#if ENABLED(E3_IS_L6470)
+  #endif
+  #if ENABLED(E3_IS_L6470)
     extern L6470 stepperE3;
     #undef E3_ENABLE_INIT
     #define E3_ENABLE_INIT ((void)0)
@@ -702,7 +708,7 @@
 
     #undef E3_DIR_READ
     #define E3_DIR_READ (stepperE3.getStatus() & STATUS_DIR)
-#endif
+  #endif
 
 #endif  //HAVE_L6470DRIVER
 

MK/module/motion/vector_3.cpp

-/**
- * MK & MK4due 3D Printer Firmware
- *
- * Based on Marlin, Sprinter and grbl
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- * Copyright (C) 2013 - 2016 Alberto Cotronei @MagoKimbra
- *
- * 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/>.
- *
- */
-
-/**
- * vector_3.cpp - Vector library for bed leveling
- * Copyright (c) 2012 Lars Brubaker.  All right reserved.
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
- *
- * This library 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
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-#include "../../base.h"
-
-#if ENABLED(AUTO_BED_LEVELING_FEATURE)
-  #include <math.h>
-  #include "vector_3.h"
-
-  vector_3::vector_3() : x(0), y(0), z(0) { }
-
-  vector_3::vector_3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) { }
-
-  vector_3 vector_3::cross(vector_3 left, vector_3 right) {
-    return vector_3(left.y * right.z - left.z * right.y,
-                    left.z * right.x - left.x * right.z,
-                    left.x * right.y - left.y * right.x);
-  }
-
-  vector_3 vector_3::operator+(vector_3 v) { return vector_3((x + v.x), (y + v.y), (z + v.z)); }
-  vector_3 vector_3::operator-(vector_3 v) { return vector_3((x - v.x), (y - v.y), (z - v.z)); }
-
-  vector_3 vector_3::get_normal() {
-    vector_3 normalized = vector_3(x, y, z);
-    normalized.normalize();
-    return normalized;
-  }
-
-  float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); }
-
-  void vector_3::normalize() {
-    float length = get_length();
-    x /= length;
-    y /= length;
-    z /= length;
-  }
-
-  void vector_3::apply_rotation(matrix_3x3 matrix) {
-    float resultX = x * matrix.matrix[0][0] + y * matrix.matrix[1][0] + z * matrix.matrix[2][0];
-    float resultY = x * matrix.matrix[0][1] + y * matrix.matrix[1][1] + z * matrix.matrix[2][1];
-    float resultZ = x * matrix.matrix[0][2] + y * matrix.matrix[1][2] + z * matrix.matrix[2][2];
-    x = resultX;
-    y = resultY;
-    z = resultZ;
-  }
-
-  void vector_3::debug(const char title[]) {
-    ECHO_ST(DB, title);
-    ECHO_MV(" x: ", x, 6);
-    ECHO_MV(" y: ", y, 6);
-    ECHO_EMV(" z: ", z, 6);
-  }
-
-  void apply_rotation_xyz(matrix_3x3 matrix, float& x, float& y, float& z) {
-    vector_3 vector = vector_3(x, y, z);
-    vector.apply_rotation(matrix);
-    x = vector.x;
-    y = vector.y;
-    z = vector.z;
-  }
-
-  matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) {
-    //row_0.debug("row_0");
-    //row_1.debug("row_1");
-    //row_2.debug("row_2");
-    matrix_3x3 new_matrix;
-    new_matrix.matrix[0][0] = row_0.x; new_matrix.matrix[0][1] = row_0.y; new_matrix.matrix[0][2] = row_0.z;
-    new_matrix.matrix[1][0] = row_1.x; new_matrix.matrix[1][1] = row_1.y; new_matrix.matrix[1][2] = row_1.z;
-    new_matrix.matrix[2][0] = row_2.x; new_matrix.matrix[2][1] = row_2.y; new_matrix.matrix[2][2] = row_2.z;
-    //new_matrix.debug("new_matrix");
-    return new_matrix;
-  }
-
-  void matrix_3x3::set_to_identity() {
-    matrix[0][0] = 1; matrix[0][1] = 0; matrix[0][2] = 0;
-    matrix[1][0] = 0; matrix[1][1] = 1; matrix[1][2] = 0;
-    matrix[2][0] = 0; matrix[2][1] = 0; matrix[2][2] = 1;
-  }
-
-  matrix_3x3 matrix_3x3::create_look_at(vector_3 target) {
-    vector_3 z_row = target.get_normal();
-    vector_3 x_row = vector_3(1, 0, -target.x / target.z).get_normal();
-    vector_3 y_row = vector_3::cross(z_row, x_row).get_normal();
-
-    // x_row.debug("x_row");
-    // y_row.debug("y_row");
-    // z_row.debug("z_row");
-
-    // create the matrix already correctly transposed
-    matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row);
-
-    // rot.debug("rot");
-    return rot;
-  }
-
-  matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) {
-    matrix_3x3 new_matrix;
-    new_matrix.matrix[0][0] = original.matrix[0][0]; new_matrix.matrix[0][1] = original.matrix[1][0]; new_matrix.matrix[0][2] = original.matrix[2][0];
-    new_matrix.matrix[1][0] = original.matrix[0][1]; new_matrix.matrix[1][1] = original.matrix[1][1]; new_matrix.matrix[1][2] = original.matrix[2][1];
-    new_matrix.matrix[2][0] = original.matrix[0][2]; new_matrix.matrix[2][1] = original.matrix[1][2]; new_matrix.matrix[2][2] = original.matrix[2][2];
-    return new_matrix;
-  }
-
-  void matrix_3x3::debug(const char title[]) {
-    ECHO_LT(DB, title);
-    for (uint8_t i = 0; i < 3; i++) {
-      ECHO_S(DB);
-      for (uint8_t j = 0; j < 3; j++) {
-        if (matrix[i][j] >= 0.0) ECHO_C('+');
-        ECHO_V(matrix[i][j], 6);
-        ECHO_C(' ');
-      }
-      ECHO_E;
-    }
-  }
-
-#endif // AUTO_BED_LEVELING_FEATURE

MK/module/motion/qr_solve.h → MK/module/planner/qr_solve.h

@@ -20,23 +20,31 @@
  *
  */
 
+#include "../../base.h"
+
+#ifndef QR_SOLVE_H
+#define QR_SOLVE_H
+
 #if ENABLED(AUTO_BED_LEVELING_GRID)
 
-  void daxpy(int n, double da, double dx[], int incx, double dy[], int incy);
-  double ddot(int n, double dx[], int incx, double dy[], int incy);
-  double dnrm2(int n, double x[], int incx);
-  void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
+void daxpy(int n, double da, double dx[], int incx, double dy[], int incy);
+double ddot(int n, double dx[], int incx, double dy[], int incy);
+double dnrm2(int n, double x[], int incx);
+void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
             int jpvt[], double qraux[]);
-  void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
+void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
            double work[], int job);
-  int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
+int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
           double x[], double rsd[], int jpvt[], double qraux[], int itask);
-  void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
+void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
             double rsd[], int jpvt[], double qraux[]);
-  int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
+int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
           double qy[], double qty[], double b[], double rsd[], double ab[], int job);
-  void dscal(int n, double sa, double x[], int incx);
-  void dswap(int n, double x[], int incx, double y[], int incy);
-  void qr_solve(double x[], int m, int n, double a[], double b[]);
+void dscal(int n, double sa, double x[], int incx);
+void dswap(int n, double x[], int incx, double y[], int incy);
+void qr_solve(double x[], int m, int n, double a[], double b[]);
+
+#endif // ENABLED(AUTO_BED_LEVELING_GRID)
+
+#endif // QR_SOLVE_H
 
-#endif