d467e97679
Also... The memory corruption issue may be fixed. The GCC compiler was inlining static functions and this caused the G29() stack frame to become much larger than the AVR could handle.
96 lines
3.1 KiB
C++
96 lines
3.1 KiB
C++
/**
|
|
* Marlin 3D Printer Firmware
|
|
* Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
|
|
*
|
|
* Based on Sprinter and grbl.
|
|
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
|
|
*
|
|
* 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/>.
|
|
*
|
|
*/
|
|
|
|
/**
|
|
* Least Squares Best Fit By Roxy and Ed Williams
|
|
*
|
|
* This algorithm is high speed and has a very small code footprint.
|
|
* Its results are identical to both the Iterative Least-Squares published
|
|
* earlier by Roxy and the QR_SOLVE solution. If used in place of QR_SOLVE
|
|
* it saves roughly 10K of program memory. It also does not require all of
|
|
* coordinates to be present during the calculations. Each point can be
|
|
* probed and then discarded.
|
|
*
|
|
*/
|
|
|
|
#include "MarlinConfig.h"
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_UBL) // Currently only used by UBL, but is applicable to Grid Based (Linear) Bed Leveling
|
|
|
|
#include "macros.h"
|
|
#include <math.h>
|
|
|
|
#include "least_squares_fit.h"
|
|
|
|
void incremental_LSF_reset(struct linear_fit_data *lsf) {
|
|
lsf->n = 0;
|
|
lsf->A = 0.0; // probably a memset() can be done to zero
|
|
lsf->B = 0.0; // this whole structure
|
|
lsf->D = 0.0;
|
|
lsf->xbar = lsf->ybar = lsf->zbar = 0.0;
|
|
lsf->x2bar = lsf->y2bar = lsf->z2bar = 0.0;
|
|
lsf->xybar = lsf->xzbar = lsf->yzbar = 0.0;
|
|
lsf->max_absx = lsf->max_absy = 0.0;
|
|
}
|
|
|
|
void incremental_LSF(struct linear_fit_data *lsf, float x, float y, float z) {
|
|
lsf->xbar += x;
|
|
lsf->ybar += y;
|
|
lsf->zbar += z;
|
|
lsf->x2bar += x*x;
|
|
lsf->y2bar += y*y;
|
|
lsf->z2bar += z*z;
|
|
lsf->xybar += x*y;
|
|
lsf->xzbar += x*z;
|
|
lsf->yzbar += y*z;
|
|
lsf->max_absx = (fabs(x) > lsf->max_absx) ? fabs(x) : lsf->max_absx;
|
|
lsf->max_absy = (fabs(y) > lsf->max_absy) ? fabs(y) : lsf->max_absy;
|
|
lsf->n++;
|
|
return;
|
|
}
|
|
|
|
int finish_incremental_LSF(struct linear_fit_data *lsf) {
|
|
float DD, N;
|
|
|
|
N = (float) lsf->n;
|
|
lsf->xbar /= N;
|
|
lsf->ybar /= N;
|
|
lsf->zbar /= N;
|
|
lsf->x2bar = lsf->x2bar/N - lsf->xbar*lsf->xbar;
|
|
lsf->y2bar = lsf->y2bar/N - lsf->ybar*lsf->ybar;
|
|
lsf->z2bar = lsf->z2bar/N - lsf->zbar*lsf->zbar;
|
|
lsf->xybar = lsf->xybar/N - lsf->xbar*lsf->ybar;
|
|
lsf->yzbar = lsf->yzbar/N - lsf->ybar*lsf->zbar;
|
|
lsf->xzbar = lsf->xzbar/N - lsf->xbar*lsf->zbar;
|
|
|
|
DD = lsf->x2bar*lsf->y2bar - lsf->xybar*lsf->xybar;
|
|
if (fabs(DD) <= 1e-10*(lsf->max_absx+lsf->max_absy))
|
|
return -1;
|
|
|
|
lsf->A = (lsf->yzbar*lsf->xybar - lsf->xzbar*lsf->y2bar) / DD;
|
|
lsf->B = (lsf->xzbar*lsf->xybar - lsf->yzbar*lsf->x2bar) / DD;
|
|
lsf->D = -(lsf->zbar + lsf->A*lsf->xbar + lsf->B*lsf->ybar);
|
|
return 0;
|
|
}
|
|
#endif
|
|
|
|
|