add ability to save the bed plane with M500 and config setting to enable that

diff --git a/ConfigSamples/AzteegX5Mini/config b/ConfigSamples/AzteegX5Mini/config
index 05c8e30..5c4a020 100755 (executable)
--- a/ConfigSamples/AzteegX5Mini/config
+++ b/ConfigSamples/AzteegX5Mini/config
@@ -162,6 +162,7 @@ zprobe.probe_height                          5               # how much above be
 #leveling-strategy.three-point-leveling.home_first     true        # home the XY axis before probing
 #leveling-strategy.three-point-leveling.tolerance      0.03        # the probe tolerance in mm, anything less that this will be ignored, default is 0.03mm
 #leveling-strategy.three-point-leveling.probe_offsets  0,0,0       # the probe offsets from nozzle, must be x,y,z, default is no offset
+#leveling-strategy.three-point-leveling.save_plane     false       # set to true to allow the bed plane to be saved with M500 default is false
 
 
 # Pause button

diff --git a/ConfigSamples/Smoothieboard/config b/ConfigSamples/Smoothieboard/config
index 259a4a2..b91ddfb 100644 (file)
--- a/ConfigSamples/Smoothieboard/config
+++ b/ConfigSamples/Smoothieboard/config
@@ -245,6 +245,7 @@ zprobe.probe_height                          5               # how much above be
 #leveling-strategy.three-point-leveling.home_first     true        # home the XY axis before probing
 #leveling-strategy.three-point-leveling.tolerance      0.03        # the probe tolerance in mm, anything less that this will be ignored, default is 0.03mm
 #leveling-strategy.three-point-leveling.probe_offsets  0,0,0       # the probe offsets from nozzle, must be x,y,z, default is no offset
+#leveling-strategy.three-point-leveling.save_plane     false       # set to true to allow the bed plane to be saved with M500 default is false
 
 # Pause button
 pause_button_enable                          true             #

diff --git a/src/modules/communication/utils/Gcode.cpp b/src/modules/communication/utils/Gcode.cpp
index 71810fc..328ad93 100644 (file)
--- a/src/modules/communication/utils/Gcode.cpp
+++ b/src/modules/communication/utils/Gcode.cpp
@@ -112,6 +112,23 @@ int Gcode::get_int( char letter, char **ptr ) const
     return 0;
 }
 
+uint32_t Gcode::get_uint( char letter, char **ptr ) const
+{
+    const char *cs = command;
+    char *cn = NULL;
+    for (; *cs; cs++) {
+        if( letter == *cs ) {
+            cs++;
+            int r = strtoul(cs, &cn, 10);
+            if(ptr != nullptr) *ptr= cn;
+            if (cn > cs)
+                return r;
+        }
+    }
+    if(ptr != nullptr) *ptr= nullptr;
+    return 0;
+}
+
 int Gcode::get_num_args() const
 {
     int count = 0;

diff --git a/src/modules/communication/utils/Gcode.h b/src/modules/communication/utils/Gcode.h
index 029c964..ed5b93a 100644 (file)
--- a/src/modules/communication/utils/Gcode.h
+++ b/src/modules/communication/utils/Gcode.h
@@ -25,6 +25,7 @@ class Gcode {
         bool has_letter ( char letter ) const;
         float get_value ( char letter, char **ptr= nullptr ) const;
         int get_int ( char letter, char **ptr= nullptr ) const;
+        uint32_t get_uint ( char letter, char **ptr= nullptr ) const;
         int get_num_args() const;
         void mark_as_taken();
         void strip_parameters();

diff --git a/src/modules/tools/zprobe/Plane3D.cpp b/src/modules/tools/zprobe/Plane3D.cpp
index cbe3211..ee38e9b 100644 (file)
--- a/src/modules/tools/zprobe/Plane3D.cpp
+++ b/src/modules/tools/zprobe/Plane3D.cpp
@@ -15,6 +15,26 @@ Plane3D::Plane3D(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3)
     d = -dv[0] - dv[1] - dv[2];
 }
 
+typedef union { float f; uint32_t u; } conv_t;
+// ctor used to restore a saved plane
+Plane3D::Plane3D(uint32_t a, uint32_t b, uint32_t c, uint32_t d)
+{
+    conv_t ca, cb, cc, cd;
+    ca.u= a; cb.u= b; cc.u= c; cd.u= d;
+    this->normal.set(ca.f, cb.f, cc.f);
+    this->d= cd.f;
+}
+
+void Plane3D::encode(uint32_t& a, uint32_t& b, uint32_t& c, uint32_t& d)
+{
+    conv_t ca, cb, cc, cd;
+    ca.f= this->normal[0];
+    cb.f= this->normal[1];
+    cc.f= this->normal[2];
+    cd.f= this->d;
+    a= ca.u; b= cb.u; c= cc.u; d= cd.u;
+}
+
 // solve for z given x and y
 // z= (-ax - by - d)/c
 float Plane3D::getz(float x, float y)

diff --git a/src/modules/tools/zprobe/Plane3D.h b/src/modules/tools/zprobe/Plane3D.h
index 2fc69c2..2dbc009 100644 (file)
--- a/src/modules/tools/zprobe/Plane3D.h
+++ b/src/modules/tools/zprobe/Plane3D.h
@@ -3,6 +3,8 @@
 
 #include "Vector3.h"
 
+#include <stdint.h>
+
 // define a plane given three points
 class Plane3D
 {
@@ -12,11 +14,10 @@ private:
 
 public:
     Plane3D(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3);
+    Plane3D(uint32_t a, uint32_t b, uint32_t c, uint32_t d);
     float getz(float x, float y);
     Vector3 getNormal() const;
-    // TODO to save plane
-    // string encode() const;
-    // void decode(const char *);
+    void encode(uint32_t& a, uint32_t& b, uint32_t& c, uint32_t& d);
 };
 
 #endif

diff --git a/src/modules/tools/zprobe/ThreePointStrategy.cpp b/src/modules/tools/zprobe/ThreePointStrategy.cpp
index bda1bbd..5ca4ddf 100644 (file)
--- a/src/modules/tools/zprobe/ThreePointStrategy.cpp
+++ b/src/modules/tools/zprobe/ThreePointStrategy.cpp
@@ -76,6 +76,7 @@
 #define probe_offsets_checksum       CHECKSUM("probe_offsets")
 #define home_checksum                CHECKSUM("home_first")
 #define tolerance_checksum           CHECKSUM("tolerance")
+#define save_plane_checksum          CHECKSUM("save_plane")
 
 ThreePointStrategy::ThreePointStrategy(ZProbe *zprobe) : LevelingStrategy(zprobe)
 {
@@ -106,6 +107,7 @@ bool ThreePointStrategy::handleConfig()
 
     this->home= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, home_checksum)->by_default(true)->as_bool();
     this->tolerance= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, tolerance_checksum)->by_default(0.03F)->as_number();
+    this->save= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, save_plane_checksum)->by_default(false)->as_bool();
     return true;
 }
 
@@ -147,11 +149,23 @@ bool ThreePointStrategy::handleGcode(Gcode *gcode)
             }
             return true;
 
-        } else if(gcode->m == 561) { // M561: Set Identity Transform
+        } else if(gcode->m == 561) { // M561: Set Identity Transform with no parameters, set the saved plane if A B C D are given
             delete this->plane;
-            this->plane= nullptr;
-            // delete the adjustZfnc in robot
-            setAdjustFunction(false);
+            if(gcode->get_num_args() == 0) {
+                this->plane= nullptr;
+                // delete the adjustZfnc in robot
+                setAdjustFunction(false);
+            }else{
+                // smoothie specific way to restire a saved plane
+                uint32_t a,b,c,d;
+                a=b=c=d= 0;
+                if(gcode->has_letter('A')) a = gcode->get_uint('A');
+                if(gcode->has_letter('B')) b = gcode->get_uint('B');
+                if(gcode->has_letter('C')) c = gcode->get_uint('C');
+                if(gcode->has_letter('D')) d = gcode->get_uint('D');
+                this->plane= new Plane3D(a, b, c, d);
+
+            }
             return true;
 
         } else if(gcode->m == 565) { // M565: Set Z probe offsets
@@ -162,7 +176,7 @@ bool ThreePointStrategy::handleGcode(Gcode *gcode)
             probe_offsets = std::make_tuple(x, y, z);
             return true;
 
-        } else if(gcode->m == 503) {
+        } else if(gcode->m == 500 || gcode->m == 503) { // M500 save, M503 display
             float x, y, z;
             gcode->stream->printf(";Probe points:\n");
             for (int i = 0; i < 3; ++i) {
@@ -173,7 +187,12 @@ bool ThreePointStrategy::handleGcode(Gcode *gcode)
             std::tie(x, y, z) = probe_offsets;
             gcode->stream->printf("M565 X%1.5f Y%1.5f Z%1.5f\n", x, y, z);
 
-            // TODO encode plane if set and M500
+            // encode plane and save if set and M500 and enabled
+            if(this->save && this->plane != nullptr && gcode->m == 500) {
+                uint32_t a, b, c, d;
+                this->plane->encode(a, b, c, d);
+                gcode->stream->printf(";Saved bed plane:\nM561 A%lu B%lu C%lu D%lu \n", a, b, c, d);
+            }
             return true;
 
         }

diff --git a/src/modules/tools/zprobe/ThreePointStrategy.h b/src/modules/tools/zprobe/ThreePointStrategy.h
index 017e4ac..c671442 100644 (file)
--- a/src/modules/tools/zprobe/ThreePointStrategy.h
+++ b/src/modules/tools/zprobe/ThreePointStrategy.h
@@ -32,6 +32,7 @@ private:
     Plane3D *plane;
     struct {
         bool home:1;
+        bool save:1;
     };
     float tolerance;
 };