mesh_bed_leveling.cpp 6.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179
  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. for (int y = 0; y < MESH_NUM_Y_POINTS; y++)
  10. for (int x = 0; x < MESH_NUM_X_POINTS; x++)
  11. z_values[y][x] = 0;
  12. }
  13. static inline bool vec_undef(const float v[2])
  14. {
  15. const uint32_t *vx = (const uint32_t*)v;
  16. return vx[0] == 0x0FFFFFFFF || vx[1] == 0x0FFFFFFFF;
  17. }
  18. void mesh_bed_leveling::get_meas_xy(int ix, int iy, float &x, float &y, bool use_default)
  19. {
  20. #if 0
  21. float cntr[2] = {
  22. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0)),
  23. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4))
  24. };
  25. float vec_x[2] = {
  26. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0)),
  27. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4))
  28. };
  29. float vec_y[2] = {
  30. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0)),
  31. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4))
  32. };
  33. if (use_default || vec_undef(cntr) || vec_undef(vec_x) || vec_undef(vec_y)) {
  34. // Default, uncorrected positions of the calibration points. Works well for correctly built printers.
  35. x = float(MESH_MIN_X) + float(MEAS_NUM_X_DIST) * float(ix) - X_PROBE_OFFSET_FROM_EXTRUDER;
  36. //FIXME
  37. //x -= 5.f;
  38. y = float(MESH_MIN_Y) + float(MEAS_NUM_Y_DIST) * float(iy) - Y_PROBE_OFFSET_FROM_EXTRUDER;
  39. } else {
  40. #if 0
  41. SERIAL_ECHO("Running bed leveling. Calibration data: ");
  42. SERIAL_ECHO(cntr[0]);
  43. SERIAL_ECHO(",");
  44. SERIAL_ECHO(cntr[1]);
  45. SERIAL_ECHO(", x: ");
  46. SERIAL_ECHO(vec_x[0]);
  47. SERIAL_ECHO(",");
  48. SERIAL_ECHO(vec_x[1]);
  49. SERIAL_ECHO(", y: ");
  50. SERIAL_ECHO(vec_y[0]);
  51. SERIAL_ECHO(",");
  52. SERIAL_ECHO(vec_y[1]);
  53. SERIAL_ECHOLN("");
  54. #endif
  55. x = cntr[0];
  56. y = cntr[1];
  57. if (ix < 1) {
  58. x -= vec_x[0];
  59. y -= vec_x[1];
  60. } else if (ix > 1) {
  61. x += vec_x[0];
  62. y += vec_x[1];
  63. }
  64. if (iy < 1) {
  65. x -= vec_y[0];
  66. y -= vec_y[1];
  67. } else if (iy > 1) {
  68. x += vec_y[0];
  69. y += vec_y[1];
  70. }
  71. #if 0
  72. SERIAL_ECHO("Calibration point position: ");
  73. SERIAL_ECHO(x);
  74. SERIAL_ECHO(",");
  75. SERIAL_ECHO(y);
  76. SERIAL_ECHOLN("");
  77. #endif
  78. }
  79. #else
  80. // Default, uncorrected positions of the calibration points.
  81. // This coordinate will be corrected by the planner.
  82. x = pgm_read_float(bed_ref_points + 2 * (iy * 3 + ix));
  83. y = pgm_read_float(bed_ref_points + 2 * (iy * 3 + ix) + 1);
  84. #endif
  85. }
  86. #if MESH_NUM_X_POINTS>=5 && MESH_NUM_Y_POINTS>=5 && (MESH_NUM_X_POINTS&1)==1 && (MESH_NUM_Y_POINTS&1)==1
  87. // Works for an odd number of MESH_NUM_X_POINTS and MESH_NUM_Y_POINTS
  88. // #define MBL_BILINEAR
  89. void mesh_bed_leveling::upsample_3x3()
  90. {
  91. int idx0 = 0;
  92. int idx1 = MESH_NUM_X_POINTS / 2;
  93. int idx2 = MESH_NUM_X_POINTS - 1;
  94. {
  95. // First interpolate the points in X axis.
  96. static const float x0 = MESH_MIN_X;
  97. static const float x1 = 0.5f * float(MESH_MIN_X + MESH_MAX_X);
  98. static const float x2 = MESH_MAX_X;
  99. for (int j = 0; j < 3; ++ j) {
  100. // 1) Copy the source points to their new destination.
  101. z_values[j][idx2] = z_values[j][2];
  102. z_values[j][idx1] = z_values[j][1];
  103. // 2) Interpolate the remaining values by Largrangian polynomials.
  104. for (int i = idx0 + 1; i < idx2; ++ i) {
  105. if (i == idx1)
  106. continue;
  107. float x = get_x(i);
  108. #ifdef MBL_BILINEAR
  109. z_values[j][i] = (x < x1) ?
  110. ((z_values[j][idx0] * (x - x0) + z_values[j][idx1] * (x1 - x)) / (x1 - x0)) :
  111. ((z_values[j][idx1] * (x - x1) + z_values[j][idx2] * (x2 - x)) / (x2 - x1));
  112. #else
  113. z_values[j][i] =
  114. z_values[j][idx0] * (x - x1) * (x - x2) / ((x0 - x1) * (x0 - x2)) +
  115. z_values[j][idx1] * (x - x0) * (x - x2) / ((x1 - x0) * (x1 - x2)) +
  116. z_values[j][idx2] * (x - x0) * (x - x1) / ((x2 - x0) * (x2 - x1));
  117. #endif
  118. }
  119. }
  120. }
  121. {
  122. // Second interpolate the points in Y axis.
  123. static const float y0 = MESH_MIN_Y;
  124. static const float y1 = 0.5f * float(MESH_MIN_Y + MESH_MAX_Y);
  125. static const float y2 = MESH_MAX_Y;
  126. for (int i = 0; i < MESH_NUM_X_POINTS; ++ i) {
  127. // 1) Copy the intermediate points to their new destination.
  128. z_values[idx2][i] = z_values[2][i];
  129. z_values[idx1][i] = z_values[1][i];
  130. // 2) Interpolate the remaining values by Largrangian polynomials.
  131. for (int j = 1; j + 1 < MESH_NUM_Y_POINTS; ++ j) {
  132. if (j == idx1)
  133. continue;
  134. float y = get_y(j);
  135. #ifdef MBL_BILINEAR
  136. z_values[j][i] = (y < y1) ?
  137. ((z_values[idx0][i] * (y - y0) + z_values[idx1][i] * (y1 - y)) / (y1 - y0)) :
  138. ((z_values[idx1][i] * (y - y1) + z_values[idx2][i] * (y2 - y)) / (y2 - y1));
  139. #else
  140. z_values[j][i] =
  141. z_values[idx0][i] * (y - y1) * (y - y2) / ((y0 - y1) * (y0 - y2)) +
  142. z_values[idx1][i] * (y - y0) * (y - y2) / ((y1 - y0) * (y1 - y2)) +
  143. z_values[idx2][i] * (y - y0) * (y - y1) / ((y2 - y0) * (y2 - y1));
  144. #endif
  145. }
  146. }
  147. }
  148. /*
  149. // Relax the non-measured points.
  150. const float weight = 0.2f;
  151. for (uint8_t iter = 0; iter < 20; ++ iter) {
  152. for (int8_t j = 1; j < 6; ++ j) {
  153. for (int8_t i = 1; i < 6; ++ i) {
  154. if (i == 3 || j == 3)
  155. continue;
  156. if ((i % 3) == 0 && (j % 3) == 0)
  157. continue;
  158. float avg = 0.25f * (z_values[j][i-1]+z_values[j][i+1]+z_values[j-1][i]+z_values[j+1][i]);
  159. z_values[j][i] = (1.f-weight)*z_values[j][i] + weight*avg;
  160. }
  161. }
  162. }
  163. */
  164. }
  165. #endif
  166. #endif // MESH_BED_LEVELING