Merge pull request #673 from fsantini/ErikZalm

Improvements to the auto bed leveling feature

Marlin/Configuration.h

 
   #endif
 
+  // with accurate bed leveling, the bed is sampled in a ACCURATE_BED_LEVELING_POINTSxACCURATE_BED_LEVELING_POINTS grid and least squares solution is calculated
+  // Note: this feature occupies 10'206 byte
+  #define ACCURATE_BED_LEVELING
+  
+  #ifdef ACCURATE_BED_LEVELING
+     // I wouldn't see a reason to go above 3 (=9 probing points on the bed)
+    #define ACCURATE_BED_LEVELING_POINTS 2
+  #endif
+  
 #endif
 
 

Marlin/Marlin_main.cpp

 
 #ifdef ENABLE_AUTO_BED_LEVELING
 #include "vector_3.h"
+  #ifdef ACCURATE_BED_LEVELING
+    #include "qr_solve.h"
+  #endif
 #endif // ENABLE_AUTO_BED_LEVELING
 
 #include "ultralcd.h"
 }
 
 #ifdef ENABLE_AUTO_BED_LEVELING
+#ifdef ACCURATE_BED_LEVELING
+static void set_bed_level_equation_lsq(double *plane_equation_coefficients)
+{
+    vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
+    planeNormal.debug("planeNormal");
+    plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
+    //bedLevel.debug("bedLevel");
+
+    //plan_bed_level_matrix.debug("bed level before");
+    //vector_3 uncorrected_position = plan_get_position_mm();
+    //uncorrected_position.debug("position before");
+
+    vector_3 corrected_position = plan_get_position();
+//    corrected_position.debug("position after");
+    current_position[X_AXIS] = corrected_position.x;
+    current_position[Y_AXIS] = corrected_position.y;
+    current_position[Z_AXIS] = corrected_position.z;
+
+    // but the bed at 0 so we don't go below it.
+    current_position[Z_AXIS] = -Z_PROBE_OFFSET_FROM_EXTRUDER;
+
+    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+}
+
+#else
 static void set_bed_level_equation(float z_at_xLeft_yFront, float z_at_xRight_yFront, float z_at_xLeft_yBack) {
     plan_bed_level_matrix.set_to_identity();
 
 
     vector_3 xPositive = (xRightyFront - xLeftyFront).get_normal();
     vector_3 yPositive = (xLeftyBack - xLeftyFront).get_normal();
-    vector_3 planeNormal = vector_3::cross(yPositive, xPositive).get_normal();
+    vector_3 planeNormal = vector_3::cross(xPositive, yPositive).get_normal();
 
     //planeNormal.debug("planeNormal");
     //yPositive.debug("yPositive");
-    matrix_3x3 bedLevel = matrix_3x3::create_look_at(planeNormal, yPositive);
+    plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
     //bedLevel.debug("bedLevel");
 
     //plan_bed_level_matrix.debug("bed level before");
     //uncorrected_position.debug("position before");
 
     // and set our bed level equation to do the right thing
-    plan_bed_level_matrix = matrix_3x3::create_inverse(bedLevel);
     //plan_bed_level_matrix.debug("bed level after");
 
     vector_3 corrected_position = plan_get_position();
 
     plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 }
+#endif // ACCURATE_BED_LEVELING
 
 static void run_z_probe() {
     plan_bed_level_matrix.set_to_identity();
             setup_for_endstop_move();
 
             feedrate = homing_feedrate[Z_AXIS];
-
+#ifdef ACCURATE_BED_LEVELING
+            
+            int xGridSpacing = (RIGHT_PROBE_BED_POSITION - LEFT_PROBE_BED_POSITION) / (ACCURATE_BED_LEVELING_POINTS-1);
+            int yGridSpacing = (BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION) / (ACCURATE_BED_LEVELING_POINTS-1);
+            
+            
+            // solve the plane equation ax + by + d = z
+            // A is the matrix with rows [x y 1] for all the probed points
+            // B is the vector of the Z positions
+            // the normal vector to the plane is formed by the coefficients of the plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
+            // so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
+            
+            // "A" matrix of the linear system of equations
+            double eqnAMatrix[ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS*3];
+            // "B" vector of Z points
+            double eqnBVector[ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS];
+            
+            
+            int probePointCounter = 0;
+            bool zig = true;
+            
+            for (int yProbe=FRONT_PROBE_BED_POSITION; yProbe <= BACK_PROBE_BED_POSITION; yProbe += yGridSpacing)
+            {
+              int xProbe, xInc;
+              if (zig)
+              {
+                xProbe = LEFT_PROBE_BED_POSITION;
+                //xEnd = RIGHT_PROBE_BED_POSITION;
+                xInc = xGridSpacing;
+                zig = false;
+              } else // zag
+              {
+                xProbe = RIGHT_PROBE_BED_POSITION;
+                //xEnd = LEFT_PROBE_BED_POSITION;
+                xInc = -xGridSpacing;
+                zig = true;
+              }
+              
+              for (int xCount=0; xCount < ACCURATE_BED_LEVELING_POINTS; xCount++)
+              {
+                if (probePointCounter == 0)
+                {
+                  // raise before probing
+                  do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_BEFORE_PROBING);
+                } else
+                {               
+                  // raise extruder
+                  do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
+                }
+                
+                
+                do_blocking_move_to(xProbe - X_PROBE_OFFSET_FROM_EXTRUDER, yProbe - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
+    
+                engage_z_probe();   // Engage Z Servo endstop if available
+                run_z_probe();
+                eqnBVector[probePointCounter] = current_position[Z_AXIS];
+                retract_z_probe();
+    
+                SERIAL_PROTOCOLPGM("Bed x: ");
+                SERIAL_PROTOCOL(xProbe);
+                SERIAL_PROTOCOLPGM(" y: ");
+                SERIAL_PROTOCOL(yProbe);
+                SERIAL_PROTOCOLPGM(" z: ");
+                SERIAL_PROTOCOL(current_position[Z_AXIS]);
+                SERIAL_PROTOCOLPGM("\n");
+                
+                eqnAMatrix[probePointCounter + 0*ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS] = xProbe;
+                eqnAMatrix[probePointCounter + 1*ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS] = yProbe;
+                eqnAMatrix[probePointCounter + 2*ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS] = 1;
+                probePointCounter++;
+                xProbe += xInc;
+              }
+            }
+            clean_up_after_endstop_move();
+            
+            // solve lsq problem
+            double *plane_equation_coefficients = qr_solve(ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS, 3, eqnAMatrix, eqnBVector);
+            
+            SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
+            SERIAL_PROTOCOL(plane_equation_coefficients[0]);
+            SERIAL_PROTOCOLPGM(" b: ");
+            SERIAL_PROTOCOL(plane_equation_coefficients[1]);
+            SERIAL_PROTOCOLPGM(" d: ");
+            SERIAL_PROTOCOLLN(plane_equation_coefficients[2]);
+            
+            
+            set_bed_level_equation_lsq(plane_equation_coefficients);
+            
+            free(plane_equation_coefficients);
+            
+#else // ACCURATE_BED_LEVELING not defined
+            
+            
             // prob 1
             do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_BEFORE_PROBING);
             do_blocking_move_to(LEFT_PROBE_BED_POSITION - X_PROBE_OFFSET_FROM_EXTRUDER, BACK_PROBE_BED_POSITION - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
             clean_up_after_endstop_move();
 
             set_bed_level_equation(z_at_xLeft_yFront, z_at_xRight_yFront, z_at_xLeft_yBack);
-
+         
+            
+#endif // ACCURATE_BED_LEVELING
             st_synchronize();
 
             // The following code correct the Z height difference from z-probe position and hotend tip position.

Marlin/planner.cpp

 
 	//position.debug("in plan_get position");
 	//plan_bed_level_matrix.debug("in plan_get bed_level");
-	matrix_3x3 inverse = matrix_3x3::create_inverse(plan_bed_level_matrix);
+	matrix_3x3 inverse = matrix_3x3::transpose(plan_bed_level_matrix);
 	//inverse.debug("in plan_get inverse");
 	position.apply_rotation(inverse);
 	//position.debug("after rotation");

Marlin/qr_solve.h

+#include "Configuration.h"
+
+#ifdef ACCURATE_BED_LEVELING
+
+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[], 
+  double work[], int job );
+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[], 
+  double rsd[], int jpvt[], double qraux[] );
+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 );
+double *qr_solve ( int m, int n, double a[], double b[] );
+
+#endif

Marlin/vector_3.cpp

 	matrix[6] = 0; matrix[7] = 0; matrix[8] = 1;
 }
 
-matrix_3x3 matrix_3x3::create_look_at(vector_3 target, vector_3 up)
-{
-    // There are lots of examples of look at code on the internet that don't do all these noramize and also find the position
-    // through several dot products.  The problem with them is that they have a bit of error in that all the vectors arn't normal and need to be.
-    vector_3 z_row = vector_3(-target.x, -target.y, -target.z).get_normal();
-    vector_3 x_row = vector_3::cross(up, z_row).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");
-    
-    matrix_3x3 rot = matrix_3x3::create_from_rows(vector_3(x_row.x, y_row.x, z_row.x),
-                                vector_3(x_row.y, y_row.y, z_row.y),
-                                vector_3(x_row.z, y_row.z, z_row.z));
-
-    //rot.debug("rot");
+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(0, 1, -target.y/target.z).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::create_inverse(matrix_3x3 original)
-{
-	//original.debug("original");
-	float* A = original.matrix;
-	float determinant = 
-		+ A[0 * 3 + 0] * (A[1 * 3 + 1] * A[2 * 3 + 2] - A[2 * 3 + 1] * A[1 * 3 + 2])
-		- A[0 * 3 + 1] * (A[1 * 3 + 0] * A[2 * 3 + 2] - A[1 * 3 + 2] * A[2 * 3 + 0])
-		+ A[0 * 3 + 2] * (A[1 * 3 + 0] * A[2 * 3 + 1] - A[1 * 3 + 1] * A[2 * 3 + 0]);
-	matrix_3x3 inverse;
-	inverse.matrix[0 * 3 + 0] = +(A[1 * 3 + 1] * A[2 * 3 + 2] - A[2 * 3 + 1] * A[1 * 3 + 2]) / determinant;
-	inverse.matrix[0 * 3 + 1] = -(A[0 * 3 + 1] * A[2 * 3 + 2] - A[0 * 3 + 2] * A[2 * 3 + 1]) / determinant;
-	inverse.matrix[0 * 3 + 2] = +(A[0 * 3 + 1] * A[1 * 3 + 2] - A[0 * 3 + 2] * A[1 * 3 + 1]) / determinant;
-	inverse.matrix[1 * 3 + 0] = -(A[1 * 3 + 0] * A[2 * 3 + 2] - A[1 * 3 + 2] * A[2 * 3 + 0]) / determinant;
-	inverse.matrix[1 * 3 + 1] = +(A[0 * 3 + 0] * A[2 * 3 + 2] - A[0 * 3 + 2] * A[2 * 3 + 0]) / determinant;
-	inverse.matrix[1 * 3 + 2] = -(A[0 * 3 + 0] * A[1 * 3 + 2] - A[1 * 3 + 0] * A[0 * 3 + 2]) / determinant;
-	inverse.matrix[2 * 3 + 0] = +(A[1 * 3 + 0] * A[2 * 3 + 1] - A[2 * 3 + 0] * A[1 * 3 + 1]) / determinant;
-	inverse.matrix[2 * 3 + 1] = -(A[0 * 3 + 0] * A[2 * 3 + 1] - A[2 * 3 + 0] * A[0 * 3 + 1]) / determinant;
-	inverse.matrix[2 * 3 + 2] = +(A[0 * 3 + 0] * A[1 * 3 + 1] - A[1 * 3 + 0] * A[0 * 3 + 1]) / determinant;
-
-	vector_3 row0 = vector_3(inverse.matrix[0 * 3 + 0], inverse.matrix[0 * 3 + 1], inverse.matrix[0 * 3 + 2]);
-	vector_3 row1 = vector_3(inverse.matrix[1 * 3 + 0], inverse.matrix[1 * 3 + 1], inverse.matrix[1 * 3 + 2]);
-	vector_3 row2 = vector_3(inverse.matrix[2 * 3 + 0], inverse.matrix[2 * 3 + 1], inverse.matrix[2 * 3 + 2]);
-
-    row0.normalize();
-    row1.normalize();
-    row2.normalize();
-
-	inverse = matrix_3x3::create_from_rows(row0, row1, row2);
-
-	//inverse.debug("inverse");
-	return inverse;
+
+matrix_3x3 matrix_3x3::transpose(matrix_3x3 original)
+{
+  matrix_3x3 new_matrix;
+  new_matrix.matrix[0] = original.matrix[0]; new_matrix.matrix[1] = original.matrix[3]; new_matrix.matrix[2] = original.matrix[6]; 
+  new_matrix.matrix[3] = original.matrix[1]; new_matrix.matrix[4] = original.matrix[4]; new_matrix.matrix[5] = original.matrix[7]; 
+  new_matrix.matrix[6] = original.matrix[2]; new_matrix.matrix[7] = original.matrix[5]; new_matrix.matrix[8] = original.matrix[8];
+  return new_matrix;
 }
 
 void matrix_3x3::debug(char* title)

Marlin/vector_3.h

 	float matrix[9];
 
 	static matrix_3x3 create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2);
-	static matrix_3x3 create_look_at(vector_3 target, vector_3 up);
-	static matrix_3x3 create_inverse(matrix_3x3 original);
+	static matrix_3x3 create_look_at(vector_3 target);
+	static matrix_3x3 transpose(matrix_3x3 original);
 
 	void set_to_identity();