Patch G29 for linear leveling, reachable with probe

Marlin/Marlin_main.cpp

          * so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
          */
 
-        int abl2 = abl_grid_points_x * abl_grid_points_y;
+        int abl2 = abl_grid_points_x * abl_grid_points_y,
+            indexIntoAB[abl_grid_points_x][abl_grid_points_y],
+            probePointCounter = -1;
 
-        double eqnAMatrix[abl2 * 3], // "A" matrix of the linear system of equations
-               eqnBVector[abl2],     // "B" vector of Z points
-               mean = 0.0;
-        int indexIntoAB[abl_grid_points_x][abl_grid_points_y];
+        float eqnAMatrix[abl2 * 3], // "A" matrix of the linear system of equations
+              eqnBVector[abl2],     // "B" vector of Z points
+              mean = 0.0;
 
       #endif // AUTO_BED_LEVELING_LINEAR_GRID
 
-      int probePointCounter = 0;
       bool zig = abl_grid_points_y & 1; //always end at [RIGHT_PROBE_BED_POSITION, BACK_PROBE_BED_POSITION]
 
       for (uint8_t yCount = 0; yCount < abl_grid_points_y; yCount++) {
           float xBase = left_probe_bed_position + xGridSpacing * xCount;
           xProbe = floor(xBase + (xBase < 0 ? 0 : 0.5));
 
-          #if ENABLED(DELTA)
-            // Avoid probing outside the round or hexagonal area of a delta printer
-            float pos[XYZ] = { xProbe + X_PROBE_OFFSET_FROM_EXTRUDER, yProbe + Y_PROBE_OFFSET_FROM_EXTRUDER, 0 };
-            if (!position_is_reachable(pos)) continue;
+          #if ENABLED(AUTO_BED_LEVELING_LINEAR_GRID)
+            indexIntoAB[xCount][yCount] = ++probePointCounter;
+          #endif
+
+          #if IS_KINEMATIC
+            // Avoid probing outside the round or hexagonal area
+            float pos[XYZ] = { xProbe, yProbe, 0 };
+            if (!position_is_reachable(pos, true)) continue;
           #endif
 
           measured_z = probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
             eqnAMatrix[probePointCounter + 0 * abl2] = xProbe;
             eqnAMatrix[probePointCounter + 1 * abl2] = yProbe;
             eqnAMatrix[probePointCounter + 2 * abl2] = 1;
-            indexIntoAB[xCount][yCount] = probePointCounter;
 
           #elif ENABLED(AUTO_BED_LEVELING_NONLINEAR)
 
 
           #endif
 
-          probePointCounter++;
-
           idle();
 
         } //xProbe
       // For LINEAR leveling calculate matrix, print reports, correct the position
 
       // solve lsq problem
-      double plane_equation_coefficients[3];
+      float plane_equation_coefficients[3];
       qr_solve(plane_equation_coefficients, abl2, 3, eqnAMatrix, eqnBVector);
 
       mean /= abl2;

Marlin/qr_solve.cpp

   return (i1 < i2) ? i1 : i2;
 }
 
-double r8_epsilon(void)
+float r8_epsilon(void)
 
 /******************************************************************************/
 /**
 
   Parameters:
 
-    Output, double R8_EPSILON, the R8 round-off unit.
+    Output, float R8_EPSILON, the R8 round-off unit.
 */
 {
-  const double value = 2.220446049250313E-016;
+  const float value = 2.220446049250313E-016;
   return value;
 }
 
-double r8_max(double x, double y)
+float r8_max(float x, float y)
 
 /******************************************************************************/
 /**
 
   Parameters:
 
-    Input, double X, Y, the quantities to compare.
+    Input, float X, Y, the quantities to compare.
 
-    Output, double R8_MAX, the maximum of X and Y.
+    Output, float R8_MAX, the maximum of X and Y.
 */
 {
   return (y < x) ? x : y;
 }
 
-double r8_abs(double x)
+float r8_abs(float x)
 
 /******************************************************************************/
 /**
 
   Parameters:
 
-    Input, double X, the quantity whose absolute value is desired.
+    Input, float X, the quantity whose absolute value is desired.
 
-    Output, double R8_ABS, the absolute value of X.
+    Output, float R8_ABS, the absolute value of X.
 */
 {
   return (x < 0.0) ? -x : x;
 }
 
-double r8_sign(double x)
+float r8_sign(float x)
 
 /******************************************************************************/
 /**
 
   Parameters:
 
-    Input, double X, the number whose sign is desired.
+    Input, float X, the number whose sign is desired.
 
-    Output, double R8_SIGN, the sign of X.
+    Output, float R8_SIGN, the sign of X.
 */
 {
   return (x < 0.0) ? -1.0 : 1.0;
 }
 
-double r8mat_amax(int m, int n, double a[])
+float r8mat_amax(int m, int n, float a[])
 
 /******************************************************************************/
 /**
 
     Input, int N, the number of columns in A.
 
-    Input, double A[M*N], the M by N matrix.
+    Input, float A[M*N], the M by N matrix.
 
-    Output, double R8MAT_AMAX, the maximum absolute value entry of A.
+    Output, float R8MAT_AMAX, the maximum absolute value entry of A.
 */
 {
-  double value = r8_abs(a[0 + 0 * m]);
+  float value = r8_abs(a[0 + 0 * m]);
   for (int j = 0; j < n; j++) {
     for (int i = 0; i < m; i++) {
       NOLESS(value, r8_abs(a[i + j * m]));
   return value;
 }
 
-void r8mat_copy(double a2[], int m, int n, double a1[])
+void r8mat_copy(float a2[], int m, int n, float a1[])
 
 /******************************************************************************/
 /**
 
     Input, int M, N, the number of rows and columns.
 
-    Input, double A1[M*N], the matrix to be copied.
+    Input, float A1[M*N], the matrix to be copied.
 
-    Output, double R8MAT_COPY_NEW[M*N], the copy of A1.
+    Output, float R8MAT_COPY_NEW[M*N], the copy of A1.
 */
 {
   for (int j = 0; j < n; j++) {
 
 /******************************************************************************/
 
-void daxpy(int n, double da, double dx[], int incx, double dy[], int incy)
+void daxpy(int n, float da, float dx[], int incx, float dy[], int incy)
 
 /******************************************************************************/
 /**
 
     Input, int N, the number of elements in DX and DY.
 
-    Input, double DA, the multiplier of DX.
+    Input, float DA, the multiplier of DX.
 
-    Input, double DX[*], the first vector.
+    Input, float DX[*], the first vector.
 
     Input, int INCX, the increment between successive entries of DX.
 
-    Input/output, double DY[*], the second vector.
+    Input/output, float DY[*], the second vector.
     On output, DY[*] has been replaced by DY[*] + DA * DX[*].
 
     Input, int INCY, the increment between successive entries of DY.
 }
 /******************************************************************************/
 
-double ddot(int n, double dx[], int incx, double dy[], int incy)
+float ddot(int n, float dx[], int incx, float dy[], int incy)
 
 /******************************************************************************/
 /**
 
     Input, int N, the number of entries in the vectors.
 
-    Input, double DX[*], the first vector.
+    Input, float DX[*], the first vector.
 
     Input, int INCX, the increment between successive entries in DX.
 
-    Input, double DY[*], the second vector.
+    Input, float DY[*], the second vector.
 
     Input, int INCY, the increment between successive entries in DY.
 
-    Output, double DDOT, the sum of the product of the corresponding
+    Output, float DDOT, the sum of the product of the corresponding
     entries of DX and DY.
 */
 {
   if (n <= 0) return 0.0;
 
   int i, m;
-  double dtemp = 0.0;
+  float dtemp = 0.0;
 
   /**
     Code for unequal increments or equal increments
 }
 /******************************************************************************/
 
-double dnrm2(int n, double x[], int incx)
+float dnrm2(int n, float x[], int incx)
 
 /******************************************************************************/
 /**
 
     Input, int N, the number of entries in the vector.
 
-    Input, double X[*], the vector whose norm is to be computed.
+    Input, float X[*], the vector whose norm is to be computed.
 
     Input, int INCX, the increment between successive entries of X.
 
-    Output, double DNRM2, the Euclidean norm of X.
+    Output, float DNRM2, the Euclidean norm of X.
 */
 {
-  double norm;
+  float norm;
   if (n < 1 || incx < 1)
     norm = 0.0;
   else if (n == 1)
     norm = r8_abs(x[0]);
   else {
-    double scale = 0.0, ssq = 1.0;
+    float scale = 0.0, ssq = 1.0;
     int ix = 0;
     for (int i = 0; i < n; i++) {
       if (x[ix] != 0.0) {
-        double absxi = r8_abs(x[ix]);
+        float absxi = r8_abs(x[ix]);
         if (scale < absxi) {
           ssq = 1.0 + ssq * (scale / absxi) * (scale / absxi);
           scale = absxi;
 }
 /******************************************************************************/
 
-void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
-            int jpvt[], double qraux[])
+void dqrank(float a[], int lda, int m, int n, float tol, int* kr,
+            int jpvt[], float qraux[])
 
 /******************************************************************************/
 /**
 
   Parameters:
 
-    Input/output, double A[LDA*N].  On input, the matrix whose
+    Input/output, float A[LDA*N].  On input, the matrix whose
     decomposition is to be computed.  On output, the information from DQRDC.
     The triangular matrix R of the QR factorization is contained in the
     upper triangle and information needed to recover the orthogonal
 
     Input, int N, the number of columns of A.
 
-    Input, double TOL, a relative tolerance used to determine the
+    Input, float TOL, a relative tolerance used to determine the
     numerical rank.  The problem should be scaled so that all the elements
     of A have roughly the same absolute accuracy, EPS.  Then a reasonable
     value for TOL is roughly EPS divided by the magnitude of the largest
     independent to within the tolerance TOL and the remaining columns
     are linearly dependent.
 
-    Output, double QRAUX[N], will contain extra information defining
+    Output, float QRAUX[N], will contain extra information defining
     the QR factorization.
 */
 {
-  double work[n];
+  float work[n];
 
   for (int i = 0; i < n; i++)
     jpvt[i] = 0;
 }
 /******************************************************************************/
 
-void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
-           double work[], int job)
+void dqrdc(float a[], int lda, int n, int p, float qraux[], int jpvt[],
+           float work[], int job)
 
 /******************************************************************************/
 /**
 
   Parameters:
 
-    Input/output, double A(LDA,P).  On input, the N by P matrix
+    Input/output, float A(LDA,P).  On input, the N by P matrix
     whose decomposition is to be computed.  On output, A contains in
     its upper triangle the upper triangular matrix R of the QR
     factorization.  Below its diagonal A contains information from
 
     Input, int P, the number of columns of the matrix A.
 
-    Output, double QRAUX[P], contains further information required
+    Output, float QRAUX[P], contains further information required
     to recover the orthogonal part of the decomposition.
 
     Input/output, integer JPVT[P].  On input, JPVT contains integers that
     original matrix that has been interchanged into the K-th column, if
     pivoting was requested.
 
-    Workspace, double WORK[P].  WORK is not referenced if JOB == 0.
+    Workspace, float WORK[P].  WORK is not referenced if JOB == 0.
 
     Input, int JOB, initiates column pivoting.
     0, no pivoting is done.
   int j;
   int lup;
   int maxj;
-  double maxnrm, nrmxl, t, tt;
+  float maxnrm, nrmxl, t, tt;
 
   int pl = 1, pu = 0;
   /**
 }
 /******************************************************************************/
 
-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)
+int dqrls(float a[], int lda, int m, int n, float tol, int* kr, float b[],
+          float x[], float rsd[], int jpvt[], float qraux[], int itask)
 
 /******************************************************************************/
 /**
 
   Parameters:
 
-    Input/output, double A[LDA*N], an M by N matrix.
+    Input/output, float A[LDA*N], an M by N matrix.
     On input, the matrix whose decomposition is to be computed.
     In a least squares data fitting problem, A(I,J) is the
     value of the J-th basis (model) function at the I-th data point.
 
     Input, int N, the number of columns of A.
 
-    Input, double TOL, a relative tolerance used to determine the
+    Input, float TOL, a relative tolerance used to determine the
     numerical rank.  The problem should be scaled so that all the elements
     of A have roughly the same absolute accuracy EPS.  Then a reasonable
     value for TOL is roughly EPS divided by the magnitude of the largest
 
     Output, int *KR, the numerical rank.
 
-    Input, double B[M], the right hand side of the linear system.
+    Input, float B[M], the right hand side of the linear system.
 
-    Output, double X[N], a least squares solution to the linear
+    Output, float X[N], a least squares solution to the linear
     system.
 
-    Output, double RSD[M], the residual, B - A*X.  RSD may
+    Output, float RSD[M], the residual, B - A*X.  RSD may
     overwrite B.
 
     Workspace, int JPVT[N], required if ITASK = 1.
     of the condition number of the matrix of independent columns,
     and of R.  This estimate will be <= 1/TOL.
 
-    Workspace, double QRAUX[N], required if ITASK = 1.
+    Workspace, float QRAUX[N], required if ITASK = 1.
 
     Input, int ITASK.
     1, DQRLS factors the matrix A and solves the least squares problem.
 }
 /******************************************************************************/
 
-void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
-            double rsd[], int jpvt[], double qraux[])
+void dqrlss(float a[], int lda, int m, int n, int kr, float b[], float x[],
+            float rsd[], int jpvt[], float qraux[])
 
 /******************************************************************************/
 /**
 
   Parameters:
 
-    Input, double A[LDA*N], the QR factorization information
+    Input, float A[LDA*N], the QR factorization information
     from DQRANK.  The triangular matrix R of the QR factorization is
     contained in the upper triangle and information needed to recover
     the orthogonal matrix Q is stored below the diagonal in A and in
 
     Input, int KR, the rank of the matrix, as estimated by DQRANK.
 
-    Input, double B[M], the right hand side of the linear system.
+    Input, float B[M], the right hand side of the linear system.
 
-    Output, double X[N], a least squares solution to the
+    Output, float X[N], a least squares solution to the
     linear system.
 
-    Output, double RSD[M], the residual, B - A*X.  RSD may
+    Output, float RSD[M], the residual, B - A*X.  RSD may
     overwrite B.
 
     Input, int JPVT[N], the pivot information from DQRANK.
     independent to within the tolerance TOL and the remaining columns
     are linearly dependent.
 
-    Input, double QRAUX[N], auxiliary information from DQRANK
+    Input, float QRAUX[N], auxiliary information from DQRANK
     defining the QR factorization.
 */
 {
   int j;
   int job;
   int k;
-  double t;
+  float t;
 
   if (kr != 0) {
     job = 110;
 }
 /******************************************************************************/
 
-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)
+int dqrsl(float a[], int lda, int n, int k, float qraux[], float y[],
+          float qy[], float qty[], float b[], float rsd[], float ab[], int job)
 
 /******************************************************************************/
 /**
 
   Parameters:
 
-    Input, double A[LDA*P], contains the output of DQRDC.
+    Input, float A[LDA*P], contains the output of DQRDC.
 
     Input, int LDA, the leading dimension of the array A.
 
     must not be greater than min(N,P), where P is the same as in the
     calling sequence to DQRDC.
 
-    Input, double QRAUX[P], the auxiliary output from DQRDC.
+    Input, float QRAUX[P], the auxiliary output from DQRDC.
 
-    Input, double Y[N], a vector to be manipulated by DQRSL.
+    Input, float Y[N], a vector to be manipulated by DQRSL.
 
-    Output, double QY[N], contains Q * Y, if requested.
+    Output, float QY[N], contains Q * Y, if requested.
 
-    Output, double QTY[N], contains Q' * Y, if requested.
+    Output, float QTY[N], contains Q' * Y, if requested.
 
-    Output, double B[K], the solution of the least squares problem
+    Output, float B[K], the solution of the least squares problem
       minimize norm2 ( Y - AK * B),
     if its computation has been requested.  Note that if pivoting was
     requested in DQRDC, the J-th component of B will be associated with
     column JPVT(J) of the original matrix A that was input into DQRDC.
 
-    Output, double RSD[N], the least squares residual Y - AK * B,
+    Output, float RSD[N], the least squares residual Y - AK * B,
     if its computation has been requested.  RSD is also the orthogonal
     projection of Y onto the orthogonal complement of the column space
     of AK.
 
-    Output, double AB[N], the least squares approximation Ak * B,
+    Output, float AB[N], the least squares approximation Ak * B,
     if its computation has been requested.  AB is also the orthogonal
     projection of Y onto the column space of A.
 
   int j;
   int jj;
   int ju;
-  double t;
-  double temp;
+  float t;
+  float temp;
   /**
     Set INFO flag.
   */
 
 /******************************************************************************/
 
-void dscal(int n, double sa, double x[], int incx)
+void dscal(int n, float sa, float x[], int incx)
 
 /******************************************************************************/
 /**
 
     Input, int N, the number of entries in the vector.
 
-    Input, double SA, the multiplier.
+    Input, float SA, the multiplier.
 
-    Input/output, double X[*], the vector to be scaled.
+    Input/output, float X[*], the vector to be scaled.
 
     Input, int INCX, the increment between successive entries of X.
 */
 /******************************************************************************/
 
 
-void dswap(int n, double x[], int incx, double y[], int incy)
+void dswap(int n, float x[], int incx, float y[], int incy)
 
 /******************************************************************************/
 /**
 
     Input, int N, the number of entries in the vectors.
 
-    Input/output, double X[*], one of the vectors to swap.
+    Input/output, float X[*], one of the vectors to swap.
 
     Input, int INCX, the increment between successive entries of X.
 
-    Input/output, double Y[*], one of the vectors to swap.
+    Input/output, float Y[*], one of the vectors to swap.
 
     Input, int INCY, the increment between successive elements of Y.
 */
   if (n <= 0) return;
 
   int i, ix, iy, m;
-  double temp;
+  float temp;
 
   if (incx == 1 && incy == 1) {
     m = n % 3;
 
 /******************************************************************************/
 
-void qr_solve(double x[], int m, int n, double a[], double b[])
+void qr_solve(float x[], int m, int n, float a[], float b[])
 
 /******************************************************************************/
 /**
 
     Input, int N, the number of columns of A.
 
-    Input, double A[M*N], the matrix.
+    Input, float A[M*N], the matrix.
 
-    Input, double B[M], the right hand side.
+    Input, float B[M], the right hand side.
 
-    Output, double QR_SOLVE[N], the least squares solution.
+    Output, float QR_SOLVE[N], the least squares solution.
 */
 {
-  double a_qr[n * m], qraux[n], r[m], tol;
+  float a_qr[n * m], qraux[n], r[m], tol;
   int ind, itask, jpvt[n], kr, lda;
 
   r8mat_copy(a_qr, m, n, a);

Marlin/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,
-            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);
-void qr_solve(double x[], int m, int n, double a[], double b[]);
+void daxpy(int n, float da, float dx[], int incx, float dy[], int incy);
+float ddot(int n, float dx[], int incx, float dy[], int incy);
+float dnrm2(int n, float x[], int incx);
+void dqrank(float a[], int lda, int m, int n, float tol, int* kr,
+            int jpvt[], float qraux[]);
+void dqrdc(float a[], int lda, int n, int p, float qraux[], int jpvt[],
+           float work[], int job);
+int dqrls(float a[], int lda, int m, int n, float tol, int* kr, float b[],
+          float x[], float rsd[], int jpvt[], float qraux[], int itask);
+void dqrlss(float a[], int lda, int m, int n, int kr, float b[], float x[],
+            float rsd[], int jpvt[], float qraux[]);
+int dqrsl(float a[], int lda, int n, int k, float qraux[], float y[],
+          float qy[], float qty[], float b[], float rsd[], float ab[], int job);
+void dscal(int n, float sa, float x[], int incx);
+void dswap(int n, float x[], int incx, float y[], int incy);
+void qr_solve(float x[], int m, int n, float a[], float b[]);
 
 #endif