From 7cf191ef7b74e55f0d6605592c1747769f446fdd Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Sat, 18 Nov 2023 14:28:12 -1000 Subject: [PATCH] Rebase merge and add ready() method --- FluidNC/src/Channel.cpp | 96 +++++++++++---------------- FluidNC/src/Channel.h | 59 ++++++++-------- FluidNC/src/HashFS.cpp | 1 - FluidNC/src/Logging.h | 4 +- FluidNC/src/Machine/EventPin.cpp | 3 + FluidNC/src/Machine/LimitPin.cpp | 1 - FluidNC/src/Machine/MachineConfig.cpp | 4 +- FluidNC/src/Pin.cpp | 3 - FluidNC/src/Pins/ChannelPinDetail.cpp | 61 ++++++++--------- FluidNC/src/Pins/ChannelPinDetail.h | 4 +- FluidNC/src/ProcessSettings.cpp | 84 +++++++++++++++++++++++ FluidNC/src/RealtimeCmd.cpp | 18 ++--- FluidNC/src/RealtimeCmd.h | 6 +- FluidNC/src/Report.cpp | 32 ++++----- FluidNC/src/Report.h | 3 + FluidNC/src/Serial.cpp | 17 +++-- FluidNC/src/Serial.h | 1 + FluidNC/src/UartChannel.cpp | 5 +- FluidNC/src/UartChannel.h | 9 ++- FluidNC/src/WebUI/WSChannel.cpp | 31 +++++---- FluidNC/src/WebUI/WSChannel.h | 2 +- FluidNC/src/WebUI/WebServer.cpp | 9 ++- 22 files changed, 259 insertions(+), 194 deletions(-) diff --git a/FluidNC/src/Channel.cpp b/FluidNC/src/Channel.cpp index f1ef1a1e1..11fb2c8b3 100644 --- a/FluidNC/src/Channel.cpp +++ b/FluidNC/src/Channel.cpp @@ -101,17 +101,19 @@ void Channel::autoReportGCodeState() { } void Channel::autoReport() { if (_reportInterval) { - auto limitState = limits_get_state(); auto probeState = config->_probe->get_state(); - if (_reportWco || sys.state != _lastState || limitState != _lastLimits || probeState != _lastProbe || + if (probeState != _lastProbe) { + report_recompute_pin_string(); + } + if (_reportWco || sys.state != _lastState || probeState != _lastProbe || _lastPinString != report_pin_string || (motionState() && (int32_t(xTaskGetTickCount()) - _nextReportTime) >= 0)) { if (_reportWco) { report_wco_counter = 0; } - _reportWco = false; - _lastState = sys.state; - _lastLimits = limitState; - _lastProbe = probeState; + _reportWco = false; + _lastState = sys.state; + _lastProbe = probeState; + _lastPinString = report_pin_string; _nextReportTime = xTaskGetTickCount() + _reportInterval; report_realtime_status(*this); @@ -140,20 +142,12 @@ Channel* Channel::pollLine(char* line) { if (ch < 0) { break; } - if (_last_rt_cmd == Cmd::PinLow) { - try { - auto event_pin = _events.at(ch); - _pin_values[ch] = false; - event_pin->trigger(false); - } catch (std::exception& ex) {} - _last_rt_cmd = Cmd::None; - continue; - } - if (_last_rt_cmd == Cmd::PinHigh) { + if (_last_rt_cmd == Cmd::PinLow || _last_rt_cmd == Cmd::PinHigh) { + bool isHigh = _last_rt_cmd == Cmd::PinHigh; try { - auto event_pin = _events.at(ch); - _pin_values[ch] = true; - event_pin->trigger(true); + auto event_pin = _events.at(ch); + *_pin_values[ch] = isHigh; + event_pin->trigger(isHigh); } catch (std::exception& ex) {} _last_rt_cmd = Cmd::None; continue; @@ -170,12 +164,13 @@ Channel* Channel::pollLine(char* line) { } if (realtimeOkay(ch)) { - if (is_extended_realtime_command(ch)) { - _last_rt_cmd = static_cast(ch); - continue; - } if (is_realtime_command(ch)) { - execute_realtime_command(static_cast(ch), *this); + auto cmd = static_cast(ch); + if (cmd == Cmd::PinLow || cmd == Cmd::PinHigh) { + _last_rt_cmd = cmd; + } else { + execute_realtime_command(cmd, *this); + } continue; } } @@ -193,34 +188,29 @@ Channel* Channel::pollLine(char* line) { return nullptr; } -void Channel::setAttr(int index, Pins::PinAttributes attr) { - _pin_attributes[index] = _pin_attributes[index] | attr; +void Channel::setAttr(int index, bool* value, const std::string& attrString) { + if (value) { + _pin_values[index] = value; + } + while (_ackwait) { + pollLine(NULL); + delay_ms(10); + } + log_msg_to(*this, attrString); + _ackwait = true; + log_debug(attrString); } -Pins::PinAttributes Channel::getAttr(int index) const { - try { - return _pin_attributes.at(index); - } catch (std::exception& ex) { return Pins::PinAttributes::None; } + +void Channel::out(const std::string& s) { + log_msg_to(*this, s); + // _channel->_ackwait = true; + log_debug(s); } -void Channel::out(int index, int value) { - if (value == _pin_values[index]) { - return; +void Channel::ready() { + if (!_pin_values.empty()) { + out("GET: io.*"); } - _pin_values[index] = value; - std::string s = "[MSG:SET: io."; - s += std::to_string(index); - s += "="; - s += std::to_string(value); - s += "]"; - - if (!sendCtrlCmd(s.c_str(), true)) {// send it out - // do something about the NAK - } - - //log_info(s.c_str()); -} -int Channel::in(int index) { - return _pin_values[index]; } void Channel::registerEvent(uint8_t code, EventPin* obj) { @@ -242,13 +232,3 @@ void Channel::ack(Error status) { msg << static_cast(status); } } - -// send a command and optionally wait for an ACK -bool Channel::sendCtrlCmd(std::string cmd, bool need_Ack) { // return false is command was NAK'd - println(cmd.c_str()); - if (need_Ack) { - // need some code here - return true; - } - return true; -} diff --git a/FluidNC/src/Channel.h b/FluidNC/src/Channel.h index b17759db9..04f8fb3fd 100644 --- a/FluidNC/src/Channel.h +++ b/FluidNC/src/Channel.h @@ -34,43 +34,48 @@ class Channel : public Stream { static const int maxLine = 255; protected: - const char* _name; + std::string _name; char _line[maxLine]; size_t _linelen; - bool _addCR = false; - char _lastWasCR = false; - bool _controller = false; // the device is a pin extender + bool _addCR = false; + char _lastWasCR = false; std::queue _queue; uint32_t _reportInterval = 0; int32_t _nextReportTime = 0; - gc_modal_t _lastModal; - uint8_t _lastTool; - float _lastSpindleSpeed; - float _lastFeedRate; - State _lastState; - MotorMask _lastLimits; - bool _lastProbe; + gc_modal_t _lastModal; + uint8_t _lastTool; + float _lastSpindleSpeed; + float _lastFeedRate; + State _lastState; + MotorMask _lastLimits; + bool _lastProbe; + std::string _lastPinString; bool _reportWco = true; CoordIndex _reportNgc = CoordIndex::End; Cmd _last_rt_cmd; - std::map _events; - std::map _pin_values; - std::map _pin_attributes; + std::map _events; + std::map _pin_values; public: Channel(const char* name, bool addCR = false) : _name(name), _linelen(0), _addCR(addCR) {} + Channel(const char* name, int num, bool addCR = false) { + _name = name; + _name += std::to_string(num), _linelen = 0, _addCR = addCR; + } virtual ~Channel() = default; - virtual void handle() {}; - virtual Channel* pollLine(char* line); - virtual void ack(Error status); - const char* name() { return _name; } + bool _ackwait = false; + + virtual void handle() {}; + virtual Channel* pollLine(char* line); + virtual void ack(Error status); + const std::string& name() { return _name; } // rx_buffer_available() is the number of bytes that can be sent without overflowing // a reception buffer, even if the system is busy. Channels that can handle external @@ -113,11 +118,9 @@ class Channel : public Stream { void notifyWco() { _reportWco = true; } void notifyNgc(CoordIndex coord) { _reportNgc = coord; } - int peek() override { return -1; } - int read() override { return -1; } - int available() override { return _queue.size(); } - bool isController() { return _controller; } - bool sendCtrlCmd(std::string s, bool need_Ack); + int peek() override { return -1; } + int read() override { return -1; } + int available() override { return _queue.size(); } uint32_t setReportInterval(uint32_t ms); uint32_t getReportInterval() { return _reportInterval; } @@ -125,10 +128,8 @@ class Channel : public Stream { void autoReportGCodeState(); // Pin extender functions - virtual void out(int index, int value); - virtual int in(int index); - virtual void setAttr(int index, Pins::PinAttributes value); - virtual Pins::PinAttributes getAttr(int index) const; - - virtual void registerEvent(uint8_t code, EventPin* obj); + void out(const std::string& s); + void setAttr(int index, bool* valuep, const std::string& s); + void ready(); + void registerEvent(uint8_t code, EventPin* obj); }; diff --git a/FluidNC/src/HashFS.cpp b/FluidNC/src/HashFS.cpp index 9c780c412..b8ebf1afc 100644 --- a/FluidNC/src/HashFS.cpp +++ b/FluidNC/src/HashFS.cpp @@ -70,7 +70,6 @@ void HashFS::rehash_file(const std::filesystem::path& path) { delete_file(path); } else { localFsHashes[path.filename()] = hash; - log_debug(path.filename() << " hash " << hash); } } } diff --git a/FluidNC/src/Logging.h b/FluidNC/src/Logging.h index 256cfe90f..b5c246903 100644 --- a/FluidNC/src/Logging.h +++ b/FluidNC/src/Logging.h @@ -50,7 +50,7 @@ extern bool atMsgLevel(MsgLevel level); // Note: these '{'..'}' scopes are here for a reason: the destructor should flush. // #define log_bare(prefix, x) { LogStream ss(prefix); ss << x; } -#define log_msg(x) { LogStream ss("[MSG: "); ss << x; } +#define log_msg(x) { LogStream ss("[MSG:"); ss << x; } #define log_verbose(x) if (atMsgLevel(MsgLevelVerbose)) { LogStream ss("[MSG:VRB: "); ss << x; } #define log_debug(x) if (atMsgLevel(MsgLevelDebug)) { LogStream ss("[MSG:DBG: "); ss << x; } #define log_info(x) if (atMsgLevel(MsgLevelInfo)) { LogStream ss("[MSG:INFO: "); ss << x; } @@ -58,7 +58,7 @@ extern bool atMsgLevel(MsgLevel level); #define log_error(x) if (atMsgLevel(MsgLevelError)) { LogStream ss("[MSG:ERR: "); ss << x; } #define log_fatal(x) { LogStream ss("[MSG:FATAL: "); ss << x; Assert(false, "A fatal error occurred."); } -#define log_msg_to(out, x) { LogStream ss(out, "[MSG: "); ss << x; } +#define log_msg_to(out, x) { LogStream ss(out, "[MSG:"); ss << x; } #define log_verbose_to(out, x) if (atMsgLevel(MsgLevelVerbose)) { LogStream ss(out, "[MSG:VRB: "); ss << x; } #define log_debug_to(out, x) if (atMsgLevel(MsgLevelDebug)) { LogStream ss(out, "[MSG:DBG: "); ss << x; } #define log_info_to(out, x) if (atMsgLevel(MsgLevelInfo)) { LogStream ss(out, "[MSG:INFO: "); ss << x; } diff --git a/FluidNC/src/Machine/EventPin.cpp b/FluidNC/src/Machine/EventPin.cpp index 28b5bd250..1ffc251ff 100644 --- a/FluidNC/src/Machine/EventPin.cpp +++ b/FluidNC/src/Machine/EventPin.cpp @@ -1,4 +1,5 @@ #include "EventPin.h" +#include "src/Report.h" #include "src/Protocol.h" // protocol_send_event @@ -6,7 +7,9 @@ EventPin::EventPin(Event* event, const char* legend) : _event(event), _legend(le void EventPin::trigger(bool active) { update(active); + log_debug(_legend << " " << active); if (active) { protocol_send_event(_event, this); } + report_recompute_pin_string(); } diff --git a/FluidNC/src/Machine/LimitPin.cpp b/FluidNC/src/Machine/LimitPin.cpp index 37cb90ce7..986ddd808 100644 --- a/FluidNC/src/Machine/LimitPin.cpp +++ b/FluidNC/src/Machine/LimitPin.cpp @@ -54,7 +54,6 @@ namespace Machine { } void LimitPin::update(bool value) { - log_debug(_legend << " " << value); if (value) { if (Homing::approach() || (sys.state != State::Homing && _pHardLimits)) { _pLimited = value; diff --git a/FluidNC/src/Machine/MachineConfig.cpp b/FluidNC/src/Machine/MachineConfig.cpp index 21f461066..c8c458dbc 100644 --- a/FluidNC/src/Machine/MachineConfig.cpp +++ b/FluidNC/src/Machine/MachineConfig.cpp @@ -42,8 +42,8 @@ namespace Machine { handler.section("uart1", _uarts[1], 1); handler.section("uart2", _uarts[2], 2); - handler.section("uart_channel1", _uart_channels[1]); - handler.section("uart_channel2", _uart_channels[2]); + handler.section("uart_channel1", _uart_channels[1], 1); + handler.section("uart_channel2", _uart_channels[2], 2); handler.section("i2so", _i2so); diff --git a/FluidNC/src/Pin.cpp b/FluidNC/src/Pin.cpp index a2f4b69e9..82c31577e 100644 --- a/FluidNC/src/Pin.cpp +++ b/FluidNC/src/Pin.cpp @@ -77,9 +77,6 @@ const char* Pin::parse(std::string_view pin_str, Pins::PinDetail*& pinImplementa return nullptr; } if (string_util::equal_ignore_case(prefix, "i2so")) { - if (config->_i2so == nullptr) { - return "i2so: section must be defined before using i2so pins"; - } pinImplementation = new Pins::I2SOPinDetail(static_cast(pin_number), parser); return nullptr; } diff --git a/FluidNC/src/Pins/ChannelPinDetail.cpp b/FluidNC/src/Pins/ChannelPinDetail.cpp index 5f5398016..f75d9bcee 100644 --- a/FluidNC/src/Pins/ChannelPinDetail.cpp +++ b/FluidNC/src/Pins/ChannelPinDetail.cpp @@ -8,11 +8,11 @@ namespace Pins { PinDetail(number), _channel(channel) { for (auto opt : options) { if (opt.is("pu")) { - _channel->setAttr(_index, PinAttributes::PullUp); + setAttr(PinAttributes::PullUp); } else if (opt.is("pd")) { - _channel->setAttr(_index, PinAttributes::PullDown); + setAttr(PinAttributes::PullDown); } else if (opt.is("low")) { - _channel->setAttr(_index, PinAttributes::ActiveLow); + setAttr(PinAttributes::ActiveLow); } else if (opt.is("high")) { // Default: Active HIGH. } @@ -24,61 +24,58 @@ namespace Pins { } void ChannelPinDetail::write(int high) { - _channel->out(_index, high); - } - int ChannelPinDetail::read() { - return _channel->in(_index); + if (high == _value) { + return; + } + std::string s = "SET: io."; + s += std::to_string(_index); + s += "="; + s += std::to_string(high); + _channel->out(s); } - void ChannelPinDetail::setAttr(PinAttributes value) { - _channel->setAttr(_index, value); - std::string s = "[MSG:INI: io."; + int ChannelPinDetail::read() { return _value; } + void ChannelPinDetail::setAttr(PinAttributes attr) { + _attributes = _attributes | attr; + + std::string s = "INI: io."; s += std::to_string(_index); s += "="; - if (value.has(PinAttributes::Input)) { + if (_attributes.has(Pins::PinAttributes::Input)) { s += "inp"; - } else if (_channel->getAttr(_index) & PinAttributes::Output) { + } else if (_attributes.has(Pins::PinAttributes::Output)) { s += "out"; } else { return; } - if (_channel->getAttr(_index) & PinAttributes::PullUp) { + if (_attributes.has(Pins::PinAttributes::PullUp)) { s += ":pu"; } - if (_channel->getAttr(_index) & PinAttributes::PullDown) { - s += ":pu"; + if (_attributes.has(Pins::PinAttributes::PullDown)) { + s += ":pd"; } - if (_channel->getAttr(_index) & PinAttributes::ActiveLow) { + if (_attributes.has(Pins::PinAttributes::ActiveLow)) { s += ":low"; } - s += "]"; - - if (!_channel->sendCtrlCmd(s, true)) { - // do something about the NAK - } - //_channel->println(s.c_str()); - } - PinAttributes ChannelPinDetail::getAttr() const { - return _channel->getAttr(_index); + _channel->setAttr(_index, _attributes.has(Pins::PinAttributes::Input) ? &this->_value : nullptr, s); } - std::string ChannelPinDetail::toString() { + PinAttributes ChannelPinDetail::getAttr() const { return _attributes; } + std::string ChannelPinDetail::toString() { std::string s = _channel->name(); s += "."; s += std::to_string(_index); - if (_channel->getAttr(_index) & PinAttributes::ActiveLow) { + if (_attributes.has(PinAttributes::ActiveLow)) { s += ":low"; } - if (_channel->getAttr(_index) & PinAttributes::PullUp) { + if (_attributes.has(PinAttributes::PullUp)) { s += ":pu"; } - if (_channel->getAttr(_index) & PinAttributes::PullDown) { + if (_attributes.has(PinAttributes::PullDown)) { s += ":pd"; } return s; } - void ChannelPinDetail::registerEvent(EventPin* obj) { - _channel->registerEvent(_index, obj); - } + void ChannelPinDetail::registerEvent(EventPin* obj) { _channel->registerEvent(_index, obj); } } diff --git a/FluidNC/src/Pins/ChannelPinDetail.h b/FluidNC/src/Pins/ChannelPinDetail.h index 487779277..82dd1f129 100644 --- a/FluidNC/src/Pins/ChannelPinDetail.h +++ b/FluidNC/src/Pins/ChannelPinDetail.h @@ -11,7 +11,9 @@ namespace Pins { class ChannelPinDetail : public PinDetail { private: - Channel* _channel; + Channel* _channel; + PinAttributes _attributes; + bool _value; public: ChannelPinDetail(Channel* channel, int number, const PinOptionsParser& options); diff --git a/FluidNC/src/ProcessSettings.cpp b/FluidNC/src/ProcessSettings.cpp index 4c2a29dab..02d85e621 100644 --- a/FluidNC/src/ProcessSettings.cpp +++ b/FluidNC/src/ProcessSettings.cpp @@ -298,6 +298,81 @@ static Error report_ngc(const char* value, WebUI::AuthenticationLevel auth_level report_ngc_parameters(out); return Error::Ok; } +static Error msg_to_uart0(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { + if (value) { + Channel* dest = allChannels.find("uart_channel0"); + if (dest) { + log_msg_to(*dest, value); + } + } + return Error::Ok; +} +static Error msg_to_uart1(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { + if (value && config->_uart_channels[1]) { + log_msg_to(*(config->_uart_channels[1]), value); + } + return Error::Ok; +} +static Error cmd_log_msg(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { + if (value) { + if (*value == '*') { + log_msg(value + 1); + } else { + log_msg_to(out, value); + } + } + return Error::Ok; +} +static Error cmd_log_error(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { + if (value) { + if (*value == '*') { + log_error(value + 1); + } else { + log_error_to(out, value); + } + } + return Error::Ok; +} +static Error cmd_log_warn(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { + if (value) { + if (*value == '*') { + log_warn(value + 1); + } else { + log_warn_to(out, value); + } + } + return Error::Ok; +} +static Error cmd_log_info(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { + if (value) { + if (*value == '*') { + log_info(value + 1); + } else { + log_info_to(out, value); + } + } + return Error::Ok; +} +static Error cmd_log_debug(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { + if (value) { + if (*value == '*') { + log_debug(value + 1); + } else { + log_debug_to(out, value); + } + } + return Error::Ok; +} +static Error cmd_log_verbose(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { + if (value) { + if (*value == '*') { + log_verbose(value + 1); + } else { + log_verbose_to(out, value); + } + } + return Error::Ok; +} static Error home(AxisMask axisMask) { if (axisMask != Machine::Homing::AllCycles) { // if not AllCycles we need to make sure the cycle is not prohibited // if there is a cycle it is the axis from $H @@ -796,6 +871,15 @@ void make_user_commands() { new UserCommand("HB", "Home/B", home_b, notIdleOrAlarm); new UserCommand("HC", "Home/C", home_c, notIdleOrAlarm); + new UserCommand("MU0", "Msg/Uart0", msg_to_uart0, anyState); + new UserCommand("MU1", "Msg/Uart1", msg_to_uart1, anyState); + new UserCommand("LM", "Log/Msg", cmd_log_msg, anyState); + new UserCommand("LE", "Log/Error", cmd_log_error, anyState); + new UserCommand("LW", "Log/Warn", cmd_log_warn, anyState); + new UserCommand("LI", "Log/Info", cmd_log_info, anyState); + new UserCommand("LD", "Log/Debug", cmd_log_debug, anyState); + new UserCommand("LV ", "Log/Verbose", cmd_log_verbose, anyState); + new UserCommand("SLP", "System/Sleep", go_to_sleep, notIdleOrAlarm); new UserCommand("I", "Build/Info", get_report_build_info, notIdleOrAlarm); new UserCommand("N", "GCode/StartupLines", show_startup_lines, notIdleOrAlarm); diff --git a/FluidNC/src/RealtimeCmd.cpp b/FluidNC/src/RealtimeCmd.cpp index a8771f3b5..2e6cdc98b 100644 --- a/FluidNC/src/RealtimeCmd.cpp +++ b/FluidNC/src/RealtimeCmd.cpp @@ -6,17 +6,6 @@ #include "System.h" #include "Machine/Macros.h" // macroNEvent -void execute_extended_realtime_command(Cmd command, uint8_t arg, Channel& channel) { - switch (command) { - case Cmd::PinLow: - //protocol_send_event(&pinLowEvent, arg); - break; - case Cmd::PinHigh: - //protocol_send_event(&pinHighEvent, arg); - break; - } -} - // Act upon a realtime character void execute_realtime_command(Cmd command, Channel& channel) { switch (command) { @@ -108,6 +97,12 @@ void execute_realtime_command(Cmd command, Channel& channel) { case Cmd::Macro3: protocol_send_event(¯o3Event); break; + case Cmd::ACK: + channel._ackwait = false; + break; + case Cmd::NAK: + channel._ackwait = false; + break; } } @@ -119,7 +114,6 @@ bool is_realtime_command(uint8_t data) { auto cmd = static_cast(data); return cmd == Cmd::Reset || cmd == Cmd::StatusReport || cmd == Cmd::CycleStart || cmd == Cmd::FeedHold; } - bool is_extended_realtime_command(uint8_t data) { auto cmd = static_cast(data); return cmd == Cmd::PinLow || cmd == Cmd::PinHigh || cmd == Cmd::ACK || cmd == Cmd::NAK; diff --git a/FluidNC/src/RealtimeCmd.h b/FluidNC/src/RealtimeCmd.h index f2e5282ab..e19ef7566 100644 --- a/FluidNC/src/RealtimeCmd.h +++ b/FluidNC/src/RealtimeCmd.h @@ -44,12 +44,11 @@ enum class Cmd : uint8_t { SpindleOvrStop = 0x9E, CoolantFloodOvrToggle = 0xA0, CoolantMistOvrToggle = 0xA1, - // UART Extender PinLow = 0xB0, // Start of two-character sequence; second is event number PinHigh = 0xB1, // Start of two-character sequence; second is event number - NAK = 0xB2, - ACK = 0xB3, + NAK = 0xB2, // IO Expander rejected command + ACK = 0xB3, // IO Expander accepted command }; class Channel; @@ -57,4 +56,3 @@ class Channel; bool is_realtime_command(uint8_t data); bool is_extended_realtime_command(uint8_t data); void execute_realtime_command(Cmd command, Channel& channel); -// void execute_extended_realtime_command(Cmd command, uint8_t arg, Channel& channel); diff --git a/FluidNC/src/Report.cpp b/FluidNC/src/Report.cpp index 9ade3d5ed..ee31f1e03 100644 --- a/FluidNC/src/Report.cpp +++ b/FluidNC/src/Report.cpp @@ -45,6 +45,10 @@ EspClass esp; #endif +volatile bool protocol_pin_changed = false; + +std::string report_pin_string; + portMUX_TYPE mmux = portMUX_INITIALIZER_UNLOCKED; void _notify(const char* title, const char* msg) { @@ -491,15 +495,10 @@ const char* state_name() { return ""; } -static std::string pinString() { - std::string msg; - bool prefixNeeded = true; +void report_recompute_pin_string() { + report_pin_string = ""; if (config->_probe->get_state()) { - if (prefixNeeded) { - prefixNeeded = false; - msg += "|Pn:"; - } - msg += 'P'; + report_pin_string += 'P'; } MotorMask lim_pin_state = limits_get_state(); @@ -508,24 +507,15 @@ static std::string pinString() { for (size_t axis = 0; axis < n_axis; axis++) { if (bitnum_is_true(lim_pin_state, Machine::Axes::motor_bit(axis, 0)) || bitnum_is_true(lim_pin_state, Machine::Axes::motor_bit(axis, 1))) { - if (prefixNeeded) { - prefixNeeded = false; - msg += "|Pn:"; - } - msg += config->_axes->axisName(axis); + report_pin_string += config->_axes->axisName(axis); } } } std::string ctrl_pin_report = config->_control->report_status(); if (ctrl_pin_report.length()) { - if (prefixNeeded) { - prefixNeeded = false; - msg += "|Pn:"; - } - msg += ctrl_pin_report; + report_pin_string += ctrl_pin_report; } - return msg; } // Define this to do something if a debug request comes in over serial @@ -574,7 +564,9 @@ void report_realtime_status(Channel& channel) { } msg << "|FS:" << setprecision(0) << rate << "," << sys.spindle_speed; - msg << pinString(); + if (report_pin_string.length()) { + msg << "|Pn:" << report_pin_string; + } if (report_wco_counter > 0) { report_wco_counter--; diff --git a/FluidNC/src/Report.h b/FluidNC/src/Report.h index 221b9015f..d90ab815e 100644 --- a/FluidNC/src/Report.h +++ b/FluidNC/src/Report.h @@ -98,3 +98,6 @@ extern const char* git_url; void display_init(); extern bool readyNext; + +extern std::string report_pin_string; +void report_recompute_pin_string(); diff --git a/FluidNC/src/Serial.cpp b/FluidNC/src/Serial.cpp index 09aced804..c0c8c1bbb 100644 --- a/FluidNC/src/Serial.cpp +++ b/FluidNC/src/Serial.cpp @@ -90,11 +90,7 @@ void AllChannels::init() { void AllChannels::ready() { for (auto channel : _channelq) { - if (channel->isController()) { - if (!channel->sendCtrlCmd("[MSG:GET: io.*]", true)){ - // do something about the NAK - } - } + channel->ready(); } } @@ -176,6 +172,17 @@ size_t AllChannels::write(const uint8_t* buffer, size_t length) { _mutex_general.unlock(); return length; } +Channel* AllChannels::find(const std::string& name) { + _mutex_general.lock(); + for (auto channel : _channelq) { + if (channel->name() == name) { + _mutex_general.unlock(); + return channel; + } + } + _mutex_general.unlock(); + return nullptr; +} Channel* AllChannels::pollLine(char* line) { Channel* deadChannel; while (xQueueReceive(_killQueue, &deadChannel, 0)) { diff --git a/FluidNC/src/Serial.h b/FluidNC/src/Serial.h index 44d87ead3..c07d16f3c 100644 --- a/FluidNC/src/Serial.h +++ b/FluidNC/src/Serial.h @@ -53,6 +53,7 @@ class AllChannels : public Channel { void listChannels(Channel& out); Channel* pollLine(char* line) override; + Channel* find(const std::string& name); void stopJob() override; }; diff --git a/FluidNC/src/UartChannel.cpp b/FluidNC/src/UartChannel.cpp index 4aff0cf8c..5b8cebba2 100644 --- a/FluidNC/src/UartChannel.cpp +++ b/FluidNC/src/UartChannel.cpp @@ -5,7 +5,7 @@ #include "Machine/MachineConfig.h" // config #include "Serial.h" // allChannels -UartChannel::UartChannel(bool addCR) : Channel("uart", addCR) { +UartChannel::UartChannel(int num, bool addCR) : Channel("uart_channel", num, addCR) { _lineedit = new Lineedit(this, _line, Channel::maxLine - 1); } @@ -22,6 +22,7 @@ void UartChannel::init(Uart* uart) { _uart = uart; allChannels.registration(this); log_info("uart_channel" << _uart_num << " created"); + log_msg_to(*this, "RST"); } size_t UartChannel::write(uint8_t c) { @@ -117,7 +118,7 @@ size_t UartChannel::timedReadBytes(char* buffer, size_t length, TickType_t timeo return length - remlen; } -UartChannel Uart0(true); // Primary serial channel with LF to CRLF conversion +UartChannel Uart0(0, true); // Primary serial channel with LF to CRLF conversion void uartInit() { auto uart0 = new Uart(0); diff --git a/FluidNC/src/UartChannel.h b/FluidNC/src/UartChannel.h index d28163b79..3865529bd 100644 --- a/FluidNC/src/UartChannel.h +++ b/FluidNC/src/UartChannel.h @@ -12,11 +12,11 @@ class UartChannel : public Channel, public Configuration::Configurable { Lineedit* _lineedit; Uart* _uart; - int _uart_num = 0; - int _report_interval_ms = 0; + int _uart_num = 0; + int _report_interval_ms = 0; public: - UartChannel(bool addCR = false); + UartChannel(int num, bool addCR = false); void init(); void init(Uart* uart); @@ -37,13 +37,12 @@ class UartChannel : public Channel, public Configuration::Configurable { size_t timedReadBytes(uint8_t* buffer, size_t length, TickType_t timeout) { return timedReadBytes((char*)buffer, length, timeout); }; bool realtimeOkay(char c) override; bool lineComplete(char* line, char c) override; - Channel* pollLine(char* line) override; + Channel* pollLine(char* line) override; // Configuration methods void group(Configuration::HandlerBase& handler) override { handler.item("report_interval_ms", _report_interval_ms); handler.item("uart_num", _uart_num); - handler.item("controller", _controller); } }; diff --git a/FluidNC/src/WebUI/WSChannel.cpp b/FluidNC/src/WebUI/WSChannel.cpp index 22aa4fae2..c2feeb90c 100644 --- a/FluidNC/src/WebUI/WSChannel.cpp +++ b/FluidNC/src/WebUI/WSChannel.cpp @@ -164,28 +164,33 @@ namespace WebUI { } } - bool WSChannels::runGCode(int pageid, std::string cmd) { + bool WSChannels::runGCode(int pageid, std::string& cmd) { bool has_error = false; WSChannel* wsChannel = getWSChannel(pageid); if (wsChannel) { + if (cmd.length() > 0 && ((cmd[0] & 0xe0) == 0xc0)) { + // In UTF-8, 0x80-0xff is encoded as follows: + // 0b1NMMMMMM -> 0b1100001N 0b10MMMMMM + // The GRBL protocol is 8-bit only, so we handle only the + // UTF-8 encodings for 0x80-0xff, ignoring encodings for + // numbers that will not fit in 8 bits. + uint8_t hibits = cmd[0] & 0x1f; + if (hibits < 4) { + // Decode from UTF-8 to 8-bit binary + cmd.erase(0, 1); + cmd[0] = (hibits << 6) | (cmd[0] & 0x3f); + } + // If hibits >= 4, we pass the byte through un-decoded. + // This probably will not happen, since URLs are supposed to + // be in UTF-8, but we consider it anyway in case of buggy clients. + } // It is very tempting to let wsChannel->push() handle the realtime // character sequences so we don't have to do it here. That does not work // because we need to know whether to add a newline. We should not add newline // on a realtime sequence, but we must add one (if not already present) // on a text command. - if (cmd.length() > 0 && cmd[0] == 0xc2) { - if (cmd.length() == 3 && is_extended_realtime_command(cmd[1])) { - wsChannel->pushRT(cmd[1]); - wsChannel->pushRT(cmd[2]); - } else if (cmd.length() == 3 && is_realtime_command(cmd[1]) && cmd[2] == '\0') { - // Handles old WebUIs that send a null after high-bit-set realtime chars - wsChannel->pushRT(cmd[1]); - } else if (cmd.length() == 2 && is_realtime_command(cmd[1])) { - // Handles old WebUIs that send a null after high-bit-set realtime chars - wsChannel->pushRT(cmd[1]); - } - } else if (cmd.length() == 2 && is_extended_realtime_command(cmd[0])) { + if (cmd.length() == 2 && is_extended_realtime_command(cmd[0])) { wsChannel->pushRT(cmd[0]); wsChannel->pushRT(cmd[1]); } else if (cmd.length() == 1 && is_realtime_command(cmd[0])) { diff --git a/FluidNC/src/WebUI/WSChannel.h b/FluidNC/src/WebUI/WSChannel.h index 01ee9ece7..5ba30c255 100644 --- a/FluidNC/src/WebUI/WSChannel.h +++ b/FluidNC/src/WebUI/WSChannel.h @@ -88,7 +88,7 @@ namespace WebUI { static void removeChannel(WSChannel* channel); static void removeChannel(uint8_t num); - static bool runGCode(int pageid, std::string cmd); + static bool runGCode(int pageid, std::string& cmd); static bool sendError(int pageid, std::string error); static void sendPing(); static void handleEvent(WebSocketsServer* server, uint8_t num, uint8_t type, uint8_t* payload, size_t length); diff --git a/FluidNC/src/WebUI/WebServer.cpp b/FluidNC/src/WebUI/WebServer.cpp index 2c0bf3e89..307d38349 100644 --- a/FluidNC/src/WebUI/WebServer.cpp +++ b/FluidNC/src/WebUI/WebServer.cpp @@ -58,7 +58,7 @@ namespace WebUI { static const char LOCATION_HEADER[] = "Location"; - Web_Server webServer __attribute__((init_priority(108))) ; + Web_Server webServer __attribute__((init_priority(108))); bool Web_Server::_setupdone = false; uint16_t Web_Server::_port = 0; @@ -438,8 +438,11 @@ namespace WebUI { } //if it is internal command [ESPXXX] // cmd.trim(); - int ESPpos = cmd.find("[ESP"); - if (ESPpos != std::string::npos) { + auto isCommand = cmd.length() && cmd[0] == '$'; + if (!isCommand) { + isCommand = cmd.find("[ESP") != std::string::npos; + } + if (isCommand) { char line[256]; strncpy(line, cmd.c_str(), 255); webClient.attachWS(_webserver, silent);