Heightmap probing ... done?

grbl_machine.h

@@ -4,6 +4,7 @@
 #include <unordered_map>
 #include "grbl_communication.h"
 #include "grbl.h"
+#include "heightmap.h"
 
 namespace grbl {
 
@@ -40,14 +41,14 @@ struct realtime_status_report {
     float actual_rpm = 0;
     union {
         struct {
-            bool x_limit : 1;
-            bool y_limit : 1;
-            bool z_limit : 1;
-            bool probe : 1;
-            bool door : 1;
-            bool hold : 1;
-            bool soft_reset : 1;
-            bool cycle_start : 1;
+            bool x_limit: 1;
+            bool y_limit: 1;
+            bool z_limit: 1;
+            bool probe: 1;
+            bool door: 1;
+            bool hold: 1;
+            bool soft_reset: 1;
+            bool cycle_start: 1;
         } bit;
         uint8_t value = 0;
     } signals;
@@ -81,6 +82,7 @@ enum class grbl_machine_state {
     run_program,
     check_program,
     idle,
+    heightmap_probing,
 };
 
 struct jog_state {
@@ -127,6 +129,7 @@ struct machine_listener {
     virtual void on_settings_reloaded() = 0;
     virtual void on_parameters_reloaded() = 0;
     virtual void on_probe_result(bool probe_touched, float probe_coords[3]) = 0;
+    virtual void on_heightmap_probe_acquired(heightmap* grid) = 0;
 };
 
 
@@ -149,6 +152,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 +233,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 {
@@ -280,6 +310,7 @@ struct machine : public transport_callbacks {
 
     void start_z_probe(float min_z, float feed_rate);
 
+    void probe_heightmap(grbl::heightmap& grid);
 protected:
     void on_connected(transport *transport) override;
     void on_disconnected(transport *transport) override;
@@ -292,6 +323,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;