Added bi-linear interpolation for getting Z coordinate at any location in the grid.

CMakeLists.txt

@@ -24,6 +24,7 @@ set_property(TARGET glfw glfw_objects nanogui PROPERTY FOLDER "dependencies")
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_VISIBILITY_PRESET hidden)
 
+set(TESTS grbl_test.cpp heightmap_test.cpp)
 
-add_executable(sender main.cpp grbl.h grbl.cpp grbl_test.cpp grbl_communication.h grbl_communication.cpp grbl_machine.h grbl_machine.cpp string_utils.h render.h render.cpp)
+add_executable(sender main.cpp grbl.h grbl.cpp ${TESTS} grbl_communication.h grbl_communication.cpp grbl_machine.h grbl_machine.cpp string_utils.h render.h render.cpp heightmap.h heightmap.cpp)
 target_link_libraries(sender nanogui GL gtest gtest_main)

docs/todo.txt

@@ -1,40 +1,44 @@
 Heightmap probing:
     1. define the grid:
-        - from: x, y
+DONE        - from: x, y
-        - to: x, y
+DONE        - to: x, y
-        - step every: 5mm?
+DONE        - step every: 5mm?
         - clearance height: Z1.5
         - start probing at: Z0.5
         - max negative z: Z-0.5 (when to fail probing)
         - z final safety height: Z15
 
-        - from and to can be filled from current project bounding box
+DONE        - from and to can be filled from current project bounding box
-
+DONE    2. probing
-    2. probing
+DONE        - machine moves at grid start position (x, y)
-        - machine moves at grid start position (x, y)
+DONE            G0X0Y0
-            G0X0Y0
+DONE        - gets down for initial probing
-        - gets down for initial probing
+DONE            G38.2 Z-65 F100
-            G38.2 Z-65 F100
+DONE        - step back a bit (1mm, relative)
-        - step back a bit (1mm, relative)
+DONE            G91 G0 Z1
-            G91 G0 Z1
+DONE        - seek again with lower feed rate
-        - seek again with lower feed rate
+DONE            G38.2 Z-5 F5
-            G38.2 Z-5 F5
+DONE        - after first probe ever, this Z becomes reference 0
-        - after first probe ever, this Z becomes reference 0
+DONE            G10 L20 P0 Z0
-            G10 L20 P0 Z0
+DONE        - all subsequent probes will be relative to this
-
+DONE        - for each probing location:
-        - all subsequent probes will be relative to this
+DONE            - G0 Z"clearance height"
-        - for each probing location:
+DONE            - G0 XxxYyyy (next point location)
-            - G0 Z"clearance height"
+DONE            - G0 Z"start probing at"
-            - G0 XxxYyyy (next point location)
+DONE            - G38.2 Z-5 F5
-            - G0 Z"start probing at"
+DONE            - store Z offset and place it in the heightmap
-            - G38.2 Z-5 F5
-            - store Z offset and place it in the heightmap
 
         3. modify loaded program with heightmap data
         - foreach line in program
             - if line contains a Z coordinate, update the Z according to the X and Y
             - if line does not contain a Z coordinate, add a Z coordinate according to the X and Y
 
+DONE -Let's get rid of the pesky inter-thread communication issue which is causing a lot of confusion.
+DONE - Solve bug in which the probing data does not get rendered properly as it gets probed.
+
+Bug: query coordinate systems after first z probe.
+Refactor: see if we can unify machine state line handling and remove duplication
+
 Edge finding
     - prerequisites
         - which edge to find? -X/+X/-Y/+Y
@@ -50,8 +54,9 @@ Render arcs
 Render quadratic splines
 Render bezier splines
 Synchronize executing program with render (show executed paths with different colors)
-Show a progress bar when executing programs.
 Add aggressive buffer execution policy and make it selectable.
+Show a progress bar when executing programs.
+Show a progress bar when probing.
 
 DONE - Show program extents
 

grbl_machine.cpp

@@ -16,6 +16,7 @@ grbl::machine::machine() {
     states[grbl_machine_state::idle] = new machine_state_idle;
     states[grbl_machine_state::check_program] = new machine_state_check_program;
     states[grbl_machine_state::run_program] = new machine_state_run_program;
+    states[grbl_machine_state::heightmap_probing] = new machine_state_heightmap_probing;
 
     switch_to_state(grbl_machine_state::disconnected);
 }
@@ -559,10 +560,6 @@ void grbl::machine::cancel_jog() const {
     pipe->send_single_char_command(0x85);
 }
 
-void grbl::machine::set_listener(grbl::machine_listener *l) {
-    listener = l;
-}
-
 void grbl::machine::request_unlock() {
     pipe->send("$X");
 }
@@ -614,7 +611,8 @@ void grbl::machine::zero_offset(int which) {
     pipe->send("$#");
     awaiting_responses++;
     while (awaiting_responses > 0);
-    listener->on_parameters_reloaded();
+
+    push_event(std::make_shared<machine_event_parameters_reloaded>());
 }
 
 void grbl::machine::zero_offset_axis(int offset_index, int axis) {
@@ -626,7 +624,8 @@ void grbl::machine::zero_offset_axis(int offset_index, int axis) {
     pipe->send("$#");
     awaiting_responses++;
     while (awaiting_responses > 0);
-    listener->on_parameters_reloaded();
+
+    push_event(std::make_shared<machine_event_parameters_reloaded>());
 }
 
 void grbl::machine::go_to_zero(bool x, bool y, bool z) {
@@ -688,6 +687,29 @@ void grbl::machine::start_z_probe(float min_z, float feed_rate) {
     awaiting_responses++;
 }
 
+void grbl::machine::probe_heightmap(grbl::heightmap& grid) {
+    std::cout << "probing heightmap" << std::endl;
+    dynamic_cast<machine_state_heightmap_probing *>(states[grbl_machine_state::heightmap_probing])->grid = &grid;
+    switch_to_state(grbl_machine_state::heightmap_probing);
+}
+
+void grbl::machine::push_event(std::shared_ptr<machine_event> event) {
+    std::scoped_lock<std::mutex> lock(event_mutex);
+    events.push_back(event);
+}
+
+std::shared_ptr<grbl::machine_event> grbl::machine::pop_event() {
+    // TODO: make this more efficient
+    std::scoped_lock<std::mutex> lock(event_mutex);
+    if (events.empty())
+        return nullptr;
+    else {
+        auto result = events.front();
+        events.pop_front();
+        return result;
+    }
+}
+
 bool grbl::jog_state::no_jogging() const {
     return !(up_pressed || down_pressed || left_pressed || right_pressed || z_up_pressed || z_down_pressed);
 }
@@ -712,7 +734,7 @@ void grbl::machine_state_connect::on_exit(grbl::machine *m) {
 void grbl::machine_state_connect::on_line_received(std::string line) {
 //    std::cerr << "Should not get content while connecting!" << std::endl;
     if (starts_with(line, "Grbl")) {
-        cnc->listener->on_banner(line);
+        cnc->push_event(std::make_shared<machine_event_banner>(line));
         cnc->switch_to_state(grbl_machine_state::init);
     }
 }
@@ -725,7 +747,7 @@ void grbl::machine_state_init::on_enter(grbl::machine *m) {
 }
 
 void grbl::machine_state_init::on_exit(grbl::machine *m) {
-    cnc->listener->on_init_completed();
+    cnc->push_event(std::make_shared<machine_event_init_completed>());
 }
 
 void grbl::machine_state_init::on_line_received(std::string line) {
@@ -801,15 +823,17 @@ void grbl::machine_state_idle::on_line_received(std::string line) {
             cnc->awaiting_responses--;
         }
     } else if (starts_with(line, "Grbl")) {
-        cnc->listener->on_banner(line);
+        cnc->push_event(std::make_shared<machine_event_banner>(line));
         cnc->reset_machine_state();
     } else if (starts_with(line, "<")) {
         cnc->last_report = parse_status_report(line, cnc->last_report);
-        cnc->listener->on_realtime_status_report(cnc->last_report);
+        cnc->push_event(std::make_shared<machine_event_report_received>(cnc->last_report));
     } else if (starts_with(line, "[MSG:")) {
-        cnc->listener->on_message(line.substr(5, line.size() - 6));
+        auto message = line.substr(5, line.size() - 6);
+        cnc->push_event(std::make_shared<machine_event_message>(message));
     } else if (starts_with(line, "ALARM:")) {
-        cnc->listener->on_alarm(std::stoi(line.substr(6)));
+        auto alarm = std::stoi(line.substr(6));
+        cnc->push_event(std::make_shared<machine_event_alarm>(alarm));
     } else if (starts_with(line, "$")) {
         auto pieces = split_string(line, "=");
         cnc->settings[pieces[0]] = pieces[1];
@@ -828,7 +852,7 @@ void grbl::machine_state_idle::on_line_received(std::string line) {
                 probe_coords[i] = std::stof(coords_as_string[i]);
             }
             bool probe_touched = pieces[2] == "1";
-            cnc->listener->on_probe_result(probe_touched, probe_coords);
+            cnc->push_event(std::make_shared<machine_event_probe_result>(probe_touched, probe_coords));
         }
     }
 }
@@ -877,9 +901,15 @@ void grbl::machine_state_check_program::move_to_next_check_stage() {
             break;
         case check_stage::disable_check_mode:
             if (check_failed) {
-                cnc->listener->on_check_completed(false, cnc->executed_instructions - 1, check_error);
+                bool success = false;
+                size_t failed_idx = cnc->executed_instructions - 1;
+                size_t error = check_error;
+                cnc->push_event(std::make_shared<machine_event_check_completed>(success, failed_idx, error));
             } else {
-                cnc->listener->on_check_completed(true, 0, 0);
+                bool success = true;
+                size_t failed_idx = 0;
+                size_t error = 0;
+                cnc->push_event(std::make_shared<machine_event_check_completed>(success, failed_idx, error));
             }
             cnc->switch_to_state(grbl_machine_state::idle);
             break;
@@ -901,15 +931,17 @@ void grbl::machine_state_check_program::on_line_received(std::string line) {
         move_to_next_check_stage();
 
     } else if (starts_with(line, "Grbl")) {
-        cnc->listener->on_banner(line);
+        cnc->push_event(std::make_shared<machine_event_banner>(line));
         cnc->reset_machine_state();
     } else if (starts_with(line, "<")) {
         cnc->last_report = parse_status_report(line, cnc->last_report);
-        cnc->listener->on_realtime_status_report(cnc->last_report);
+        cnc->push_event(std::make_shared<machine_event_report_received>(cnc->last_report));
     } else if (starts_with(line, "[MSG:")) {
-        cnc->listener->on_message(line.substr(5, line.size() - 6));
+        auto message = line.substr(5, line.size() - 6);
+        cnc->push_event(std::make_shared<machine_event_message>(message));
     } else if (starts_with(line, "ALARM:")) {
-        cnc->listener->on_alarm(std::stoi(line.substr(6)));
+        auto alarm = std::stoi(line.substr(6));
+        cnc->push_event(std::make_shared<machine_event_alarm>(alarm));
     }
 }
 
@@ -963,9 +995,15 @@ void grbl::machine_state_run_program::move_to_next_run_stage() {
         case run_stage::run_program:
             if (run_failed || !continue_program()) {
                 if (run_failed) {
-                    cnc->listener->on_run_completed(false, cnc->executed_instructions - 1, run_error);
+                    bool success = false;
+                    size_t failed_index = cnc->executed_instructions - 1;
+                    size_t error = run_error;
+                    cnc->push_event(std::make_shared<machine_event_run_completed>(success, failed_index, error));
                 } else {
-                    cnc->listener->on_run_completed(true, 0, 0);
+                    bool success = true;
+                    size_t failed_index = 0;
+                    size_t error = 0;
+                    cnc->push_event(std::make_shared<machine_event_run_completed>(success, failed_index, error));
                 }
                 cnc->switch_to_state(grbl_machine_state::idle);
             }
@@ -1006,14 +1044,228 @@ void grbl::machine_state_run_program::on_line_received(std::string line) {
         move_to_next_run_stage();
 
     } else if (starts_with(line, "Grbl")) {
-        cnc->listener->on_banner(line);
+        cnc->push_event(std::make_shared<machine_event_banner>(line));
         cnc->reset_machine_state();
     } else if (starts_with(line, "<")) {
         cnc->last_report = parse_status_report(line, cnc->last_report);
-        cnc->listener->on_realtime_status_report(cnc->last_report);
+        cnc->push_event(std::make_shared<machine_event_report_received>(cnc->last_report));
     } else if (starts_with(line, "[MSG:")) {
-        cnc->listener->on_message(line.substr(5, line.size() - 6));
+        auto message = line.substr(5, line.size() - 6);
+        cnc->push_event(std::make_shared<machine_event_message>(message));
     } else if (starts_with(line, "ALARM:")) {
-        cnc->listener->on_alarm(std::stoi(line.substr(6)));
+        auto alarm = std::stoi(line.substr(6));
+        cnc->push_event(std::make_shared<machine_event_alarm>(alarm));
     }
 }
+
+// heightmap probing
+void grbl::machine_state_heightmap_probing::on_connected(grbl::machine *m) {
+
+}
+
+void grbl::machine_state_heightmap_probing::on_disconnected(grbl::machine *m) {
+
+}
+
+void grbl::machine_state_heightmap_probing::on_enter(grbl::machine *m) {
+    cnc = m;
+    stage = heightmap_probing_stage::start;
+    failed = false;
+    error = 0;
+    probed_locations = 0;
+    move_to_next_stage();
+}
+
+void grbl::machine_state_heightmap_probing::on_exit(grbl::machine *m) {
+
+}
+
+void grbl::machine_state_heightmap_probing::on_line_received(std::string line) {
+    if (starts_with(line, "ok")) {
+        if (cnc->awaiting_responses > 0) {
+            cnc->awaiting_responses--;
+        }
+        move_to_next_stage();
+    } else if (starts_with(line, "error")) {
+        if (cnc->awaiting_responses > 0) {
+            cnc->awaiting_responses--;
+        }
+        failed = true;
+        error = std::stoi(line.substr(6));
+        move_to_next_stage();
+    } else if (starts_with(line, "Grbl")) {
+        cnc->push_event(std::make_shared<machine_event_banner>(line));
+        cnc->reset_machine_state();
+    } else if (starts_with(line, "<")) {
+        cnc->last_report = parse_status_report(line, cnc->last_report);
+        cnc->push_event(std::make_shared<machine_event_report_received>(cnc->last_report));
+    } else if (starts_with(line, "[MSG:")) {
+        auto message = line.substr(5, line.size() - 6);
+        cnc->push_event(std::make_shared<machine_event_message>(message));
+    } else if (starts_with(line, "ALARM:")) {
+        auto alarm = std::stoi(line.substr(6));
+        cnc->push_event(std::make_shared<machine_event_alarm>(alarm));
+    } else if (starts_with(line, "[PRB")) {
+        std::cout << "received PRB" << std::endl;
+        line = line.substr(1, line.size() - 2);
+        auto pieces = split_string(line, ":");
+        cnc->parameters[pieces[0]] = pieces[1] + ":" + pieces[2];
+    }
+}
+
+bool grbl::machine_state_heightmap_probing::continue_program() {
+    return false;
+}
+
+void grbl::machine_state_heightmap_probing::move_to_next_stage() {
+    switch (stage) {
+        case heightmap_probing_stage::start:
+            cnc->pipe->send("G0X0Y0Z15");
+            stage = heightmap_probing_stage::goto_home;
+            break;
+        case heightmap_probing_stage::goto_home:
+            cnc->pipe->send("G38.2 Z-65 F100");
+            stage = heightmap_probing_stage::initial_probe_step_back;
+            break;
+        case heightmap_probing_stage::initial_probe_step_back:
+            // step back a bit (1mm, relative)
+            cnc->pipe->send("G91 G0 Z1");
+            stage = heightmap_probing_stage::initial_probe_fine_seek;
+            break;
+        case heightmap_probing_stage::initial_probe_fine_seek:
+            // probe again but finer
+            cnc->pipe->send(" G38.2 Z-5 F5");
+            stage = heightmap_probing_stage::initial_probe;
+            break;
+        case heightmap_probing_stage::initial_probe:
+            std::cout << "Initial probe: " << cnc->parameters["PRB"];
+
+            std::cout << ". Setting this as new zero" << std::endl;
+            cnc->pipe->send("G10 L20 P1 Z0");
+
+            {
+                auto pieces = split_string(cnc->parameters["PRB"], ":");
+                auto axes = split_string(pieces[0], ",");
+                z_zero_in_mpos = std::stof(axes[2]);
+                std::cout << "Z zero in mpos: " << z_zero_in_mpos << std::endl;
+                grid->vertices[probed_locations].z = 0;
+            }
+
+
+            stage = heightmap_probing_stage::goto_next_location;
+            break;
+        case heightmap_probing_stage::goto_next_location: {
+            auto pieces = split_string(cnc->parameters["PRB"], ":");
+            auto axes = split_string(pieces[0], ",");
+            auto current_z = std::stof(axes[2]);
+            if (probed_locations == 0) {
+                z_zero_in_mpos = current_z;
+                std::cout << "Z zero in mpos: " << z_zero_in_mpos << std::endl;
+                grid->vertices[probed_locations].z = 0;
+            } else {
+                auto delta_z = current_z - z_zero_in_mpos;
+                std::cout << "Z[" << probed_locations << "]  = " << delta_z << std::endl;
+                grid->vertices[probed_locations].z = delta_z;
+
+                cnc->push_event(std::make_shared<machine_event_heightmap_probe_acquired>(grid, probed_locations));
+            }
+        }
+
+            probed_locations++;
+            if (probed_locations == grid->vertices.size()) {
+                cnc->pipe->send("G0Z15"); // safe height
+                stage = heightmap_probing_stage::done;
+            } else {
+                cnc->pipe->send("G90 G0 Z1.5");
+                stage = heightmap_probing_stage::goto_next_location_move;
+            }
+            break;
+        case heightmap_probing_stage::goto_next_location_move:
+            cnc->pipe->send("G0 X" + std::to_string(grid->vertices[probed_locations].x) + " Y" +
+                            std::to_string(grid->vertices[probed_locations].y));
+            stage = heightmap_probing_stage::goto_start_probing_at;
+            break;
+        case heightmap_probing_stage::goto_start_probing_at:
+            cnc->pipe->send("G0 Z0.5"); // this appears to move Z upwards instead of downwards. why?
+            // faking the G0 Z0.5
+//            cnc->pipe->send("G91 G0 Z-1"); // 1.5 - 1 = 0.5.//
+            stage = heightmap_probing_stage::probing;
+            break;
+        case heightmap_probing_stage::probing:
+            cnc->pipe->send("G38.2 Z-5 F5");
+            stage = heightmap_probing_stage::goto_next_location;
+            break;
+        case heightmap_probing_stage::done:
+            cnc->switch_to_state(grbl_machine_state::idle);
+            break;
+        default:
+            break;
+    }
+}
+
+
+grbl::machine_event_connect::machine_event_connect() {
+    machine_event::type = machine_event_type::connected;
+}
+
+grbl::machine_event_disconnect::machine_event_disconnect() {
+    machine_event::type = machine_event_type::disconnected;
+}
+
+grbl::machine_event_report_received::machine_event_report_received(const grbl::realtime_status_report& r)
+        : report(r) {
+    machine_event::type = machine_event_type::report_received;
+}
+
+grbl::machine_event_banner::machine_event_banner(const std::string& b)
+        : banner{b} {
+    machine_event::type = machine_event_type::banner;
+}
+
+grbl::machine_event_message::machine_event_message(const std::string& m)
+        : message{m} {
+    machine_event::type = machine_event_type::message;
+}
+
+grbl::machine_event_alarm::machine_event_alarm(int code)
+        : alarm(code) {
+    machine_event::type = machine_event_type::alarm;
+}
+
+grbl::machine_event_init_completed::machine_event_init_completed() {
+    machine_event::type = machine_event_type::init_completed;
+}
+
+grbl::machine_event_run_completed::machine_event_run_completed(bool success, size_t failed_index, size_t error)
+        : success(success),
+          failed_index(failed_index),
+          error(error) {
+    machine_event::type = machine_event_type::run_completed;
+}
+
+grbl::machine_event_check_completed::machine_event_check_completed(bool success, size_t failed_idx, size_t error)
+        : success(success),
+          failed_index(failed_idx),
+          error(error) {
+    machine_event::type = machine_event_type::check_completed;
+}
+
+grbl::machine_event_settings_reloaded::machine_event_settings_reloaded() {
+    machine_event::type = machine_event_type::settings_reloaded;
+}
+
+grbl::machine_event_parameters_reloaded::machine_event_parameters_reloaded() {
+    machine_event::type = machine_event_type::parameters_reloaded;
+}
+
+grbl::machine_event_probe_result::machine_event_probe_result(bool touched, const float *coords)
+        : probe_touched(touched),
+          probe_coords{coords[0], coords[1], coords[2]} {
+    machine_event::type = machine_event_type::probe_result;
+}
+
+grbl::machine_event_heightmap_probe_acquired::machine_event_heightmap_probe_acquired(grbl::heightmap *g, size_t location)
+        : grid(g),
+          probed_location(location) {
+    machine_event::type = machine_event_type::heightmap_probe_acquired;
+}

grbl_machine.h

@@ -2,8 +2,11 @@
 
 #include <map>
 #include <unordered_map>
+#include <memory>
+#include <mutex>
 #include "grbl_communication.h"
 #include "grbl.h"
+#include "heightmap.h"
 
 namespace grbl {
 
@@ -81,6 +84,7 @@ enum class grbl_machine_state {
     run_program,
     check_program,
     idle,
+    heightmap_probing,
 };
 
 struct jog_state {
@@ -114,19 +118,92 @@ static bool operator!=(const jog_state& a, const jog_state& b) {
     return !(a == b);
 }
 
-struct machine_listener {
+enum class machine_event_type {
-    virtual void on_connected() = 0;
+    connected,
-    virtual void on_disconnected() = 0;
+    disconnected,
-    virtual void on_realtime_status_report(realtime_status_report) = 0;
+    report_received,
-    virtual void on_banner(std::string line) = 0;
+    banner,
-    virtual void on_message(std::string message) = 0;
+    message,
-    virtual void on_alarm(int alarm) = 0;
+    alarm,
-    virtual void on_init_completed() = 0;
+    init_completed,
-    virtual void on_run_completed(bool success, size_t failed_index, size_t error) = 0;
+    run_completed,
-    virtual void on_check_completed(bool success, size_t failed_index, size_t error) = 0;
+    check_completed,
-    virtual void on_settings_reloaded() = 0;
+    settings_reloaded,
-    virtual void on_parameters_reloaded() = 0;
+    parameters_reloaded,
-    virtual void on_probe_result(bool probe_touched, float probe_coords[3]) = 0;
+    probe_result,
+    heightmap_probe_acquired,
+};
+
+struct machine_event {
+    machine_event_type type;
+    // TODO: ewwww
+    virtual void omg() {}
+};
+
+struct machine_event_connect : public machine_event {
+    machine_event_connect();
+};
+
+struct machine_event_disconnect : public machine_event {
+    machine_event_disconnect();
+};
+
+struct machine_event_report_received : public machine_event {
+    explicit machine_event_report_received(const realtime_status_report& report);
+    realtime_status_report report;
+};
+
+struct machine_event_banner : public machine_event {
+    explicit machine_event_banner(const std::string& banner);
+    std::string banner;
+};
+
+struct machine_event_message : public machine_event {
+    explicit machine_event_message(const std::string& message);
+    std::string message;
+};
+
+struct machine_event_alarm : public machine_event {
+    explicit machine_event_alarm(int code);
+    int alarm;
+};
+
+struct machine_event_init_completed : public machine_event {
+    machine_event_init_completed();
+};
+
+struct machine_event_run_completed : public machine_event {
+    machine_event_run_completed(bool success, size_t failed_index, size_t error);
+    bool success;
+    size_t failed_index;
+    size_t error;
+};
+
+struct machine_event_check_completed : public machine_event {
+    machine_event_check_completed(bool success, size_t failed_idx, size_t error);
+    bool success;
+    size_t failed_index;
+    size_t error;
+};
+
+struct machine_event_settings_reloaded : public machine_event {
+    machine_event_settings_reloaded();
+};
+
+struct machine_event_parameters_reloaded : public machine_event {
+    machine_event_parameters_reloaded();
+};
+
+struct machine_event_probe_result : public machine_event {
+    machine_event_probe_result(bool touched, const float *coords);
+    bool probe_touched;
+    float probe_coords[3];
+};
+
+struct machine_event_heightmap_probe_acquired : public machine_event {
+    machine_event_heightmap_probe_acquired(heightmap *g, size_t location);
+    grbl::heightmap *grid;
+    size_t probed_location;
 };
 
 
@@ -149,6 +226,14 @@ enum class run_stage {
     run_program,
 };
 
+enum class heightmap_probing_stage {
+    start,
+    goto_home,
+    initial_probe,
+    goto_next_location,
+    probing, initial_probe_step_back, initial_probe_fine_seek, goto_next_location_move, goto_start_probing_at, done
+};
+
 struct machine;
 
 struct machine_state {
@@ -222,6 +307,25 @@ struct machine_state_run_program : public machine_state {
     size_t run_error;
 };
 
+struct machine_state_heightmap_probing : public machine_state {
+    void on_connected(machine *m) override;
+    void on_disconnected(machine *m) override;
+    void on_enter(machine *m) override;
+    void on_exit(machine *m) override;
+    void on_line_received(std::string line) override;
+
+    bool continue_program();
+    void move_to_next_stage();
+
+    machine *cnc;
+    heightmap_probing_stage stage = heightmap_probing_stage::start;
+    bool failed;
+    size_t error;
+    size_t probed_locations;
+    heightmap *grid;
+    float z_zero_in_mpos = 0;
+};
+
 
 struct settings_cmp {
     bool operator()(const std::string& a, const std::string& b) const {
@@ -248,7 +352,6 @@ struct machine : public transport_callbacks {
     machine();
     ~machine();
 
-    void set_listener(grbl::machine_listener *listener);
     void connect();
 
     void set_work_offset(std::string work_offset);
@@ -280,7 +383,19 @@ struct machine : public transport_callbacks {
 
     void start_z_probe(float min_z, float feed_rate);
 
+    void probe_heightmap(grbl::heightmap& grid);
+
+    void push_event(std::shared_ptr<machine_event> event);
+
+    // pops nullptr if queue is empty
+    std::shared_ptr<machine_event> pop_event();
+
+
 protected:
+
+    std::mutex event_mutex;
+    std::deque<std::shared_ptr<machine_event>> events;
+
     void on_connected(transport *transport) override;
     void on_disconnected(transport *transport) override;
     void on_line_received(std::string line, transport *transport) override;
@@ -292,6 +407,7 @@ protected:
     friend class machine_state_idle;
     friend class machine_state_check_program;
     friend class machine_state_run_program;
+    friend class machine_state_heightmap_probing;
 
     std::map<grbl_machine_state, machine_state *> states;
     std::map<std::string, std::string, settings_cmp> settings;
@@ -305,7 +421,6 @@ protected:
 
     volatile size_t awaiting_responses = 0;
     realtime_status_report last_report{};
-    machine_listener *listener = nullptr;
     transport *pipe = nullptr;
     grbl_machine_state state = grbl_machine_state::disconnected;
     program running_program;

heightmap.cpp

@@ -0,0 +1,64 @@
+#include "heightmap.h"
+
+grbl::heightmap grbl::heightmap::from_params(float from_x, float from_y, float to_x, float to_y, float resolution) {
+    heightmap result{};
+
+    result.from_x = from_x;
+    result.from_y = from_y;
+    result.to_x = to_x;
+    result.to_y = to_y;
+    result.resolution = resolution;
+
+    result.x_segments = static_cast<size_t>(ceilf((to_x - from_x) / resolution));
+    result.y_segments = static_cast<size_t>(ceilf((to_y - from_y) / resolution));
+
+    float y_pos = from_y;
+    for (int y = 0; y < (result.y_segments + 1); y++) {
+        float x_pos = from_x;
+        for (int x = 0; x < (result.x_segments + 1); x++) {
+            // add a bit of random wobble to make it visible [-10, +10]
+//            float z = ((rand() / (float) RAND_MAX) - 0.5f) * 2.0f * 1.0f;
+            float z = 0.0f;
+            result.vertices.emplace_back(x_pos, y_pos, z);
+            x_pos += resolution;
+        }
+        y_pos += resolution;
+    }
+
+    return result;
+}
+
+size_t grbl::heightmap::index_from_coords(float x, float y) const {
+    size_t result = 0;
+    for (auto& v: vertices) {
+        if (v.x == x && v.y == y) {
+            return result;
+        }
+        result++;
+    }
+    return 0;
+}
+
+// bi-linear interpolation
+float grbl::heightmap::get_z_at(float x, float y) const {
+
+    // TODO: make faster by precalculating indices
+
+    float x1 = x - fmodf(x, resolution);
+    float x2 = x1 + resolution;
+    float y1 = y - fmodf(y, resolution);
+    float y2 = y1 + resolution;
+    float z11 = vertices[index_from_coords(x1, y1)].z;
+    float z12 = vertices[index_from_coords(x1, y2)].z;
+    float z21 = vertices[index_from_coords(x2, y1)].z;
+    float z22 = vertices[index_from_coords(x2, y2)].z;
+
+    float alpha_x = (x - x1) / resolution;
+    float a = z11 + (z21 - z11) * alpha_x;
+    float b = z12 + (z22 - z12) * alpha_x;
+
+    float alpha_y = (y - y1) / resolution;
+    float c = a + (b - a) * alpha_y;
+
+    return c;
+}

heightmap.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <cstddef>
+#include <cmath>
+#include <glm/vec3.hpp>
+#include <vector>
+
+namespace grbl {
+
+struct heightmap {
+    static heightmap from_params(float from_x, float from_y, float to_x, float to_y, float resolution);
+    float get_z_at(float x, float y) const;
+    size_t index_from_coords(float x, float y) const;
+
+    float from_x, from_y;
+    float to_x, to_y;
+    float resolution;
+    size_t x_segments, y_segments;
+    std::vector<glm::vec3> vertices;
+};
+
+
+}

heightmap_test.cpp

@@ -0,0 +1,26 @@
+#include <gtest/gtest.h>
+
+#include "heightmap.h"
+
+TEST(heightmap, get_z_at) {
+    auto grid = grbl::heightmap::from_params(0, 0, 1, 1, 1);
+    grid.vertices[0].z = 0;
+    grid.vertices[1].z = 1;
+    grid.vertices[2].z = 2;
+    grid.vertices[3].z = 3;
+
+    auto z = grid.get_z_at(0.5f, 0.5f);
+    EXPECT_EQ(1.5, z);
+
+    z = grid.get_z_at(0.5f, 0);
+    EXPECT_EQ(0.5, z);
+
+    z = grid.get_z_at(0.5f, 1);
+    EXPECT_EQ(2.5, z);
+
+    z = grid.get_z_at(0, 0.5f);
+    EXPECT_EQ(1, z);
+
+    z = grid.get_z_at(1, 0.5f);
+    EXPECT_EQ(2, z);
+}

main.cpp

@@ -34,6 +34,7 @@
 #include "glm/gtx/quaternion.hpp"
 #include "nanogui/nanogui.h"
 #include "glm/gtc/matrix_inverse.hpp"
+#include "heightmap.h"
 #include <glm/ext/quaternion_float.hpp>
 #include <glm/ext/quaternion_trigonometric.hpp>
 #include <glm/vec3.hpp> // glm::vec3
@@ -42,20 +43,21 @@
 #include <glm/ext/matrix_transform.hpp> // glm::translate, glm::rotate, glm::scale
 #include <glm/ext/matrix_clip_space.hpp> // glm::perspective
 #include <glm/ext/scalar_constants.hpp> // glm::pi
+#include <fstream>
 
 using namespace nanogui;
 
 grbl::machine cnc{};
 
-class SenderApp : public Screen, public grbl::machine_listener {
+class SenderApp : public Screen {
 public:
 
     Window *window;
     grbl::jog_state jog;
     TextBox *txt_status;
-    TextBox *txtMessage;
+    TextBox *txt_message;
     nanogui::Color color_red = nanogui::Color(255, 0, 0, 255);
-    nanogui::Color colGreen = nanogui::Color(0, 255, 0, 255);
+    nanogui::Color color_green = nanogui::Color(0, 255, 0, 255);
     int last_alarm = 0;
     grbl::program pgm;
     Button *btn_load_program, *btn_check_program, *btn_run_program;
@@ -66,6 +68,7 @@ public:
     grbl::program_renderer renderer;
 
     glm::vec3 cam_target = glm::vec3(0);
+    glm::vec3 cam_pan = glm::vec3(0);
     float cam_zoom = 0;
     glm::quat cam_src_rotation = glm::quat(1.0, 0.0, 0.0, 0.0); // identity quaternion
 
@@ -84,6 +87,7 @@ public:
 
     Button *btn_keyboard_jog;
     std::stringstream dro_ss;
+    grbl::heightmap heightmap_grid;
 
 
     SenderApp() : Screen(Vector2i(1024, 768), "GRBL Sender") {
@@ -126,9 +130,9 @@ public:
 
         perform_layout();
 
-        m_render_pass = new RenderPass({this});
+        m_render_pass = new RenderPass({this}, nullptr, nullptr, nullptr, true);
         m_render_pass->set_clear_color(0, Color(0.13f, 0.13f, 0.13f, 1.f));
-        m_render_pass->set_depth_test(RenderPass::DepthTest::Always, true);
+        m_render_pass->set_depth_test(RenderPass::DepthTest::Greater, true);
         m_render_pass->set_cull_mode(RenderPass::CullMode::Disabled);
     }
 
@@ -238,8 +242,8 @@ public:
         //
         status_text_holder->add<Label>("");
 
-        txtMessage = status_text_holder->add<TextBox>("");
+        txt_message = status_text_holder->add<TextBox>("");
-        txtMessage->set_fixed_width(300);
+        txt_message->set_fixed_width(300);
 
         status_text_holder->add<Label>("");
 
@@ -455,7 +459,67 @@ public:
 
     TextBox *txt_heightmap_from_x, *txt_heightmap_from_y;
     TextBox *txt_heightmap_to_x, *txt_heightmap_to_y;
-    TextBox *txt_grid_res;
+    TextBox *txt_heightmap_res;
+    TextBox *txt_clearance_height, *txt_start_probing_at, *txt_max_negative_z, *txt_final_z_height;
+
+    static void save_heightmap(const grbl::heightmap& grid, std::string path) {
+        std::ofstream out(path);
+        if (out) {
+            out << "from_x:" << grid.from_x << std::endl;
+            out << "from_y:" << grid.from_y << std::endl;
+            out << "to_x:" << grid.to_x << std::endl;
+            out << "to_y:" << grid.to_y << std::endl;
+            out << "resolution:" << grid.resolution << std::endl;
+            out << "probes:" << grid.vertices.size() << std::endl;
+            for (auto& v: grid.vertices) {
+                out << v.x << ":" << v.y << ":" << v.z << std::endl;
+            }
+        }
+    }
+
+    grbl::heightmap load_heightmap(std::string path) {
+        grbl::heightmap result{};
+
+        std::ifstream in_file(path);
+        if (in_file) {
+            float from_x, from_y;
+            float to_x, to_y;
+            float resolution;
+            size_t probe_location = 0;
+
+            for (std::string line; std::getline(in_file, line);) {
+                auto pieces = split_string(line, ":");
+                if (pieces[0] == "from_x") {
+                    from_x = std::stof(pieces[1]);
+                } else if (pieces[0] == "from_y") {
+                    from_y = std::stof(pieces[1]);
+                } else if (pieces[0] == "to_x") {
+                    to_x = std::stof(pieces[1]);
+                } else if (pieces[0] == "to_y") {
+                    to_y = std::stof(pieces[1]);
+                } else if (pieces[0] == "resolution") {
+                    resolution = std::stof(pieces[1]);
+                } else if (pieces[0] == "probes") {
+                    result = grbl::heightmap::from_params(from_x, from_y, to_x, to_y, resolution);
+                } else {
+                    result.vertices[probe_location].x = std::stof(pieces[0]);
+                    result.vertices[probe_location].y = std::stof(pieces[1]);
+                    result.vertices[probe_location].z = std::stof(pieces[2]);
+                    probe_location++;
+                }
+            }
+        }
+
+        return result;
+    }
+
+    void fill_heightmap_controls_from_grid(const grbl::heightmap& grid) const {
+        txt_heightmap_from_x->set_value(std::to_string(grid.from_x));
+        txt_heightmap_from_y->set_value(std::to_string(grid.from_y));
+        txt_heightmap_to_x->set_value(std::to_string(grid.to_x));
+        txt_heightmap_to_y->set_value(std::to_string(grid.to_y));
+        txt_heightmap_res->set_value(std::to_string(grid.resolution));
+    }
 
     void add_heightmap_markup() {
         heightmap_layer->add<Label>("Grid definition", "sans-bold", 20);
@@ -491,7 +555,17 @@ public:
         btn->set_callback([&]() {
             fill_heightmap_from_model();
         });
-        grid_size_holder->add<Label>("");
+        auto btn_load_heightmap = grid_size_holder->add<Button>("Load from file");
+        btn_load_heightmap->set_callback([&]() {
+            auto path = file_dialog(
+                    {{"hmap", "Heightmap files"}
+                    }, true);
+            if (!path.empty()) {
+                heightmap_grid = load_heightmap(path);
+                fill_heightmap_controls_from_grid(heightmap_grid);
+                renderer.update_grid(heightmap_grid);
+            }
+        });
 
         txt_heightmap_from_x->set_callback([&](const std::string& new_value) {
             update_grid();
@@ -516,17 +590,56 @@ public:
         auto grid_res_holder = heightmap_params_holder->add<Widget>();
         grid_res_holder->set_layout(new BoxLayout(nanogui::Orientation::Horizontal, Alignment::Middle, 4, 4));
         grid_res_holder->add<Label>("Grid resolution", "sans-bold", 20);
-        txt_grid_res = grid_res_holder->add<TextBox>("5");
+        txt_heightmap_res = grid_res_holder->add<TextBox>("5");
-        txt_grid_res->set_editable(true);
+        txt_heightmap_res->set_editable(true);
-        txt_grid_res->set_fixed_width(150);
+        txt_heightmap_res->set_fixed_width(150);
-        txt_grid_res->set_callback([&](const std::string& new_value) {
+        txt_heightmap_res->set_callback([&](const std::string& new_value) {
             update_grid();
             return true;
         });
         grid_res_holder->add<Label>("mm", "sans-bold", 20);
+
+        // business params
+        auto heightmap_business_holder = heightmap_params_holder->add<Widget>();
+        heightmap_business_holder->set_layout(new GridLayout(nanogui::Orientation::Horizontal, 3, Alignment::Fill, 4, 4));
+
+        heightmap_business_holder->add<Label>("Clearance height Z", "sans-bold", 20);
+        txt_clearance_height = heightmap_business_holder->add<TextBox>("1.5");
+        txt_clearance_height->set_editable(true);
+        heightmap_business_holder->add<Label>("", "sans-bold");
+
+        heightmap_business_holder->add<Label>("Start probing at Z", "sans-bold", 20);
+        txt_start_probing_at = heightmap_business_holder->add<TextBox>("0.5");
+        txt_start_probing_at->set_editable(true);
+        heightmap_business_holder->add<Label>("", "sans-bold");
+
+        heightmap_business_holder->add<Label>("Max negative Z", "sans-bold", 20);
+        txt_max_negative_z = heightmap_business_holder->add<TextBox>("-1.0");
+        txt_max_negative_z->set_editable(true);
+        heightmap_business_holder->add<Label>("(when to fail probing)", "sans-bold");
+
+        heightmap_business_holder->add<Label>("Final Z safety height", "sans-bold", 20);
+        txt_final_z_height = heightmap_business_holder->add<TextBox>("15");
+        txt_final_z_height->set_editable(true);
+        heightmap_business_holder->add<Label>("", "sans-bold");
+
+        heightmap_business_holder->add<Label>("", "sans-bold");
+        auto btn_start_probing = heightmap_business_holder->add<Button>("Start Probing");
+        btn_start_probing->set_callback([&]() {
+            cnc.probe_heightmap(heightmap_grid);
+        });
+        auto btn_save_heightmap = heightmap_business_holder->add<Button>("Save heightmap");
+        btn_save_heightmap->set_callback([&]() {
+            auto path = file_dialog(
+                    {{"hmap", "Heightmap files"}
+                    }, true);
+            if (!path.empty()) {
+                save_heightmap(heightmap_grid, path);
+            }
+        });
     }
 
-    void fill_heightmap_from_model() {
+    void fill_heightmap_from_model() const {
         auto min = renderer.get_extents_min();
         auto max = renderer.get_extents_max();
         txt_heightmap_from_x->set_value(std::to_string(min.x));
@@ -540,8 +653,17 @@ public:
         auto from_y = std::stof(txt_heightmap_from_y->value());
         auto to_x = std::stof(txt_heightmap_to_x->value());
         auto to_y = std::stof(txt_heightmap_to_y->value());
-        auto res = std::stoi(txt_grid_res->value());
+        auto res = std::stof(txt_heightmap_res->value());
-        renderer.update_grid(from_x, from_y, to_x, to_y, res);
+
+        if (from_x != heightmap_grid.from_x ||
+            from_y != heightmap_grid.from_y ||
+            to_x != heightmap_grid.to_x ||
+            to_y != heightmap_grid.to_y ||
+            res != heightmap_grid.resolution) {
+            heightmap_grid = std::move(grbl::heightmap::from_params(from_x, from_y, to_x, to_y, res));
+            renderer.update_grid(heightmap_grid);
+        }
+
     }
 
     void add_pins_markup() {
@@ -684,58 +806,8 @@ public:
         return Screen::resize_event(size);
     }
 
-    void on_connected() override {
-    }
-
-    void on_disconnected() override {
-    }
-
-    void on_settings_reloaded() override {
-    }
-
-    void on_parameters_reloaded() override {
-        refresh_offset();
-        update_dro();
-    }
-
-
     grbl::realtime_status_report last_report;
 
-    volatile bool machine_initialized = false;
-
-    void on_init_completed() override {
-        machine_initialized = true;
-        // need to do UI related things in UI thread
-    }
-
-    void on_realtime_status_report(grbl::realtime_status_report report) override {
-//        if (report == last_report) return;
-
-//        if (last_report.status != report.status) {
-        txt_status->set_value(grbl::status_to_string(cnc.get_status().status));
-
-        if (cnc.get_status().status == grbl::machine_status::alarm) {
-            txt_status->set_tooltip(grbl::alarm_to_string(last_alarm));
-        } else {
-            txt_status->set_tooltip(cnc.get_status().sub_status);
-        }
-//        }
-
-
-        // FIXME: ugly way of retrieving the bg color
-        btn_pin_door->set_background_color(report.signals.bit.door ? color_red : btn_load_program->background_color());
-        btn_pin_hold->set_background_color(report.signals.bit.hold ? color_red : btn_load_program->background_color());
-        btn_pin_reset->set_background_color(report.signals.bit.soft_reset ? color_red : btn_load_program->background_color());
-        btn_pin_cycle_start->set_background_color(report.signals.bit.cycle_start ? color_red : btn_load_program->background_color());
-        btn_pin_limit_x->set_background_color(report.signals.bit.x_limit ? color_red : btn_load_program->background_color());
-        btn_pin_limit_y->set_background_color(report.signals.bit.y_limit ? color_red : btn_load_program->background_color());
-        btn_pin_limit_z->set_background_color(report.signals.bit.z_limit ? color_red : btn_load_program->background_color());
-        btn_pin_probe->set_background_color(report.signals.bit.probe ? color_red : btn_load_program->background_color());
-
-        update_dro();
-        last_report = report;
-    }
-
     void update_dro() {
         auto work_pos = cnc.get_work_pos();
 
@@ -752,55 +824,6 @@ public:
         mpos_z_text->set_value(dro_ss.str());
     }
 
-    void on_banner(std::string line) override {
-    }
-
-    void on_message(std::string message) override {
-        txtMessage->set_value(message);
-        set_caption(message);
-    }
-
-    void on_alarm(int alarm) override {
-        last_alarm = alarm;
-    }
-
-    void on_probe_result(bool probe_touched, float *probe_coords) override {
-        std::stringstream ss;
-        ss << "Probing ended. Result: " << std::boolalpha << probe_touched << " at coords: ";
-        for (auto i = 0; i < 3; i++) {
-            ss << probe_coords[i] << ", ";
-        }
-        new MessageDialog(this, MessageDialog::Type::Warning, "Probe result", ss.str());
-    }
-
-    void on_check_completed(bool success, size_t failed_index, size_t error) override {
-        btn_check_program->set_background_color(success ? colGreen : color_red);
-        if (success) {
-            btn_run_program->set_enabled(true);
-            new MessageDialog(this, MessageDialog::Type::Information, "Check result", "Program checked successfully");
-        } else {
-            btn_run_program->set_enabled(false);
-            std::stringstream ss;
-            auto i = pgm.instruction_at(failed_index);
-            ss << "Program check failed at line " << i.line << " (" << i.command << "). Error: " << error << ", "
-               << grbl::error_to_string(error);
-            new MessageDialog(this, MessageDialog::Type::Warning, "Check result", ss.str());
-        }
-    }
-
-    void on_run_completed(bool success, size_t failed_index, size_t error) override {
-        btn_run_program->set_background_color(success ? colGreen : color_red);
-        if (success) {
-            new MessageDialog(this, MessageDialog::Type::Information, "Run result", "Program executed successfully");
-        } else {
-            std::stringstream ss;
-            auto i = pgm.instruction_at(failed_index);
-            ss << "Program execution failed at line " << i.line << " (" << i.command << "). Error: " << error << ", "
-               << grbl::error_to_string(error);
-            new MessageDialog(this, MessageDialog::Type::Warning, "Run result", ss.str());
-        }
-    }
-
 
     bool keyboard_event(int key, int scancode, int action, int modifiers) override {
         if (Screen::keyboard_event(key, scancode, action, modifiers))
@@ -906,8 +929,8 @@ public:
                 }
 
             } else if (is_panning) {
-//                cam_src.x += (float) rel.x() / 10.0f;
+                cam_pan.x += (float) rel.x() / 10.0f;
-//                cam_src.y += (float) rel.y() / 10.0f;
+                cam_pan.y -= (float) rel.y() / 10.0f;
             }
             return true;
         }
@@ -926,8 +949,58 @@ public:
     }
 
     void draw(NVGcontext *ctx) override {
-        // not pretty but need to do this in UI thread
+        std::shared_ptr<grbl::machine_event> event;
-        if (machine_initialized) {
+        while ((event = cnc.pop_event()) != nullptr) {
+            switch (event->type) {
+                case grbl::machine_event_type::connected:
+                    break;
+                case grbl::machine_event_type::disconnected:
+                    break;
+                case grbl::machine_event_type::report_received: {
+
+                    auto ev = dynamic_cast<grbl::machine_event_report_received *>(event.get());
+
+                    txt_status->set_value(grbl::status_to_string(cnc.get_status().status));
+
+                    if (cnc.get_status().status == grbl::machine_status::alarm) {
+                        txt_status->set_tooltip(grbl::alarm_to_string(last_alarm));
+                    } else {
+                        txt_status->set_tooltip(cnc.get_status().sub_status);
+                    }
+
+                    // FIXME: ugly way of retrieving the bg color
+                    btn_pin_door->set_background_color(ev->report.signals.bit.door ? color_red : btn_load_program->background_color());
+                    btn_pin_hold->set_background_color(ev->report.signals.bit.hold ? color_red : btn_load_program->background_color());
+                    btn_pin_reset->set_background_color(
+                            ev->report.signals.bit.soft_reset ? color_red : btn_load_program->background_color());
+                    btn_pin_cycle_start->set_background_color(
+                            ev->report.signals.bit.cycle_start ? color_red : btn_load_program->background_color());
+                    btn_pin_limit_x->set_background_color(
+                            ev->report.signals.bit.x_limit ? color_red : btn_load_program->background_color());
+                    btn_pin_limit_y->set_background_color(
+                            ev->report.signals.bit.y_limit ? color_red : btn_load_program->background_color());
+                    btn_pin_limit_z->set_background_color(
+                            ev->report.signals.bit.z_limit ? color_red : btn_load_program->background_color());
+                    btn_pin_probe->set_background_color(ev->report.signals.bit.probe ? color_red : btn_load_program->background_color());
+
+                    update_dro();
+                    last_report = ev->report;
+                    break;
+                }
+                case grbl::machine_event_type::banner:
+                    break;
+                case grbl::machine_event_type::message: {
+                    auto ev = dynamic_cast<grbl::machine_event_message *>(event.get());
+                    txt_message->set_value(ev->message);
+                    set_caption(ev->message);
+                    break;
+                }
+                case grbl::machine_event_type::alarm: {
+                    auto ev = dynamic_cast<grbl::machine_event_alarm *>(event.get());
+                    last_alarm = ev->alarm;
+                    break;
+                }
+                case grbl::machine_event_type::init_completed:
                     fill_in_settings();
                     fill_in_parameters();
 
@@ -935,8 +1008,66 @@ public:
 
                     refresh_offset();
                     update_dro();
-            machine_initialized = false;
+                    break;
+                case grbl::machine_event_type::run_completed: {
+                    auto ev = dynamic_cast<grbl::machine_event_run_completed *>(event.get());
+                    btn_run_program->set_background_color(ev->success ? color_green : color_red);
+                    if (ev->success) {
+                        new MessageDialog(this, MessageDialog::Type::Information, "Run result", "Program executed successfully");
+                    } else {
+                        std::stringstream ss;
+                        auto i = pgm.instruction_at(ev->failed_index);
+                        ss << "Program execution failed at line " << i.line << " (" << i.command << "). Error: "
+                           << ev->error << ", "
+                           << grbl::error_to_string(ev->error);
+                        new MessageDialog(this, MessageDialog::Type::Warning, "Run result", ss.str());
                     }
+                    break;
+                }
+                case grbl::machine_event_type::check_completed: {
+                    auto ev = dynamic_cast<grbl::machine_event_check_completed *>(event.get());
+
+                    btn_check_program->set_background_color(ev->success ? color_green : color_red);
+                    if (ev->success) {
+                        btn_run_program->set_enabled(true);
+                        new MessageDialog(this, MessageDialog::Type::Information, "Check result", "Program checked successfully");
+                    } else {
+                        btn_run_program->set_enabled(false);
+                        std::stringstream ss;
+                        auto i = pgm.instruction_at(ev->failed_index);
+                        ss << "Program check failed at line " << i.line << " (" << i.command << "). Error: "
+                           << ev->error << ", "
+                           << grbl::error_to_string(ev->error);
+                        new MessageDialog(this, MessageDialog::Type::Warning, "Check result", ss.str());
+                    }
+                    break;
+                }
+                case grbl::machine_event_type::settings_reloaded:
+                    break;
+                case grbl::machine_event_type::parameters_reloaded:
+                    refresh_offset();
+                    update_dro();
+                    break;
+                case grbl::machine_event_type::probe_result: {
+                    auto ev = dynamic_cast<grbl::machine_event_probe_result *>(event.get());
+
+                    std::stringstream ss;
+                    ss << "Probing ended. Result: " << std::boolalpha << ev->probe_touched << " at coords: ";
+                    for (float probe_coord: ev->probe_coords) {
+                        ss << probe_coord << ", ";
+                    }
+                    new MessageDialog(this, MessageDialog::Type::Warning, "Probe result", ss.str());
+                    break;
+                }
+                case grbl::machine_event_type::heightmap_probe_acquired: {
+                    std::cout << "Updating grid" << std::endl;
+                    auto ev = dynamic_cast<grbl::machine_event_heightmap_probe_acquired *>(event.get());
+                    renderer.update_grid(*ev->grid);
+                    break;
+                }
+            }
+        }
+
 
         Widget::draw(ctx);
     }
@@ -948,7 +1079,6 @@ public:
         m_render_pass->resize(framebuffer_size());
         m_render_pass->begin();
 
-        if (pgm.is_loaded) {
         renderer.update(pgm, cnc);
         // compute mvp
         glm::mat4 projection = glm::perspective(45.0f * glm::pi<float>() / 180.0f,
@@ -958,12 +1088,12 @@ public:
 
         glm::mat4 view = glm::translate(glm::mat4(1.0f), glm::vec3(0, 0, -cam_zoom)) *
                          glm::toMat4(cam_src_rotation) *
-                             glm::translate(glm::mat4(1.0f), -cam_target);
+                         glm::translate(glm::mat4(1.0f), cam_pan - cam_target);
 
         glm::mat4 model = glm::mat4(1.0f);
         glm::mat3 normal_mat = glm::inverseTranspose(glm::mat3(view * model));
+
         renderer.render(model, view, projection, normal_mat, glm::vec2(fb_size.x(), fb_size.y()));
-        }
 
         m_render_pass->end();
     }
@@ -974,7 +1104,6 @@ private:
 
     using ImageHolder = std::unique_ptr<uint8_t[], void (*)(void *)>;
     std::vector<std::pair<ref<Texture>, ImageHolder>> m_images;
-    int m_current_image;
 };
 
 
@@ -996,7 +1125,6 @@ int main(int argc, char **argv) {
             app->draw_all();
             app->set_visible(true);
 
-            cnc.set_listener(app);
             cnc.connect();
 
             nanogui::mainloop(1 / 60.f * 1000);

render.cpp

@@ -145,6 +145,8 @@ static void add_triangle(std::vector<float>& buffer_data, glm::vec3 p1, glm::vec
 
 
 void grbl::program_renderer::render(glm::mat4 model, glm::mat4 view, glm::mat4 projection, glm::mat3 normal_mat, glm::vec2 viewport_size) {
+    if (shader == nullptr || heightmap_shader == nullptr) return;
+
     // draw heightmap
     heightmap_shader->bind();
     heightmap_shader->set_mat4(glm::value_ptr(model), "mmtx");
@@ -337,6 +339,8 @@ void grbl::program_renderer::initialize_program_buffers() {
 }
 
 GLsizei grbl::program_renderer::update_model_vbo(const grbl::program& pgm) {
+    if (!pgm.is_loaded) return 0;
+
     static auto movement_re = std::regex(R"(([gG]0*1?\s+|[xXyYzZ]\s*[0-9\.\-]+))");
     bool is_tool_on = false;
 
@@ -422,43 +426,42 @@ GLsizei grbl::program_renderer::update_model_vbo(const grbl::program& pgm) {
 }
 
 
-void grbl::program_renderer::update_grid(float from_x, float from_y, float to_x, float to_y, int resolution) {
+void grbl::program_renderer::update_grid(const grbl::heightmap& grid) {
-
-    int x_segments = ceil((to_x - from_x) / resolution);
-    int y_segments = ceil((to_y - from_y) / resolution);
 
     glm::vec4 color = {0.5, 0.3, 0, 1};
 
-    std::vector<glm::vec3> vertices;
+    // this should only be called whenever the grid changes
-
+    // therefor it should be called each and every time we
-    float y_pos = from_y;
+    // probe a new location. let's check that.
-    for (int y = 0; y < (y_segments + 1); y++) {
-        float x_pos = from_x;
-        for (int x = 0; x < (x_segments + 1); x++) {
-            vertices.emplace_back(x_pos, y_pos, 0);
-            x_pos += resolution;
-        }
-        y_pos += resolution;
-    }
-
     std::vector<float> buffer_data;
 
-    for (int y = 0; y < y_segments; y++) {
+    for (int y = 0; y < grid.y_segments; y++) {
-        for (int x = 0; x < x_segments; x++) {
+        for (int x = 0; x < grid.x_segments; x++) {
-            int current = x + y * (x_segments + 1);
+            int current = x + y * (grid.x_segments + 1);
             int next = current + 1;
-            int bottom = next + x_segments;
+            int bottom = next + grid.x_segments;
             int bottom_next = bottom + 1;
 
-            auto p1 = vertices[current];
+            auto p1 = grid.vertices[current];
-            auto p2 = vertices[bottom];
+            auto p2 = grid.vertices[bottom];
-            auto p3 = vertices[next];
+            auto p3 = grid.vertices[next];
+
+            // exaggerate Z
+            auto exaggeration_factor = 100.0f;
+            p1.z *= exaggeration_factor;
+            p2.z *= exaggeration_factor;
+            p3.z *= exaggeration_factor;
+
             glm::vec3 normal = glm::normalize(glm::cross(p1 - p2, p3 - p1));
             add_triangle(buffer_data, p1, p2, p3, normal, color);
 
-            p1 = vertices[next];
+            p1 = grid.vertices[next];
-            p2 = vertices[bottom];
+            p2 = grid.vertices[bottom];
-            p3 = vertices[bottom_next];
+            p3 = grid.vertices[bottom_next];
+            p1.z *= exaggeration_factor;
+            p2.z *= exaggeration_factor;
+            p3.z *= exaggeration_factor;
+
             normal = glm::normalize(glm::cross(p1 - p2, p3 - p1));
             add_triangle(buffer_data, p1, p2, p3, normal, color);
         }
@@ -468,7 +471,7 @@ void grbl::program_renderer::update_grid(float from_x, float from_y, float to_x,
     heightmap_vertices_count = buffer_data.size() * sizeof(float) / size_of_vertex_in_bytes;
 
     glBindBuffer(GL_ARRAY_BUFFER, heightmap_vbo_id);
-    glBufferData(GL_ARRAY_BUFFER, sizeof(float) * buffer_data.size(), buffer_data.data(), GL_STATIC_DRAW);
+    glBufferData(GL_ARRAY_BUFFER, sizeof(float) * buffer_data.size(), buffer_data.data(), GL_DYNAMIC_DRAW);
 }
 
 void grbl::program_renderer::initialize_heightmap_buffers() {
@@ -505,6 +508,7 @@ std::string get_shader_info_log(GLuint id) {
     char error_log[log_length]; // length includes the NULL character
     glGetShaderInfoLog(id, log_length, &log_length, &error_log[0]);
 
+
     return std::string(error_log, log_length);
 }
 
@@ -518,7 +522,6 @@ std::string get_program_info_log(GLuint id) {
     return {error_log, (size_t) log_length};
 }
 
-
 bool check_compile_error(GLuint shader_id) {
     GLint is_compiled = 0;
     glGetShaderiv(shader_id, GL_COMPILE_STATUS, &is_compiled);

render.h

@@ -3,6 +3,7 @@
 #include "grbl.h"
 #include "grbl_machine.h"
 #include "glm/ext/matrix_float4x4.hpp"
+#include "heightmap.h"
 #include <nanogui/opengl.h>
 
 namespace grbl {
@@ -18,7 +19,7 @@ public:
 
     glm::vec3 get_extents_max() const { return max_pos; };
 
-    void update_grid(float from_x, float from_y, float to_x, float to_y, int resolution);
+    void update_grid(const grbl::heightmap& grid);
 private:
     GLsizei update_model_vbo(const grbl::program& pgm);