diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index dce5010aa2..63115ff7c5 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -3546,16 +3546,16 @@ inline void gcode_G28() { * 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++) { @@ -3581,10 +3581,14 @@ inline void gcode_G28() { 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); @@ -3596,7 +3600,6 @@ inline void gcode_G28() { 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) @@ -3604,8 +3607,6 @@ inline void gcode_G28() { #endif - probePointCounter++; - idle(); } //xProbe @@ -3664,7 +3665,7 @@ inline void gcode_G28() { // 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; diff --git a/Marlin/qr_solve.cpp b/Marlin/qr_solve.cpp index 0fadd67063..9404f8ee5e 100644 --- a/Marlin/qr_solve.cpp +++ b/Marlin/qr_solve.cpp @@ -59,7 +59,7 @@ int i4_min(int i1, int i2) return (i1 < i2) ? i1 : i2; } -double r8_epsilon(void) +float r8_epsilon(void) /******************************************************************************/ /** @@ -89,14 +89,14 @@ double 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) /******************************************************************************/ /** @@ -118,15 +118,15 @@ double r8_max(double x, double 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) /******************************************************************************/ /** @@ -148,15 +148,15 @@ double r8_abs(double 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) /******************************************************************************/ /** @@ -178,15 +178,15 @@ double r8_sign(double 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[]) /******************************************************************************/ /** @@ -217,12 +217,12 @@ double r8mat_amax(int m, int n, double 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])); @@ -231,7 +231,7 @@ double r8mat_amax(int m, int n, double a[]) return value; } -void r8mat_copy(double a2[], int m, int n, double a1[]) +void r8mat_copy(float a2[], int m, int n, float a1[]) /******************************************************************************/ /** @@ -260,9 +260,9 @@ void r8mat_copy(double a2[], int m, int n, double 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++) { @@ -273,7 +273,7 @@ void r8mat_copy(double a2[], int m, int n, double a1[]) /******************************************************************************/ -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) /******************************************************************************/ /** @@ -313,13 +313,13 @@ void daxpy(int n, double da, double dx[], int incx, double 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. @@ -364,7 +364,7 @@ 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) +float ddot(int n, float dx[], int incx, float dy[], int incy) /******************************************************************************/ /** @@ -404,15 +404,15 @@ double ddot(int n, double dx[], int incx, double 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. */ { @@ -420,7 +420,7 @@ double ddot(int n, double dx[], int incx, double dy[], int incy) if (n <= 0) return 0.0; int i, m; - double dtemp = 0.0; + float dtemp = 0.0; /** Code for unequal increments or equal increments @@ -454,7 +454,7 @@ double ddot(int n, double dx[], int incx, double dy[], int incy) } /******************************************************************************/ -double dnrm2(int n, double x[], int incx) +float dnrm2(int n, float x[], int incx) /******************************************************************************/ /** @@ -494,24 +494,24 @@ double dnrm2(int n, double 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; @@ -527,8 +527,8 @@ 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 dqrank(float a[], int lda, int m, int n, float tol, int* kr, + int jpvt[], float qraux[]) /******************************************************************************/ /** @@ -572,7 +572,7 @@ void dqrank(double a[], int lda, int m, int n, double tol, int* kr, 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 @@ -585,7 +585,7 @@ void dqrank(double a[], int lda, int m, int n, double tol, int* kr, 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 @@ -598,11 +598,11 @@ void dqrank(double a[], int lda, int m, int n, double tol, int* kr, 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; @@ -621,8 +621,8 @@ void dqrank(double a[], int lda, int m, int n, double tol, int* kr, } /******************************************************************************/ -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) /******************************************************************************/ /** @@ -660,7 +660,7 @@ void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[], 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 @@ -676,7 +676,7 @@ void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[], 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 @@ -695,7 +695,7 @@ void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[], 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. @@ -706,7 +706,7 @@ void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[], int j; int lup; int maxj; - double maxnrm, nrmxl, t, tt; + float maxnrm, nrmxl, t, tt; int pl = 1, pu = 0; /** @@ -815,8 +815,8 @@ void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[], } /******************************************************************************/ -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) /******************************************************************************/ /** @@ -871,7 +871,7 @@ int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[], 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. @@ -886,7 +886,7 @@ int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[], 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 @@ -894,12 +894,12 @@ int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[], 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. @@ -909,7 +909,7 @@ int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[], 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. @@ -962,8 +962,8 @@ int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[], } /******************************************************************************/ -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[]) /******************************************************************************/ /** @@ -1004,7 +1004,7 @@ void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[], 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 @@ -1019,12 +1019,12 @@ void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[], 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. @@ -1032,7 +1032,7 @@ void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[], 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. */ { @@ -1041,7 +1041,7 @@ void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[], int j; int job; int k; - double t; + float t; if (kr != 0) { job = 110; @@ -1071,8 +1071,8 @@ void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[], } /******************************************************************************/ -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) /******************************************************************************/ /** @@ -1158,7 +1158,7 @@ int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[], 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. @@ -1169,26 +1169,26 @@ int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[], 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. @@ -1220,8 +1220,8 @@ int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[], int j; int jj; int ju; - double t; - double temp; + float t; + float temp; /** Set INFO flag. */ @@ -1366,7 +1366,7 @@ int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[], /******************************************************************************/ -void dscal(int n, double sa, double x[], int incx) +void dscal(int n, float sa, float x[], int incx) /******************************************************************************/ /** @@ -1402,9 +1402,9 @@ void dscal(int n, double sa, double 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. */ @@ -1441,7 +1441,7 @@ void dscal(int n, double sa, double x[], int incx) /******************************************************************************/ -void dswap(int n, double x[], int incx, double y[], int incy) +void dswap(int n, float x[], int incx, float y[], int incy) /******************************************************************************/ /** @@ -1477,11 +1477,11 @@ void dswap(int n, double x[], int incx, double 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. */ @@ -1489,7 +1489,7 @@ void dswap(int n, double x[], int incx, double y[], int incy) if (n <= 0) return; int i, ix, iy, m; - double temp; + float temp; if (incx == 1 && incy == 1) { m = n % 3; @@ -1526,7 +1526,7 @@ 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 qr_solve(float x[], int m, int n, float a[], float b[]) /******************************************************************************/ /** @@ -1569,14 +1569,14 @@ void qr_solve(double x[], int m, int n, double a[], double 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); diff --git a/Marlin/qr_solve.h b/Marlin/qr_solve.h index 3ea30e9e7d..a83509db78 100644 --- a/Marlin/qr_solve.h +++ b/Marlin/qr_solve.h @@ -24,21 +24,21 @@ #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