mesh_bed_leveling.cpp 4.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105
  1. #include "mesh_bed_leveling.h"
  2. #include "mesh_bed_calibration.h"
  3. #include "Configuration.h"
  4. #ifdef MESH_BED_LEVELING
  5. mesh_bed_leveling mbl;
  6. mesh_bed_leveling::mesh_bed_leveling() { reset(); }
  7. void mesh_bed_leveling::reset() {
  8. active = 0;
  9. memset(z_values, 0, sizeof(float) * MESH_NUM_X_POINTS * MESH_NUM_Y_POINTS);
  10. }
  11. static inline bool vec_undef(const float v[2])
  12. {
  13. const uint32_t *vx = (const uint32_t*)v;
  14. return vx[0] == 0x0FFFFFFFF || vx[1] == 0x0FFFFFFFF;
  15. }
  16. #if MESH_NUM_X_POINTS>=5 && MESH_NUM_Y_POINTS>=5 && (MESH_NUM_X_POINTS&1)==1 && (MESH_NUM_Y_POINTS&1)==1
  17. // Works for an odd number of MESH_NUM_X_POINTS and MESH_NUM_Y_POINTS
  18. // #define MBL_BILINEAR
  19. void mesh_bed_leveling::upsample_3x3()
  20. {
  21. int idx0 = 0;
  22. int idx1 = MESH_NUM_X_POINTS / 2;
  23. int idx2 = MESH_NUM_X_POINTS - 1;
  24. {
  25. // First interpolate the points in X axis.
  26. static const float x0 = MESH_MIN_X;
  27. static const float x1 = 0.5f * float(MESH_MIN_X + MESH_MAX_X);
  28. static const float x2 = MESH_MAX_X;
  29. for (int j = 0; j < 3; ++ j) {
  30. // 1) Copy the source points to their new destination.
  31. z_values[j][idx2] = z_values[j][2];
  32. z_values[j][idx1] = z_values[j][1];
  33. // 2) Interpolate the remaining values by Largrangian polynomials.
  34. for (int i = idx0 + 1; i < idx2; ++ i) {
  35. if (i == idx1)
  36. continue;
  37. float x = get_x(i);
  38. #ifdef MBL_BILINEAR
  39. z_values[j][i] = (x < x1) ?
  40. ((z_values[j][idx0] * (x - x0) + z_values[j][idx1] * (x1 - x)) / (x1 - x0)) :
  41. ((z_values[j][idx1] * (x - x1) + z_values[j][idx2] * (x2 - x)) / (x2 - x1));
  42. #else
  43. z_values[j][i] =
  44. z_values[j][idx0] * (x - x1) * (x - x2) / ((x0 - x1) * (x0 - x2)) +
  45. z_values[j][idx1] * (x - x0) * (x - x2) / ((x1 - x0) * (x1 - x2)) +
  46. z_values[j][idx2] * (x - x0) * (x - x1) / ((x2 - x0) * (x2 - x1));
  47. #endif
  48. }
  49. }
  50. }
  51. {
  52. // Second interpolate the points in Y axis.
  53. static const float y0 = MESH_MIN_Y;
  54. static const float y1 = 0.5f * float(MESH_MIN_Y + MESH_MAX_Y);
  55. static const float y2 = MESH_MAX_Y;
  56. for (int i = 0; i < MESH_NUM_X_POINTS; ++ i) {
  57. // 1) Copy the intermediate points to their new destination.
  58. z_values[idx2][i] = z_values[2][i];
  59. z_values[idx1][i] = z_values[1][i];
  60. // 2) Interpolate the remaining values by Largrangian polynomials.
  61. for (int j = 1; j + 1 < MESH_NUM_Y_POINTS; ++ j) {
  62. if (j == idx1)
  63. continue;
  64. float y = get_y(j);
  65. #ifdef MBL_BILINEAR
  66. z_values[j][i] = (y < y1) ?
  67. ((z_values[idx0][i] * (y - y0) + z_values[idx1][i] * (y1 - y)) / (y1 - y0)) :
  68. ((z_values[idx1][i] * (y - y1) + z_values[idx2][i] * (y2 - y)) / (y2 - y1));
  69. #else
  70. z_values[j][i] =
  71. z_values[idx0][i] * (y - y1) * (y - y2) / ((y0 - y1) * (y0 - y2)) +
  72. z_values[idx1][i] * (y - y0) * (y - y2) / ((y1 - y0) * (y1 - y2)) +
  73. z_values[idx2][i] * (y - y0) * (y - y1) / ((y2 - y0) * (y2 - y1));
  74. #endif
  75. }
  76. }
  77. }
  78. /*
  79. // Relax the non-measured points.
  80. const float weight = 0.2f;
  81. for (uint8_t iter = 0; iter < 20; ++ iter) {
  82. for (int8_t j = 1; j < 6; ++ j) {
  83. for (int8_t i = 1; i < 6; ++ i) {
  84. if (i == 3 || j == 3)
  85. continue;
  86. if ((i % 3) == 0 && (j % 3) == 0)
  87. continue;
  88. float avg = 0.25f * (z_values[j][i-1]+z_values[j][i+1]+z_values[j-1][i]+z_values[j+1][i]);
  89. z_values[j][i] = (1.f-weight)*z_values[j][i] + weight*avg;
  90. }
  91. }
  92. }
  93. */
  94. }
  95. #endif
  96. #endif // MESH_BED_LEVELING