Add numbered paramter 5399 - M66 input read

bdring · Sep 24, 2024 · caebc4a · caebc4a
1 parent 66227f2
commit caebc4a
Show file tree

Hide file tree

Showing 4 changed files with 89 additions and 46 deletions.
diff --git a/FluidNC/src/Error.cpp b/FluidNC/src/Error.cpp
@@ -88,4 +88,5 @@ const std::map<Error, const char*> ErrorNames = {
     { Error::FlowControlOutOfMemory, "Flow Control Out of Memory" },
     { Error::FlowControlStackOverflow, "Flow Control Stack Overflow" },
     { Error::ParameterAssignmentFailed, "Parameter Assignment Failed" },
+    { Error::GcodeValueWordInvalid, "Gcode invalid word value" },
 };
diff --git a/FluidNC/src/GCode.cpp b/FluidNC/src/GCode.cpp
@@ -2011,11 +2011,18 @@ template <class... Ts>
 overloaded(Ts...) -> overloaded<Ts...>;
 
 static Error gc_wait_on_input(bool is_digital, uint8_t input_number, WaitOnInputMode mode, float timeout) {
+    // TODO - only Immediate read mode is supported
     if (mode == WaitOnInputMode::Immediate) {
         auto const result = is_digital ? config->_userInputs->readDigitalInput(input_number) :
                                          config->_userInputs->readAnalogInput(input_number);
-        return std::visit(overloaded { [&](bool result) { return Error::Ok; }, [&](Error error) { return error; } }, result);
+        auto const on_ok  = [&](bool result) {
+            set_numbered_param(5399, result ? 1.0 : 0.0);
+            return Error::Ok;
+        };
+        auto const on_error = [&](Error error) { return error; };
+        return std::visit(overloaded { on_ok, on_error }, result);
     }
 
+    // TODO - implement rest of modes
     return Error::GcodeValueWordInvalid;
 }
diff --git a/FluidNC/src/Parameters.cpp b/FluidNC/src/Parameters.cpp
@@ -17,14 +17,41 @@
 
 #include "Expression.h"
 
+// See documentation "4.1. Numbered Parameters" for list of numbered parameters
+// that LinuxCNC supports.
+// http://wiki.fluidnc.com/en/features/gcode_parameters_expressions
+// https://linuxcnc.org/docs/stable/html/gcode/overview.html#sub:numbered-parameters
+
 // clang-format off
 const std::map<const int, bool *> bool_params = {
     { 5070, &probe_succeeded },
-    // { 5399, &m66okay },
 };
-typedef int ngc_param_id_t;
 
-std::map<const ngc_param_id_t, float> user_params = {};
+std::map<const ngc_param_id_t, float> float_params = {};
+
+static bool can_write_float_param(ngc_param_id_t id) {
+    if (id == 5399) {
+        // M66 last immediate read input result
+        return true;
+    }
+    if(id >= 1 && id <= 5000) {
+        // User parameters
+        return true;
+    }
+    return false;
+}
+
+static bool can_read_float_param(ngc_param_id_t id) {
+    if (id == 5399) {
+        // M66
+        return true;
+    }
+    if (id >= 31 && id <= 5000) {
+        // User parameters
+        return true;
+    }
+    return false;
+}
 
 const std::map<const ngc_param_id_t, CoordIndex> axis_params = {
     { 5161, CoordIndex::G28 },
@@ -42,7 +69,6 @@ const std::map<const ngc_param_id_t, CoordIndex> axis_params = {
     // { 5401, CoordIndex::TLO },
 };
 
-
 const std::map<const std::string, int> work_positions = {
     { "_x", 0 },
     { "_y", 1 },
@@ -101,40 +127,6 @@ static float to_mm(int axis, float value) {
     }
     return value;
 }
-bool set_numbered_param(ngc_param_id_t id, float value) {
-    int axis;
-    for (auto const& [key, coord_index] : axis_params) {
-        axis = id - key;
-        if (is_axis(axis)) {
-            coords[coord_index]->set(axis, to_mm(axis, value));
-            gc_ngc_changed(coord_index);
-            return true;
-        }
-    }
-    // Non-volatile G92
-    axis = id - 5211;
-    if (is_axis(axis)) {
-        gc_state.coord_offset[axis] = to_mm(axis, value);
-        gc_ngc_changed(CoordIndex::G92);
-        return true;
-    }
-    if (id == 5220) {
-        gc_state.modal.coord_select = static_cast<CoordIndex>(value);
-        return true;
-    }
-    if (id == 5400) {
-        gc_state.selected_tool = static_cast<uint32_t>(value);
-        return true;
-    }
-    if (id >= 1 && id <= 5000) {
-        // 1-30 are for subroutine arguments, but since we don't
-        // implement subroutines, we treat them the same as user params
-        user_params[id] = value;
-        return true;
-    }
-    log_info("N " << id << " is not found");
-    return false;
-}
 
 bool get_numbered_param(ngc_param_id_t id, float& result) {
     int axis;
@@ -181,20 +173,25 @@ bool get_numbered_param(ngc_param_id_t id, float& result) {
         return true;
     }
 
-    for (const auto& [key, valuep] : bool_params) {
-        if (key == id) {
-            result = *valuep;
+    if (auto param = bool_params.find(id); param != bool_params.end()) {
+        result = *param->second ? 1.0 : 0.0;
+        return true;
+    }
+
+    if (can_read_float_param(id)) {
+        if (auto param = float_params.find(id); param != float_params.end()) {
+            result = param->second;
             return true;
+        } else {
+            log_info("param #" << id << " is not found");
+            return false;
         }
     }
-    if (id >= 31 && id <= 5000) {
-        result = user_params[id];
-        return true;
-    }
 
     return false;
 }
 
+// TODO - make this a variant?
 struct param_ref_t {
     std::string    name;  // If non-empty, the parameter is named
     ngc_param_id_t id;    // Valid if name is empty
@@ -486,6 +483,39 @@ bool set_named_param(const std::string& name, float value) {
     return true;
 }
 
+bool set_numbered_param(ngc_param_id_t id, float value) {
+    int axis;
+    for (auto const& [key, coord_index] : axis_params) {
+        axis = id - key;
+        if (is_axis(axis)) {
+            coords[coord_index]->set(axis, to_mm(axis, value));
+            gc_ngc_changed(coord_index);
+            return true;
+        }
+    }
+    // Non-volatile G92
+    axis = id - 5211;
+    if (is_axis(axis)) {
+        gc_state.coord_offset[axis] = to_mm(axis, value);
+        gc_ngc_changed(CoordIndex::G92);
+        return true;
+    }
+    if (id == 5220) {
+        gc_state.modal.coord_select = static_cast<CoordIndex>(value);
+        return true;
+    }
+    if (id == 5400) {
+        gc_state.selected_tool = static_cast<uint32_t>(value);
+        return true;
+    }
+    if (can_write_float_param(id)) {
+        float_params[id] = value;
+        return true;
+    }
+    log_info("param #" << id << " is not found");
+    return false;
+}
+
 bool set_param(const param_ref_t& param_ref, float value) {
     if (param_ref.name.length()) {  // Named parameter
         auto name = param_ref.name;

diff --git a/FluidNC/src/Parameters.h b/FluidNC/src/Parameters.h
@@ -6,8 +6,13 @@
 #include <stddef.h>
 #include <string>
 
+// TODO - make ngc_param_id_t an enum, give names to numbered parameters where
+// possible
+typedef int ngc_param_id_t;
+
 bool assign_param(const char* line, size_t& pos);
 bool read_number(const char* line, size_t& pos, float& value, bool in_expression = false);
 bool perform_assignments();
 bool named_param_exists(std::string& name);
 bool set_named_param(const char* name, float value);
+bool set_numbered_param(ngc_param_id_t, float value);