microlab
/
scube
1
1
Fork 0
Code Issues Pull Requests Releases Activity

Patch G29 for linear leveling, reachable with probe

Browse Source
2.0.x
Scott Lahteine 8 years ago
parent b800eb0fed
commit e242946ac3
3 changed files with 117 additions and 116 deletions
Show Stats Download Patch File Download Diff File

29
Marlin/Marlin_main.cpp
Unescape Escape View File

@ -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;

172
Marlin/qr_solve.cpp
Unescape Escape View File

@ -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);

32
Marlin/qr_solve.h
Unescape Escape View File

@ -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

Write Preview
Loading…
Cancel
Save