Prusa-Firmware/Firmware/mesh_bed_leveling.h

#include "Marlin.h"

#include "float.h"

#ifdef MESH_BED_LEVELING

#define MEAS_NUM_X_DIST (float(MESH_MAX_X - MESH_MIN_X)/float(MESH_MEAS_NUM_X_POINTS - 1))
#define MEAS_NUM_Y_DIST (float(MESH_MAX_Y - MESH_MIN_Y)/float(MESH_MEAS_NUM_Y_POINTS - 1))

#define MESH_X_DIST (float(MESH_MAX_X - MESH_MIN_X)/float(MESH_NUM_X_POINTS - 1))
#define MESH_Y_DIST (float(MESH_MAX_Y - MESH_MIN_Y)/float(MESH_NUM_Y_POINTS - 1))

class mesh_bed_leveling {
public:
    uint8_t active;
    float z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS];

#ifdef ENABLE_LEVELING_FADE_HEIGHT
    // Average Z height of z_values.
    float z_avg;
    // Z height at which to fade to zero.
    float z_fade;
    // Previous z, at which fade_factor was calculated.
    float z_prev;
    // Fade factor, calculated for z_prev.
    float fade_factor;
#endif
    
    mesh_bed_leveling();
    
    void reset();

#ifdef ENABLE_LEVELING_FADE_HEIGHT
    void set_z_fade_height(float z) { z_fade = (z > 0.f) ? z : 0.f; z_prev = - FLT_MAX; }
#endif
    
#if MESH_NUM_X_POINTS>=5 && MESH_NUM_Y_POINTS>=5 && (MESH_NUM_X_POINTS&1)==1 && (MESH_NUM_Y_POINTS&1)==1
    void upsample_3x3();
#endif
    
    static float get_x(int i) { return float(MESH_MIN_X) + float(MESH_X_DIST) * float(i); }
    static float get_y(int i) { return float(MESH_MIN_Y) + float(MESH_Y_DIST) * float(i); }
    
    // Measurement point for the Z probe.
    // If use_default=true, then the default positions for a correctly built printer are used.
    // Otherwise a correction matrix is pulled from the EEPROM if available.
    static void get_meas_xy(int ix, int iy, float &x, float &y, bool use_default);
    
    void set_z(int ix, int iy, float z) { z_values[iy][ix] = z; }
    
    int select_x_index(float x) {
        int i = 1;
        while (x > get_x(i) && i < MESH_NUM_X_POINTS - 1) i++;
        return i - 1;
    }
    
    int select_y_index(float y) {
        int i = 1;
        while (y > get_y(i) && i < MESH_NUM_Y_POINTS - 1) i++;
        return i - 1;
    }
    
    float get_z(float x, float y, float z) {
        int   i, j;
        float s, t;

#ifdef ENABLE_LEVELING_FADE_HEIGHT
        if (z != z_prev) {
            if (z > z_fade || z_fade == 0.)
                fade_factor = 0.f;
            else if (z <= 0.f)
                fade_factor = 1.f;
            else {
                fade_factor = (z_fade - z) / z_fade;
            }
#if 0
            SERIAL_ECHOPGM("fading z: ");
            MYSERIAL.print(z);
            SERIAL_ECHOPGM(", fade: ");
            MYSERIAL.print(z_fade);
            SERIAL_ECHOLNPGM(", fade_factor: ");
            MYSERIAL.print(fade_factor);
            SERIAL_ECHOLNPGM("");
#endif
            z_prev = z;
        }
        if (fade_factor == 0.f)
            return z_avg;
#endif
        
#if MESH_NUM_X_POINTS==3 && MESH_NUM_Y_POINTS==3
#define MESH_MID_X (0.5f*(MESH_MIN_X+MESH_MAX_X))
#define MESH_MID_Y (0.5f*(MESH_MIN_Y+MESH_MAX_Y))
        if (x < MESH_MID_X) {
            i = 0;
            s = (x - MESH_MIN_X) / MESH_X_DIST;
            if (s > 1.f)
                s = 1.f;
        } else {
            i = 1;
            s = (x - MESH_MID_X) / MESH_X_DIST;
            if (s < 0)
                s = 0;
        }
        if (y < MESH_MID_Y) {
            j = 0;
            t = (y - MESH_MIN_Y) / MESH_Y_DIST;
            if (t > 1.f)
                t = 1.f;
        } else {
            j = 1;
            t = (y - MESH_MID_Y) / MESH_Y_DIST;
            if (t < 0)
                t = 0;
        }
#else
        i = int(floor((x - MESH_MIN_X) / MESH_X_DIST));
        if (i < 0) {
            i = 0;
            s = (x - MESH_MIN_X) / MESH_X_DIST;
            if (s > 1.f)
                s = 1.f;
        }
        else if (i > MESH_NUM_X_POINTS - 2) {
            i = MESH_NUM_X_POINTS - 2;
            s = (x - get_x(i)) / MESH_X_DIST;
            if (s < 0)
                s = 0;
        } else {
            s = (x - get_x(i)) / MESH_X_DIST;
            if (s < 0)
                s = 0;
            else if (s > 1.f)
                s = 1.f;
        }
        j = int(floor((y - MESH_MIN_Y) / MESH_Y_DIST));
        if (j < 0) {
            j = 0;
            t = (y - MESH_MIN_Y) / MESH_Y_DIST;
            if (t > 1.f)
                t = 1.f;
        } else if (j > MESH_NUM_Y_POINTS - 2) {
            j = MESH_NUM_Y_POINTS - 2;
            t = (y - get_y(j)) / MESH_Y_DIST;
            if (t < 0)
                t = 0;
        } else {
            t = (y - get_y(j)) / MESH_Y_DIST;
            if (t < 0)
                t = 0;
            else if (t > 1.f)
                t = 1.f;
        }
#endif /* MESH_NUM_X_POINTS==3 && MESH_NUM_Y_POINTS==3 */
        
        float si = 1.f-s;
        float z0 = si * z_values[j  ][i] + s * z_values[j  ][i+1];
        float z1 = si * z_values[j+1][i] + s * z_values[j+1][i+1];
        return 
#ifdef ENABLE_LEVELING_FADE_HEIGHT
            z_avg + fade_factor * ((1.f-t) * z0 + t * z1 - z_avg);
#else
            ((1.f-t) * z0 + t * z1);
#endif
    }
    
};

extern mesh_bed_leveling mbl;

#endif  // MESH_BED_LEVELING