Generic Modbus VFD (#1429)

* Generic VFD initial version - works with YL620 * WIP After Huanyang fixes * _speed_ -> _rpm_ in some config names, added _retries * Added % scaler --------- Co-authored-by: bdring <[email protected]>
bdring · Jan 19, 2025 · a86a3fe · a86a3fe
1 parent 597a4e9
commit a86a3fe
Show file tree

Hide file tree

Showing 20 changed files with 414 additions and 89 deletions.
diff --git a/FluidNC/src/Report.cpp b/FluidNC/src/Report.cpp
@@ -628,7 +628,7 @@ void hex_msg(uint8_t* buf, const char* prefix, int len) {
     char temp[20];
     sprintf(report, "%s", prefix);
     for (int i = 0; i < len; i++) {
-        sprintf(temp, " 0x%02X", buf[i]);
+        sprintf(temp, " %02X", buf[i]);
         strcat(report, temp);
     }
 

diff --git a/FluidNC/src/Spindles/VFD/DanfossVLT2800Protocol.h b/FluidNC/src/Spindles/VFD/DanfossVLT2800Protocol.h
@@ -60,7 +60,9 @@ namespace Spindles {
             void direction_command(SpindleState mode, ModbusCommand& data) override;
             void set_speed_command(uint32_t rpm, ModbusCommand& data) override;
 
-            response_parser initialization_sequence(int index, ModbusCommand& data) { return get_status_ok_and_init(data, true); }
+            response_parser initialization_sequence(int index, ModbusCommand& data, VFDSpindle* vfd) {
+                return get_status_ok_and_init(data, true);
+            }
             response_parser get_current_speed(ModbusCommand& data) override;
             response_parser get_current_direction(ModbusCommand& data) override { return nullptr; };
             response_parser get_status_ok(ModbusCommand& data) override { return get_status_ok_and_init(data, false); }

diff --git a/FluidNC/src/Spindles/VFD/GenericProtocol.cpp b/FluidNC/src/Spindles/VFD/GenericProtocol.cpp
@@ -0,0 +1,268 @@
+// Copyright (c) 2021 -  Marco Wagner
+// Use of this source code is governed by a GPLv3 license that can be found in the LICENSE file.
+
+#include "GenericProtocol.h"
+
+#include "../VFDSpindle.h"
+
+#include "src/string_util.h"
+#include <algorithm>
+
+namespace Spindles {
+    namespace VFD {
+        bool split(std::string_view& input, std::string_view& token, const char* delims) {
+            if (input.size() == 0) {
+                return false;
+            }
+            auto pos = input.find_first_of(delims);
+            if (pos != std::string_view::npos) {
+                token = input.substr(0, pos);
+                input = input.substr(pos + 1);
+            } else {
+                token = input;
+                input = "";
+            }
+            return true;
+        }
+
+        bool from_decimal(std::string_view str, uint32_t& value) {
+            value = 0;
+            if (str.size() == 0) {
+                return false;
+            }
+            while (str.size()) {
+                if (!isdigit(str[0])) {
+                    return false;
+                }
+                value = value * 10 + str[0] - '0';
+                str   = str.substr(1);
+            }
+            return true;
+        }
+
+        void scale(uint32_t& n, std::string_view scale_str, uint32_t maxRPM) {
+            if (scale_str.empty()) {
+                return;
+            }
+            if (scale_str[0] == '%') {
+                scale_str.remove_prefix(1);
+                n *= 100;
+                n /= maxRPM;
+            }
+            if (scale_str[0] == '*') {
+                std::string_view numerator_str;
+                scale_str = scale_str.substr(1);
+                split(scale_str, numerator_str, "/");
+                uint32_t numerator;
+                if (from_decimal(numerator_str, numerator)) {
+                    n *= numerator;
+                } else {
+                    log_error(spindle->name() << ": bad decimal number " << numerator_str);
+                    return;
+                }
+                if (!scale_str.empty()) {
+                    uint32_t denominator;
+                    if (from_decimal(scale_str, denominator)) {
+                        n /= denominator;
+                    } else {
+                        log_error(spindle->name() << ": bad decimal number " << scale_str);
+                        return;
+                    }
+                }
+            } else if (scale_str[0] == '/') {
+                std::string_view denominator_str(scale_str.substr(1));
+                uint32_t         denominator;
+                if (from_decimal(denominator_str, denominator)) {
+                    n /= denominator;
+                } else {
+                    log_error(spindle->name() << ": bad decimal number " << scale_str);
+                    return;
+                }
+            }
+        }
+
+        bool from_xdigit(char c, uint8_t& value) {
+            if (isdigit(c)) {
+                value = c - '0';
+                return true;
+            }
+            c = tolower(c);
+            if (c >= 'a' && c <= 'f') {
+                value = 10 + c - 'a';
+                return true;
+            }
+            return false;
+        }
+
+        bool from_hex(std::string_view str, uint8_t& value) {
+            value = 0;
+            if (str.size() == 0 || str.size() > 2) {
+                return false;
+            }
+            uint8_t x;
+            while (str.size()) {
+                value <<= 4;
+                if (!from_xdigit(str[0], x)) {
+                    return false;
+                }
+                value += x;
+                str = str.substr(1);
+            }
+            return true;
+        }
+        bool set_data(std::string_view token, std::basic_string_view<uint8_t>& response_view, const char* name, uint32_t& data) {
+            if (string_util::starts_with_ignore_case(token, name)) {
+                uint32_t rval  = (response_view[0] << 8) + (response_view[1] & 0xff);
+                uint32_t orval = rval;
+                scale(rval, token.substr(strlen(name)), 1);
+                data = rval;
+                response_view.remove_prefix(2);
+                return true;
+            }
+            return false;
+        }
+        bool GenericProtocol::parser(const uint8_t* response, VFDSpindle* spindle, GenericProtocol* instance) {
+            // This routine does not know the actual length of the response array
+            std::basic_string_view<uint8_t> response_view(response, VFD_RS485_MAX_MSG_SIZE);
+            response_view.remove_prefix(1);  // Remove the modbus ID which has already been checked
+
+            std::string_view token;
+            std::string_view format(_response_format);
+            while (split(format, token, " ")) {
+                uint8_t val;
+                if (token == "") {
+                    // Ignore repeated blanks
+                    continue;
+                }
+                if (set_data(token, response_view, "rpm", spindle->_sync_dev_speed)) {
+                    continue;
+                }
+                if (set_data(token, response_view, "minrpm", instance->_minRPM)) {
+                    log_debug(spindle->name() << ": got minRPM " << instance->_minRPM);
+                    continue;
+                }
+                if (set_data(token, response_view, "maxrpm", instance->_maxRPM)) {
+                    log_debug(spindle->name() << ": got maxRPM " << instance->_maxRPM);
+                    continue;
+                }
+                if (from_hex(token, val)) {
+                    if (val != response_view[0]) {
+                        log_debug(spindle->name() << ": response mismatch - expected " << to_hex(val) << " got " << to_hex(response_view[0]));
+                        return false;
+                    }
+                    response_view.remove_prefix(1);
+                    continue;
+                }
+                log_error(spindle->name() << ": bad response token " << token);
+                return false;
+            }
+            return true;
+        }
+        void GenericProtocol::send_vfd_command(const std::string cmd, ModbusCommand& data, uint32_t out) {
+            data.tx_length = 1;
+            data.rx_length = 1;
+            if (cmd.empty()) {
+                return;
+            }
+
+            std::string_view out_view;
+            std::string_view in_view(cmd);
+            split(in_view, out_view, ">");
+            _response_format = in_view;  // Remember the response format for the parser
+
+            std::string_view token;
+            while (data.tx_length < (VFD_RS485_MAX_MSG_SIZE - 3) && split(out_view, token, " ")) {
+                if (token == "") {
+                    // Ignore repeated blanks
+                    continue;
+                }
+                if (string_util::starts_with_ignore_case(token, "rpm")) {
+                    uint32_t oout = out;
+                    scale(out, token.substr(strlen("rpm")), _maxRPM);
+                    data.msg[data.tx_length++] = out >> 8;
+                    data.msg[data.tx_length++] = out & 0xff;
+                } else if (from_hex(token, data.msg[data.tx_length])) {
+                    ++data.tx_length;
+                } else {
+                    log_error(spindle->name() << ":Bad hex number " << token);
+                    return;
+                }
+            }
+            while (data.rx_length < (VFD_RS485_MAX_MSG_SIZE - 3) && split(in_view, token, " ")) {
+                if (token == "") {
+                    // Ignore repeated spaces
+                    continue;
+                }
+                uint8_t x;
+                if (string_util::equal_ignore_case(token, "echo")) {
+                    data.rx_length = data.tx_length;
+                    break;
+                }
+                if (string_util::starts_with_ignore_case(token, "rpm") || string_util::starts_with_ignore_case(token, "minrpm") ||
+                    string_util::starts_with_ignore_case(token, "maxrpm")) {
+                    data.rx_length += 2;
+                } else if (from_hex(token, x)) {
+                    ++data.rx_length;
+                } else {
+                    log_error(spindle->name() << ": bad hex number " << token);
+                }
+            }
+        }
+        void GenericProtocol::direction_command(SpindleState mode, ModbusCommand& data) {
+            switch (mode) {
+                case SpindleState::Cw:
+                    send_vfd_command(_cw_cmd, data, 0);
+                    break;
+                case SpindleState::Ccw:
+                    send_vfd_command(_ccw_cmd, data, 0);
+                    break;
+                default:  // SpindleState::Disable
+                    send_vfd_command(_off_cmd, data, 0);
+                    break;
+            }
+        }
+
+        void GenericProtocol::set_speed_command(uint32_t speed, ModbusCommand& data) {
+            send_vfd_command(_set_rpm_cmd, data, speed);
+        }
+
+        VFDProtocol::response_parser GenericProtocol::get_current_speed(ModbusCommand& data) {
+            send_vfd_command(_get_rpm_cmd, data, 0);
+            return [](const uint8_t* response, VFDSpindle* spindle, VFDProtocol* protocol) -> bool {
+                auto instance = static_cast<GenericProtocol*>(protocol);
+                return instance->parser(response, spindle, instance);
+            };
+        }
+
+        void GenericProtocol::setup_speeds(VFDSpindle* vfd) {
+            vfd->shelfSpeeds(_minRPM, _maxRPM);
+            vfd->setupSpeeds(_maxRPM);
+            vfd->_slop = 300;
+        }
+        VFDProtocol::response_parser GenericProtocol::initialization_sequence(int index, ModbusCommand& data, VFDSpindle* vfd) {
+            if (_maxRPM == 0xffffffff && !_get_max_rpm_cmd.empty()) {
+                send_vfd_command(_get_max_rpm_cmd, data, 0);
+                return [](const uint8_t* response, VFDSpindle* spindle, VFDProtocol* protocol) -> bool {
+                    auto instance = static_cast<GenericProtocol*>(protocol);
+                    return instance->parser(response, spindle, instance);
+                };
+            }
+            if (_minRPM == 0xffffffff && !_get_min_rpm_cmd.empty()) {
+                send_vfd_command(_get_min_rpm_cmd, data, 0);
+                return [](const uint8_t* response, VFDSpindle* spindle, VFDProtocol* protocol) -> bool {
+                    auto instance = static_cast<GenericProtocol*>(protocol);
+                    return instance->parser(response, spindle, instance);
+                };
+            }
+            if (vfd->_speeds.size() == 0) {
+                setup_speeds(vfd);
+            }
+            return nullptr;
+        }
+
+        // Configuration registration
+        namespace {
+            SpindleFactory::DependentInstanceBuilder<VFDSpindle, GenericProtocol> registration("ModbusVFD");
+        }
+    }
+}
diff --git a/FluidNC/src/Spindles/VFD/GenericProtocol.h b/FluidNC/src/Spindles/VFD/GenericProtocol.h
@@ -0,0 +1,57 @@
+// Copyright (c) 2024 -	Mitch Bradley
+// Use of this source code is governed by a GPLv3 license that can be found in the LICENSE file.
+
+#pragma once
+
+#include "VFDProtocol.h"
+
+namespace Spindles {
+    namespace VFD {
+        class GenericProtocol : public VFDProtocol, Configuration::Configurable {
+        protected:
+            void direction_command(SpindleState mode, ModbusCommand& data) override;
+            void set_speed_command(uint32_t dev_speed, ModbusCommand& data) override;
+
+            response_parser initialization_sequence(int index, ModbusCommand& data, VFDSpindle* vfd) override;
+            response_parser get_current_speed(ModbusCommand& data) override;
+            response_parser get_current_direction(ModbusCommand& data) override { return nullptr; };
+            response_parser get_status_ok(ModbusCommand& data) override { return nullptr; }
+
+            std::string _cw_cmd;
+            std::string _ccw_cmd;
+            std::string _off_cmd;
+            std::string _set_rpm_cmd;
+            std::string _get_min_rpm_cmd;
+            std::string _get_max_rpm_cmd;
+            std::string _get_rpm_cmd;
+
+            bool use_delay_settings() const override { return _get_rpm_cmd.empty(); }
+            bool safety_polling() const override { return false; }
+
+        private:
+            std::string _model;  // VFD Model name
+            uint32_t*   _response_data;
+            uint32_t    _minRPM = 0xffffffff;
+            uint32_t    _maxRPM = 0xffffffff;
+            bool        parser(const uint8_t* response, VFDSpindle* spindle, GenericProtocol* protocol);
+            void        send_vfd_command(const std::string cmd, ModbusCommand& data, uint32_t out);
+            std::string _response_format;
+            void        setup_speeds(VFDSpindle* vfd);
+
+        public:
+            void group(Configuration::HandlerBase& handler) override {
+                handler.item("model", _model);
+                handler.item("min_RPM", _minRPM, 0xffffffff);
+                handler.item("max_RPM", _maxRPM, 0xffffffff);
+                handler.item("cw_cmd", _cw_cmd);
+                handler.item("ccw_cmd", _ccw_cmd);
+                handler.item("off_cmd", _off_cmd);
+                handler.item("set_rpm_cmd", _set_rpm_cmd);
+                handler.item("get_min_rpm_cmd", _get_min_rpm_cmd);
+                handler.item("get_max_rpm_cmd", _get_max_rpm_cmd);
+                handler.item("get_rpm_cmd", _get_rpm_cmd);
+            }
+        };
+
+    }
+}
diff --git a/FluidNC/src/Spindles/VFD/H100Protocol.cpp b/FluidNC/src/Spindles/VFD/H100Protocol.cpp
@@ -70,7 +70,7 @@ namespace Spindles {
         }
 
         // This gets data from the VFD. It does not set any values
-        VFDProtocol::response_parser H100Protocol::initialization_sequence(int index, ModbusCommand& data) {
+        VFDProtocol::response_parser H100Protocol::initialization_sequence(int index, ModbusCommand& data, VFDSpindle* vfd) {
             // NOTE: data length is excluding the CRC16 checksum.
             data.tx_length = 6;
             data.rx_length = 5;

diff --git a/FluidNC/src/Spindles/VFD/H100Protocol.h b/FluidNC/src/Spindles/VFD/H100Protocol.h
@@ -20,7 +20,7 @@ namespace Spindles {
             void direction_command(SpindleState mode, ModbusCommand& data) override;
             void set_speed_command(uint32_t rpm, ModbusCommand& data) override;
 
-            response_parser initialization_sequence(int index, ModbusCommand& data) override;
+            response_parser initialization_sequence(int index, ModbusCommand& data, VFDSpindle* vfd) override;
             response_parser get_status_ok(ModbusCommand& data) override { return nullptr; }
             response_parser get_current_speed(ModbusCommand& data) override;
 

diff --git a/FluidNC/src/Spindles/VFD/H2AProtocol.cpp b/FluidNC/src/Spindles/VFD/H2AProtocol.cpp
@@ -56,7 +56,7 @@ namespace Spindles {
             data.msg[5] = speed & 0xFF;
         }
 
-        VFDProtocol::response_parser H2AProtocol::initialization_sequence(int index, ModbusCommand& data) {
+        VFDProtocol::response_parser H2AProtocol::initialization_sequence(int index, ModbusCommand& data, VFDSpindle* vfd) {
             if (index == -1) {
                 data.tx_length = 6;
                 data.rx_length = 8;

diff --git a/FluidNC/src/Spindles/VFD/H2AProtocol.h b/FluidNC/src/Spindles/VFD/H2AProtocol.h
@@ -12,7 +12,7 @@ namespace Spindles {
             void direction_command(SpindleState mode, ModbusCommand& data) override;
             void set_speed_command(uint32_t dev_speed, ModbusCommand& data) override;
 
-            response_parser initialization_sequence(int index, ModbusCommand& data) override;
+            response_parser initialization_sequence(int index, ModbusCommand& data, VFDSpindle* vfd) override;
             response_parser get_current_speed(ModbusCommand& data) override;
             response_parser get_current_direction(ModbusCommand& data) override;
             response_parser get_status_ok(ModbusCommand& data) override { return nullptr; }

diff --git a/FluidNC/src/Spindles/VFD/HuanyangProtocol.cpp b/FluidNC/src/Spindles/VFD/HuanyangProtocol.cpp
@@ -210,7 +210,7 @@ namespace Spindles {
         }
 
         // This gets data from the VFS. It does not set any values
-        VFDProtocol::response_parser HuanyangProtocol::initialization_sequence(int index, ModbusCommand& data) {
+        VFDProtocol::response_parser HuanyangProtocol::initialization_sequence(int index, ModbusCommand& data, VFDSpindle* vfd) {
             // NOTE: data length is excluding the CRC16 checksum.
             data.tx_length = 6;
             data.rx_length = 6;