From 837398221b9f74de5af60269c8d9104f0b534288 Mon Sep 17 00:00:00 2001 From: Terje Io Date: Sat, 19 Apr 2025 14:08:46 +0200 Subject: [PATCH] Fixed regression introduced with PR#673. Added G30 as optional position for tool change. Moved new tool change mode from PR#673 to $346 - Tool change options. Moved Modbus RTU code from spindle plugin to the core. Fixed bug in delayed task handler, might occasionally hang the controller. --- CMakeLists.txt | 1 + README.md | 4 +- changelog.md | 21 ++ config.h | 10 +- core_handlers.h | 2 + grbl.h | 2 +- grbllib.c | 69 +++++- kinematics/delta.c | 4 +- messages.c | 7 +- messages.h | 3 +- modbus.c | 34 ++- modbus.h | 22 +- modbus_rtu.c | 590 +++++++++++++++++++++++++++++++++++++++++++++ modbus_rtu.h | 56 +++++ nvs_buffer.c | 4 +- planner.c | 2 +- protocol.c | 95 +------- protocol.h | 7 +- settings.c | 48 ++-- settings.h | 9 +- stream.c | 4 +- stream_passthru.c | 2 +- task.h | 3 +- tool_change.c | 286 +++++++++++++++------- 24 files changed, 1044 insertions(+), 241 deletions(-) create mode 100644 modbus_rtu.c create mode 100644 modbus_rtu.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 6377885..97abd86 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,6 +10,7 @@ target_sources(grbl INTERFACE ${CMAKE_CURRENT_LIST_DIR}/machine_limits.c ${CMAKE_CURRENT_LIST_DIR}/messages.c ${CMAKE_CURRENT_LIST_DIR}/modbus.c + ${CMAKE_CURRENT_LIST_DIR}/modbus_rtu.c ${CMAKE_CURRENT_LIST_DIR}/motion_control.c ${CMAKE_CURRENT_LIST_DIR}/my_plugin.c ${CMAKE_CURRENT_LIST_DIR}/nuts_bolts.c diff --git a/README.md b/README.md index 9449402..9190f33 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ ## grblHAL ## -Latest build date is 20250415, see the [changelog](changelog.md) for details. +Latest build date is 20250419, see the [changelog](changelog.md) for details. > [!NOTE] > A settings reset will be performed on an update of builds prior to 20241208. Backup and restore of settings is recommended. @@ -89,4 +89,4 @@ G/M-codes not supported by [legacy Grbl](https://github.com/gnea/grbl/wiki) are Some [plugins](https://github.com/grblHAL/plugins) implements additional M-codes. --- -20250413 +20250419 diff --git a/changelog.md b/changelog.md index 8d99838..f6875da 100644 --- a/changelog.md +++ b/changelog.md @@ -1,5 +1,26 @@ ## grblHAL changelog +20250419 + +Core: + +* Fixed regression introduced with [PR#673](https://github.com/grblHAL/core/pull/673), added G30 as optional position for tool change and moved new tool change mode from PR#673 to `$346` - _Tool change options_. + +* Moved Modbus RTU code from spindle plugin to the core. + +* For developers: deprecated `protocol_enqueue_foreground_task()`, replaced by `task_run_on_startup()` - added alias for the deprecated version. +Changed signature of `modbus_isup()` to return capabilities flag instead of boolean. + +* Fixed bug in delayed task handler, might occasionally hang the controller. May be part of keypad issue [#17](https://github.com/grblHAL/Plugin_keypad/issues/17). + +Plugins: + +Keypad and spindle: updated for core changes. + +Keypad, I2C display interface: fixed alignment issue that caused hardfault on WCO changes on some platforms. May resolve issue [#17](https://github.com/grblHAL/Plugin_keypad/issues/17). + +--- + 20250415 Core: diff --git a/config.h b/config.h index a07b1bc..c4833e4 100644 --- a/config.h +++ b/config.h @@ -1351,7 +1351,7 @@ and less range over the total 255 PWM levels to signal different spindle speeds. // Tool change settings (Group_Toolchange) /*! @name $341 - Setting_ToolChangeMode -0 = Normal mode, 1 = Manual change, 2 = Manual change @ G59.3, 3 = Manual change and probe sensor @ G59.3 - sets TLO +0 = Normal mode, 1 = Manual change, 2 = Manual change @ G59.3, 3 = Manual change and probe tolsetter @ G59.3, 4 = Ignore M6 */ ///@{ #if !defined DEFAULT_TOOLCHANGE_MODE || defined __DOXYGEN__ @@ -1392,12 +1392,18 @@ and less range over the total 255 PWM levels to signal different spindle speeds. #endif ///@} -/*! @name $346 - Setting_ToolChangeRestorePosition +/*! @name $346 - Setting_ToolChangeOptions */ ///@{ #if !defined DEFAULT_TOOLCHANGE_NO_RESTORE_POSITION || defined __DOXYGEN__ #define DEFAULT_TOOLCHANGE_NO_RESTORE_POSITION Off #endif +#if !defined DEFAULT_TOOLCHANGE_AT_G30 || defined __DOXYGEN__ +#define DEFAULT_TOOLCHANGE_AT_G30 Off +#endif +#if !defined DEFAULT_TOOLCHANGE_FAST_PROBE_PULLOFF || defined __DOXYGEN__ +#define DEFAULT_TOOLCHANGE_FAST_PROBE_PULLOFF Off +#endif ///@} // Homing settings (Group_Homing) diff --git a/core_handlers.h b/core_handlers.h index 8c3ce7c..ea32824 100644 --- a/core_handlers.h +++ b/core_handlers.h @@ -89,6 +89,7 @@ typedef void (*on_state_change_ptr)(sys_state_t state); typedef void (*on_override_changed_ptr)(override_changed_t override); typedef void (*on_spindle_programmed_ptr)(spindle_ptrs_t *spindle, spindle_state_t state, float rpm, spindle_rpm_mode_t mode); typedef void (*on_wco_changed_ptr)(void); +typedef void (*on_wco_saved_ptr)(coord_system_id_t id, coord_data_t *data); typedef void (*on_program_completed_ptr)(program_flow_t program_flow, bool check_mode); typedef void (*on_execute_realtime_ptr)(sys_state_t state); typedef void (*on_unknown_accessory_override_ptr)(uint8_t cmd); @@ -220,6 +221,7 @@ typedef struct { on_report_handlers_init_ptr on_report_handlers_init; on_spindle_programmed_ptr on_spindle_programmed; on_wco_changed_ptr on_wco_changed; + on_wco_saved_ptr on_wco_saved; on_program_completed_ptr on_program_completed; on_execute_realtime_ptr on_execute_realtime; on_execute_realtime_ptr on_execute_delay; diff --git a/grbl.h b/grbl.h index 407edc9..1f58bf3 100644 --- a/grbl.h +++ b/grbl.h @@ -42,7 +42,7 @@ #else #define GRBL_VERSION "1.1f" #endif -#define GRBL_BUILD 20250415 +#define GRBL_BUILD 20250419 #define GRBL_URL "https://github.com/grblHAL" diff --git a/grbllib.c b/grbllib.c index e9532f3..a59ab9a 100644 --- a/grbllib.c +++ b/grbllib.c @@ -76,7 +76,7 @@ typedef union { } driver_startup_t; #ifndef CORE_TASK_POOL_SIZE -#define CORE_TASK_POOL_SIZE 30 +#define CORE_TASK_POOL_SIZE 40 #endif typedef struct core_task { @@ -92,7 +92,7 @@ DCRAM grbl_hal_t hal; DCRAM static core_task_t task_pool[CORE_TASK_POOL_SIZE]; static driver_startup_t driver = { .ok = 0xFF }; -static core_task_t *next_task = NULL, *immediate_task = NULL, *systick_task = NULL, *last_freed = NULL; +static core_task_t *next_task = NULL, *immediate_task = NULL, *on_booted = NULL, *systick_task = NULL, *last_freed = NULL; static on_linestate_changed_ptr on_linestate_changed; static settings_changed_ptr hal_settings_changed; @@ -181,8 +181,10 @@ static void output_welcome_message (void *data) static void onLinestateChanged (serial_linestate_t state) { - if(state.dtr) + if(state.dtr) { + task_delete(output_welcome_message, NULL); task_add_delayed(output_welcome_message, NULL, 200); + } if(on_linestate_changed) on_linestate_changed(state); @@ -343,7 +345,7 @@ int grbl_enter (void) if(driver.ok != 0xFF) { sys.alarm = Alarm_SelftestFailed; - protocol_enqueue_foreground_task(report_driver_error, NULL); + task_run_on_startup(report_driver_error, NULL); } hal.stepper.enable(settings.steppers.energize, true); @@ -465,6 +467,7 @@ __attribute__((always_inline)) static inline core_task_t *task_alloc (void) __attribute__((always_inline)) static inline void task_free (core_task_t *task) { task->fn = NULL; + task->next = NULL; if(last_freed == NULL) last_freed = task; } @@ -541,7 +544,7 @@ ISR_CODE bool ISR_FUNC(task_add_delayed)(foreground_task_ptr fn, void *data, uin if(next_task == NULL) next_task = task; - else if((int32_t)(task->time - next_task->time) < 0) { + else if((int32_t)(task->time - next_task->time) <= 0) { task->next = next_task; next_task = task; } else { @@ -662,3 +665,59 @@ ISR_CODE bool ISR_FUNC(task_add_immediate)(foreground_task_ptr fn, void *data) return task != NULL; } + +/*! \brief Enqueue a function to be called once by the foreground process after the boot sequence is completed. +\param fn pointer to a \a foreground_task_ptr type of function. +\param data pointer to data to be passed to the callee. +\returns true if successful, false otherwise. +*/ +ISR_CODE bool ISR_FUNC(task_run_on_startup)(foreground_task_ptr fn, void *data) +{ + if(sys.cold_start) { + + core_task_t *task = NULL; + + hal.irq_disable(); + + if(fn && (task = task_alloc())) { + + task->fn = fn; + task->data = data; + task->next = NULL; + + if(on_booted == NULL) + on_booted = task; + else { + core_task_t *t = on_booted; + while(t->next) + t = t->next; + t->next = task; + } + } + + hal.irq_enable(); + + return task != NULL; + + } else + return task_add_immediate(fn, data); // TODO: for now, to be removed... +} + +// for core use only, called once from protocol.c on cold start +void task_execute_on_startup (void) +{ + if(on_booted) do { + + core_task_t *task = on_booted; + foreground_task_ptr fn = task->fn; + void *data = task->data; + + on_booted = task->next; + task_free(task); + fn(data); + + } while(on_booted); + + if(!sys.driver_started) + while(true); +} diff --git a/kinematics/delta.c b/kinematics/delta.c index 4df1f72..5e76020 100644 --- a/kinematics/delta.c +++ b/kinematics/delta.c @@ -3,7 +3,7 @@ Part of grblHAL - Copyright (c) 2023-2024 Terje Io + Copyright (c) 2023-2025 Terje Io Transforms derived from mzavatsky at Trossen Robotics https://hypertriangle.com/~alex/delta-robot-tutorial/ get_cuboid_envelope() derived from javascript code in @@ -541,7 +541,7 @@ static void delta_homing_complete (axes_signals_t cycle, bool success) : machine.home_z - settings.homing.pulloff; if(machine.cfg.flags.home_to_cuboid_top) - protocol_enqueue_foreground_task(delta_go_home, NULL); + task_add_immediate(delta_go_home, NULL); } if(on_homing_completed) diff --git a/messages.c b/messages.c index 58ae312..4bb3e11 100644 --- a/messages.c +++ b/messages.c @@ -44,9 +44,10 @@ PROGMEM static const message_t messages[] = { { .id = Message_CycleStart2Continue, .text = "Press cycle start to continue." }, { .id = Message_TPCycleStart2Continue, .text = "Remove any touch plate and press cycle start to continue." }, { .id = Message_ProbeFailedRetry, .text = "Probe failed, try again." }, - { .id = Message_ExecuteTPW, .text = "Perform a probe with $TPW first!", .type = Message_Warning}, - { .id = Message_ProbeProtected, .text = "Probe protection activated."}, - { .id = Message_Stop, .text = "Stop"} + { .id = Message_ExecuteTPW, .text = "Perform a probe with $TPW first!", .type = Message_Warning }, + { .id = Message_ProbeProtected, .text = "Probe protection activated." }, + { .id = Message_Stop, .text = "Stop" }, + { .id = Message_CycleStart2TouchOff, .text = "Press cycle start to position for touch-off." } }; const message_t *message_get (message_code_t id) diff --git a/messages.h b/messages.h index 353579d..2c59bf4 100644 --- a/messages.h +++ b/messages.h @@ -47,7 +47,8 @@ typedef enum { Message_ExecuteTPW = 20, //!< 20 Message_ProbeProtected = 21, //!< 21 Message_Stop = 22, //!< 22 - Message_NextMessage //!< 23 - next unassigned message number. + Message_CycleStart2TouchOff = 23, //!< 23 + Message_NextMessage //!< 24 - next unassigned message number. } message_code_t; typedef enum { diff --git a/modbus.c b/modbus.c index 8fc0131..3474470 100644 --- a/modbus.c +++ b/modbus.c @@ -4,20 +4,20 @@ Part of grblHAL - Copyright (c) 2023 Terje Io + Copyright (c) 2023-2025 Terje Io - Grbl is free software: you can redistribute it and/or modify + grblHAL is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - Grbl is distributed in the hope that it will be useful, + grblHAL is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License - along with Grbl. If not, see . + along with grblHAL. If not, see . */ @@ -25,21 +25,37 @@ #include +#include "nuts_bolts.h" + #define N_MODBUS_API 2 static uint_fast16_t n_api = 0, tcp_api = N_MODBUS_API, rtu_api = N_MODBUS_API; static modbus_api_t modbus[N_MODBUS_API] = {0}; -bool modbus_isup (void) +modbus_cap_t modbus_isup (void) { - bool ok = n_api > 0; uint_fast16_t idx = n_api; + modbus_cap_t cap = {}; if(idx) do { - ok &= modbus[--idx].is_up(); + idx--; + if(modbus[idx].is_up()) switch(modbus[idx].interface) { + + case Modbus_InterfaceRTU: + cap.rtu = On; + break; + + case Modbus_InterfaceASCII: + cap.ascii = On; + break; + + case Modbus_InterfaceTCP: + cap.tcp = On; + break; + } } while(idx); - return ok; + return cap; } bool modbus_enabled (void) diff --git a/modbus.h b/modbus.h index 8fbf7e1..f58abe3 100644 --- a/modbus.h +++ b/modbus.h @@ -4,20 +4,20 @@ Part of grblHAL - Copyright (c) 2023 Terje Io + Copyright (c) 2023-2025 Terje Io - Grbl is free software: you can redistribute it and/or modify + grblHAL is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - Grbl is distributed in the hope that it will be useful, + grblHAL is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License - along with Grbl. If not, see . + along with grblHAL. If not, see . */ @@ -83,6 +83,16 @@ typedef union { }; } modbus_silence_timeout_t; +typedef union { + uint8_t ok; + struct { + uint8_t rtu :1, + ascii :1, + tcp :1, + unassigned :6; + }; +} modbus_cap_t; + typedef bool (*modbus_is_up_ptr)(void); typedef void (*modbus_flush_queue_ptr)(void); typedef void (*modbus_set_silence_ptr)(const modbus_silence_timeout_t *timeout); @@ -96,7 +106,7 @@ typedef struct { modbus_send_ptr send; } modbus_api_t; -bool modbus_isup (void); +modbus_cap_t modbus_isup (void); bool modbus_enabled (void); void modbus_flush_queue (void); void modbus_set_silence (const modbus_silence_timeout_t *timeout); diff --git a/modbus_rtu.c b/modbus_rtu.c new file mode 100644 index 0000000..f3f71e1 --- /dev/null +++ b/modbus_rtu.c @@ -0,0 +1,590 @@ +/* + + modbus_rtu.c - a lightweight ModBus RTU implementation + + Part of grblHAL + + Copyright (c) 2020-2025 Terje Io + + grblHAL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + grblHAL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with grblHAL. If not, see . + +*/ + +#include "driver.h" + +#if MODBUS_ENABLE & MODBUS_RTU_ENABLED + +#include + +#include "protocol.h" +#include "settings.h" +#include "crc.h" +#include "nvs_buffer.h" +#include "state_machine.h" +#include "modbus.h" + +#ifndef MODBUS_BAUDRATE +#define MODBUS_BAUDRATE 3 // 19200 +#endif +#ifndef MODBUS_RTU_STREAM +#ifdef MODBUS_SERIAL_PORT +#define MODBUS_RTU_STREAM MODBUS_SERIAL_PORT // Use deprecated definition +#else +#define MODBUS_RTU_STREAM -1 +#endif +#endif +#ifndef MODBUS_DIR_AUX +#define MODBUS_DIR_AUX -1 +#endif + +typedef enum { + ModBus_Idle, + ModBus_Silent, + ModBus_TX, + ModBus_AwaitReply, + ModBus_Timeout, + ModBus_GotReply, + ModBus_Exception, + ModBus_Retry +} modbus_state_t; + +typedef void (*stream_set_direction_ptr)(bool tx); + +typedef struct { + set_baud_rate_ptr set_baud_rate; + stream_set_direction_ptr set_direction; // NULL if auto direction + get_stream_buffer_count_ptr get_tx_buffer_count; + get_stream_buffer_count_ptr get_rx_buffer_count; + stream_write_n_ptr write; + stream_read_ptr read; + flush_stream_buffer_ptr flush_tx_buffer; + flush_stream_buffer_ptr flush_rx_buffer; +} modbus_stream_t; + +typedef struct queue_entry { + bool async; + modbus_message_t msg; + modbus_callbacks_t callbacks; + struct queue_entry *next; +} queue_entry_t; + +static const uint32_t baud[] = { 2400, 4800, 9600, 19200, 38400, 115200 }; +static const modbus_silence_timeout_t dflt_timeout = +{ + .b2400 = 16, + .b4800 = 8, + .b9600 = 4, + .b19200 = 2, + .b38400 = 2, + .b115200 = 2 +}; + +static modbus_stream_t stream; +static uint32_t rx_timeout = 0, silence_until = 0, silence_timeout; +static int16_t exception_code = 0; +static modbus_silence_timeout_t silence; +static queue_entry_t queue[MODBUS_QUEUE_LENGTH]; +static modbus_settings_t modbus; +static volatile bool spin_lock = false, is_up = false; +static volatile queue_entry_t *tail, *head, *packet = NULL; +static volatile modbus_state_t state = ModBus_Idle; +#if MODBUS_ENABLE & MODBUS_RTU_DIR_ENABLED +static uint8_t dir_port; +#endif + +static driver_reset_ptr driver_reset; +static on_report_options_ptr on_report_options; +static nvs_address_t nvs_address; + +/* +static bool valid_crc (const char *buf, uint_fast16_t len) +{ + uint16_t crc = modbus_crc16x(buf, len - 2); + + return buf[len - 1] == (crc >> 8) && buf[len - 2] == (crc & 0xFF); +} +*/ + +static void retry_exception (uint8_t code, void *context) +{ + if(packet && packet->callbacks.retries) { + state = ModBus_Retry; + silence_until = hal.get_elapsed_ticks() + silence_timeout + packet->callbacks.retry_delay; + } +} + +static inline queue_entry_t *add_message (queue_entry_t *packet, modbus_message_t *msg, bool async, const modbus_callbacks_t *callbacks) +{ + memcpy(&packet->msg, msg, sizeof(modbus_message_t)); + + packet->async = async; + + if(callbacks) { + memcpy(&packet->callbacks, callbacks, sizeof(modbus_callbacks_t)); + if(!packet->async && packet->callbacks.retries) + packet->callbacks.on_rx_exception = retry_exception; + } else { + packet->callbacks.retries = 0; + packet->callbacks.on_rx_packet = NULL; + packet->callbacks.on_rx_exception = NULL; + } + + return packet; +} + +static void tx_message (volatile queue_entry_t *msg) +{ + if(stream.set_direction) + stream.set_direction(true); + + packet = msg; + state = ModBus_TX; + rx_timeout = modbus.rx_timeout; + + stream.flush_rx_buffer(); + stream.write((char *)((queue_entry_t *)msg)->msg.adu, ((queue_entry_t *)msg)->msg.tx_length); +} + +// called once every ms +static void modbus_poll (void *data) +{ + if(spin_lock) + return; + + spin_lock = true; + + switch(state) { + + case ModBus_Idle: + if(tail != head && !packet) { + tx_message(tail); + tail = tail->next; + } + break; + + case ModBus_Silent: + if((int32_t)(silence_until - hal.get_elapsed_ticks()) <= 0) { + silence_until = 0; + state = ModBus_Idle; + } + break; + + case ModBus_TX: + if(!stream.get_tx_buffer_count()) { + + // When an auto-direction sense circuit supports higher baudrates is used at slower rates, it can switch during the off time (TXD is high) of some bit sequences. + // In some cases (teensy4.1) this can result in garbage characters in the RX buffer after a message is transmitted. + // Flushing the buffer prevents these characters from appearing as an RX message. + // Since Modbus is half-duplex, there should never be valid data recived during a message transmit. + stream.flush_rx_buffer(); + + state = ModBus_AwaitReply; + + if(stream.set_direction) + stream.set_direction(false); + } + break; + + case ModBus_AwaitReply: + if(rx_timeout && --rx_timeout == 0) { + if(packet->async) { + state = ModBus_Silent; + if(packet->callbacks.on_rx_exception) + packet->callbacks.on_rx_exception(0, packet->msg.context); + packet = NULL; + } else if(stream.read() == packet->msg.adu[0] && (stream.read() & 0x80)) { + exception_code = stream.read(); + state = ModBus_Exception; + } else + state = ModBus_Timeout; + spin_lock = false; + if(state != ModBus_AwaitReply) + silence_until = hal.get_elapsed_ticks() + silence_timeout; + return; + } + + if(stream.get_rx_buffer_count() >= packet->msg.rx_length) { + + char *buf = (char *)((queue_entry_t *)packet)->msg.adu; + uint16_t rx_len = packet->msg.rx_length; // store original length for CRC check + + do { + *buf++ = stream.read(); + } while(--packet->msg.rx_length); + + if(packet->msg.crc_check) { + uint_fast16_t crc = modbus_crc16x(((queue_entry_t *)packet)->msg.adu, rx_len - 2); + + if(packet->msg.adu[rx_len - 2] != (crc & 0xFF) || packet->msg.adu[rx_len - 1] != (crc >> 8)) { + // CRC check error + if((state = packet->async ? ModBus_Silent : ModBus_Exception) == ModBus_Silent) { + if(packet->callbacks.on_rx_exception) + packet->callbacks.on_rx_exception(0, packet->msg.context); + packet = NULL; + } + silence_until = hal.get_elapsed_ticks() + silence_timeout; + break; + } + } + + if((state = packet->async ? ModBus_Silent : ModBus_GotReply) == ModBus_Silent) { + if(packet->callbacks.on_rx_packet) { + packet->msg.rx_length = rx_len; + packet->callbacks.on_rx_packet(&((queue_entry_t *)packet)->msg); + } + packet = NULL; + } + + silence_until = hal.get_elapsed_ticks() + silence_timeout; + } + break; + + case ModBus_Timeout: + if(packet->async) + state = ModBus_Silent; + silence_until = hal.get_elapsed_ticks() + silence_timeout; + break; + + default: + break; + } + + spin_lock = false; +} + +static bool modbus_send_rtu (modbus_message_t *msg, const modbus_callbacks_t *callbacks, bool block) +{ + static bool poll = false; + static queue_entry_t sync_msg = {0}; + + if(msg->tx_length > MODBUS_MAX_ADU_SIZE || msg->rx_length > MODBUS_MAX_ADU_SIZE) { + if(callbacks->on_rx_exception) + callbacks->on_rx_exception(0, msg->context); + return false; + } + + uint_fast16_t crc = modbus_crc16x(msg->adu, msg->tx_length - 2); + + msg->adu[msg->tx_length - 1] = crc >> 8; + msg->adu[msg->tx_length - 2] = crc & 0xFF; + + while(spin_lock); + + if(block) { + + if(poll) + return false; + + poll = true; + + do { + grbl.on_execute_realtime(state_get()); + } while(state != ModBus_Idle); + + tx_message(add_message(&sync_msg, msg, false, callbacks)); + + while(poll) { + + grbl.on_execute_realtime(state_get()); + + switch(state) { + + case ModBus_Timeout: + if(packet->callbacks.on_rx_exception) + packet->callbacks.on_rx_exception(0, packet->msg.context); + poll = packet->callbacks.retries > 0; + break; + + case ModBus_Exception: + if(packet->callbacks.on_rx_exception) + packet->callbacks.on_rx_exception(exception_code == -1 ? 0 : (uint8_t)(exception_code & 0xFF), packet->msg.context); + poll = packet->callbacks.retries > 0; + break; + + case ModBus_GotReply: + if(packet->callbacks.on_rx_packet) + packet->callbacks.on_rx_packet(&((queue_entry_t *)packet)->msg); + poll = block = false; + break; + + case ModBus_Retry: + if((int32_t)(silence_until - hal.get_elapsed_ticks()) <= 0) { + silence_until = 0; + if(--packet->callbacks.retries == 0) + packet->callbacks.on_rx_exception = callbacks->on_rx_exception; + packet = add_message(&sync_msg, msg, false, (const modbus_callbacks_t *)&packet->callbacks); + tx_message(packet); + } + break; + + default: + break; + } + } + + poll = false; + packet = NULL; + state = silence_until > 0 ? ModBus_Silent : ModBus_Idle; + + } else if(packet != &sync_msg) { + if(head->next != tail) { + add_message((queue_entry_t *)head, msg, true, callbacks); + head = head->next; + } + } + + return !block; +} + +static void modbus_reset (void) +{ + while(spin_lock); + + if(sys.abort) { + + if(packet) { + packet = NULL; + packet->callbacks.retries = 0; + packet->callbacks.on_rx_exception = NULL; + } + + tail = head; + silence_until = hal.get_elapsed_ticks() + 500; + state = ModBus_Silent; + + stream.flush_tx_buffer(); + stream.flush_rx_buffer(); + } + + if(state == ModBus_Retry) { + silence_until = hal.get_elapsed_ticks() + 500; + state = ModBus_Silent; + } + + driver_reset(); +} + +static uint32_t get_baudrate (uint32_t rate) +{ + uint32_t idx = sizeof(baud) / sizeof(uint32_t); + + do { + if(baud[--idx] == rate) + return idx; + } while(idx); + + return MODBUS_BAUDRATE; +} + +static const setting_group_detail_t modbus_groups [] = { + { Group_Root, Group_ModBus, "ModBus"} +}; + +static status_code_t modbus_set_baud (setting_id_t id, uint_fast16_t value) +{ + modbus.baud_rate = baud[(uint32_t)value]; + silence_timeout = silence.timeout[(uint32_t)value]; + stream.set_baud_rate(modbus.baud_rate); + + return Status_OK; +} + +static uint32_t modbus_get_baud (setting_id_t setting) +{ + return get_baudrate(modbus.baud_rate); +} + +static const setting_detail_t modbus_settings[] = { + { Settings_ModBus_BaudRate, Group_ModBus, "ModBus baud rate", NULL, Format_RadioButtons, "2400,4800,9600,19200,38400,115200", NULL, NULL, Setting_NonCoreFn, modbus_set_baud, modbus_get_baud, NULL }, + { Settings_ModBus_RXTimeout, Group_ModBus, "ModBus RX timeout", "milliseconds", Format_Integer, "####0", "50", "250", Setting_NonCore, &modbus.rx_timeout, NULL, NULL } +}; + +static void modbus_settings_save (void) +{ + hal.nvs.memcpy_to_nvs(nvs_address, (uint8_t *)&modbus, sizeof(modbus_settings_t), true); +} + +static void modbus_settings_restore (void) +{ + modbus.rx_timeout = 50; + modbus.baud_rate = baud[MODBUS_BAUDRATE]; + + hal.nvs.memcpy_to_nvs(nvs_address, (uint8_t *)&modbus, sizeof(modbus_settings_t), true); +} + +static void modbus_settings_load (void) +{ + if(hal.nvs.memcpy_from_nvs((uint8_t *)&modbus, nvs_address, sizeof(modbus_settings_t), true) != NVS_TransferResult_OK || + modbus.baud_rate != baud[get_baudrate(modbus.baud_rate)]) + modbus_settings_restore(); + + is_up = true; + silence_timeout = silence.timeout[get_baudrate(modbus.baud_rate)]; + + stream.set_baud_rate(modbus.baud_rate); +} + +static void onReportOptions (bool newopt) +{ + on_report_options(newopt); + + if(!newopt) + report_plugin("MODBUS", "0.19"); +} + +static bool modbus_rtu_isup (void) +{ + return is_up; +} + +static void modbus_rtu_flush_queue (void) +{ + while(spin_lock); + + tail = head; +} + +static void modbus_rtu_set_silence (const modbus_silence_timeout_t *timeout) +{ + if(timeout) + memcpy(&silence, timeout, sizeof(modbus_silence_timeout_t)); + else + memcpy(&silence, &dflt_timeout, sizeof(modbus_silence_timeout_t)); + + silence_timeout = silence.timeout[get_baudrate(modbus.baud_rate)]; +} + +static bool stream_is_valid (const io_stream_t *stream) +{ + return stream && + !(stream->set_baud_rate == NULL || + stream->get_tx_buffer_count == NULL || + stream->get_rx_buffer_count == NULL || + stream->write_n == NULL || + stream->read == NULL || + stream->reset_write_buffer == NULL || + stream->reset_read_buffer == NULL || + stream->set_enqueue_rt_handler == NULL); +} + +#if MODBUS_ENABLE & MODBUS_RTU_DIR_ENABLED +static void modbus_set_direction (bool tx) +{ + ioport_digital_out(dir_port, tx); +} +#endif + +static bool claim_stream (io_stream_properties_t const *sstream) +{ + io_stream_t const *claimed = NULL; + +#if MODBUS_RTU_STREAM >= 0 + if(sstream->type == StreamType_Serial && sstream->instance == MODBUS_RTU_STREAM) { +#else + if(sstream->type == StreamType_Serial && sstream->flags.modbus_ready && !sstream->flags.claimed) { +#endif + if((claimed = sstream->claim(baud[MODBUS_BAUDRATE])) && stream_is_valid(claimed)) { + + claimed->set_enqueue_rt_handler(stream_buffer_all); + + stream.set_baud_rate = claimed->set_baud_rate; + stream.get_tx_buffer_count = claimed->get_tx_buffer_count; + stream.get_rx_buffer_count = claimed->get_rx_buffer_count; + stream.write = claimed->write_n; + stream.read = claimed->read; + stream.flush_tx_buffer = claimed->reset_write_buffer; + stream.flush_rx_buffer = claimed->reset_read_buffer; +#if MODBUS_ENABLE & MODBUS_RTU_DIR_ENABLED + stream.set_direction = modbus_set_direction; +#endif + if(hal.periph_port.set_pin_description) { + hal.periph_port.set_pin_description(Output_TX, (pin_group_t)(PinGroup_UART + claimed->instance), "Modbus"); + hal.periph_port.set_pin_description(Input_RX, (pin_group_t)(PinGroup_UART + claimed->instance), "Modbus"); + } + } else + claimed = NULL; + } + + return claimed != NULL; +} + +void modbus_rtu_init (void) +{ + static const modbus_api_t api = { + .interface = Modbus_InterfaceRTU, + .is_up = modbus_rtu_isup, + .flush_queue = modbus_rtu_flush_queue, + .set_silence = modbus_rtu_set_silence, + .send = modbus_send_rtu + }; + + static setting_details_t setting_details = { + .groups = modbus_groups, + .n_groups = sizeof(modbus_groups) / sizeof(setting_group_detail_t), + .settings = modbus_settings, + .n_settings = sizeof(modbus_settings) / sizeof(setting_detail_t), + .save = modbus_settings_save, + .load = modbus_settings_load, + .restore = modbus_settings_restore + }; + +#if MODBUS_ENABLE & MODBUS_RTU_DIR_ENABLED + + uint8_t n_out = ioports_available(Port_Digital, Port_Output); + + #if MODBUS_DIR_AUX >= 0 + dir_port = MODBUS_DIR_AUX; + #else + dir_port = n_out - 1; + #endif + + if(!(n_out > dir_port && ioport_claim(Port_Digital, Port_Output, &dir_port, "Modbus RX/TX direction"))) { + task_run_on_startup(report_warning, "Modbus failed to initialize!"); + system_raise_alarm(Alarm_SelftestFailed); + return; + } + +#endif + + if(stream_enumerate_streams(claim_stream) && (nvs_address = nvs_alloc(sizeof(modbus_settings_t)))) { + + driver_reset = hal.driver_reset; + hal.driver_reset = modbus_reset; + + task_add_systick(modbus_poll, NULL); + + on_report_options = grbl.on_report_options; + grbl.on_report_options = onReportOptions; + + //TODO: subscribe to grbl.on_reset event to terminate polling? + + settings_register(&setting_details); + + head = tail = &queue[0]; + + uint_fast8_t idx; + for(idx = 0; idx < MODBUS_QUEUE_LENGTH; idx++) + queue[idx].next = idx == MODBUS_QUEUE_LENGTH - 1 ? &queue[0] : &queue[idx + 1]; + + modbus_register_api(&api); + + modbus_set_silence(NULL); + + } else { + task_run_on_startup(report_warning, "Modbus failed to initialize!"); + system_raise_alarm(Alarm_SelftestFailed); + } +} + +#endif // MODBUS_ENABLE & MODBUS_RTU_ENABLED diff --git a/modbus_rtu.h b/modbus_rtu.h new file mode 100644 index 0000000..d955568 --- /dev/null +++ b/modbus_rtu.h @@ -0,0 +1,56 @@ +/* + + modbus_rtu.h - a lightweight ModBus RTU implementation + + Part of grblHAL + + Copyright (c) 2020-2025 Terje Io + + grblHAL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + grblHAL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with grblHAL. If not, see . + +*/ + +#ifndef _MODBUS_RTU_H_ +#define _MODBUS_RTU_H_ + +#include "grbl/modbus.h" + +typedef enum { + ModBus_Idle, + ModBus_Silent, + ModBus_TX, + ModBus_AwaitReply, + ModBus_Timeout, + ModBus_GotReply, + ModBus_Exception, + ModBus_Retry +} modbus_state_t; + +typedef void (*stream_set_direction_ptr)(bool tx); + +typedef struct { + set_baud_rate_ptr set_baud_rate; + stream_set_direction_ptr set_direction; // NULL if auto direction + get_stream_buffer_count_ptr get_tx_buffer_count; + get_stream_buffer_count_ptr get_rx_buffer_count; + stream_write_n_ptr write; + stream_read_ptr read; + flush_stream_buffer_ptr flush_tx_buffer; + flush_stream_buffer_ptr flush_rx_buffer; +} modbus_stream_t; + +void modbus_rtu_init (void); +bool modbus_rtu_send (modbus_message_t *msg, const modbus_callbacks_t *callbacks, bool block); + +#endif diff --git a/nvs_buffer.c b/nvs_buffer.c index c099295..74b464a 100644 --- a/nvs_buffer.c +++ b/nvs_buffer.c @@ -3,7 +3,7 @@ Part of grblHAL - Copyright (c) 2017-2024 Terje Io + Copyright (c) 2017-2025 Terje Io Copyright (c) 2012-2016 Sungeun K. Jeon for Gnea Research LLC Copyright (c) 2009-2011 Simen Svale Skogsrud @@ -286,7 +286,7 @@ bool nvs_buffer_init (void) grbl.report.status_message(Status_SettingReadFail); } } else - protocol_enqueue_foreground_task(report_warning, "Not enough heap for NVS buffer!"); + task_run_on_startup(report_warning, "Not enough heap for NVS buffer!"); // Clear settings dirty flags memset(&settings_dirty, 0, sizeof(settings_dirty_t)); diff --git a/planner.c b/planner.c index d229390..4da2ee9 100644 --- a/planner.c +++ b/planner.c @@ -232,7 +232,7 @@ bool plan_reset (void) } if(block_buffer_size != settings.planner_buffer_blocks) - protocol_enqueue_foreground_task(report_plain, "Planner buffer size was reduced!"); + task_run_on_startup(report_plain, "Planner buffer size was reduced!"); } if(block_buffer == NULL) diff --git a/protocol.c b/protocol.c index 0e406ca..7da6fdc 100644 --- a/protocol.c +++ b/protocol.c @@ -49,25 +49,13 @@ typedef union { }; } line_flags_t; -typedef struct { - void *data; - fg_task_ptr task; -} delayed_task_t; - -typedef struct { - volatile uint_fast8_t head; - volatile uint_fast8_t tail; - delayed_task_t task[RT_QUEUE_SIZE]; -} realtime_queue_t; +extern void task_execute_on_startup (void); static uint_fast16_t char_counter = 0; static char line[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated. static char xcommand[LINE_BUFFER_SIZE]; static bool keep_rt_commands = false; -static realtime_queue_t realtime_queue = {0}; -static on_execute_realtime_ptr on_execute_delay; -static void protocol_execute_rt_commands (sys_state_t state); static void protocol_exec_rt_suspend (sys_state_t state); // add gcode to execute not originating from normal input stream @@ -86,16 +74,6 @@ bool protocol_enqueue_gcode (char *gcode) return ok; } -static void protocol_on_execute_delay (sys_state_t state) -{ - if(sys.rt_exec_state & EXEC_RT_COMMAND) { - system_clear_exec_state_flag(EXEC_RT_COMMAND); - protocol_execute_rt_commands(0); - } - - on_execute_delay(state); -} - static bool recheck_line (char *line, line_flags_t *flags) { bool keep_rt_commands = false, first_char = true; @@ -196,20 +174,16 @@ bool protocol_main_loop (void) } #endif // All systems go! - protocol_enqueue_foreground_task(system_execute_startup, NULL); // Schedule startup script for execution. + task_add_immediate(system_execute_startup, NULL); // Schedule startup script for execution. } // Ensure spindle and coolant is switched off on a cold start if(sys.cold_start) { spindle_all_off(); hal.coolant.set_state((coolant_state_t){0}); - if(realtime_queue.head != realtime_queue.tail) - system_set_exec_state_flag(EXEC_RT_COMMAND); // execute any boot up commands - on_execute_delay = grbl.on_execute_delay; - grbl.on_execute_delay = protocol_on_execute_delay; sys.cold_start = false; - } else // TODO: if flushing entries from the queue that has allocated data associated then these will be orphaned/leaked. - memset(&realtime_queue, 0, sizeof(realtime_queue_t)); + system_set_exec_state_flag(EXEC_RT_COMMAND); // execute any statup up tasks + } // --------------------------------------------------------------------------------- // Primary loop! Upon a system abort, this exits back to main() to reset the system. @@ -642,7 +616,7 @@ bool protocol_exec_rt_system (void) report_pid_log(); if(rt_exec & EXEC_RT_COMMAND) - protocol_execute_rt_commands(0); + task_execute_on_startup(); rt_exec &= ~(EXEC_STOP|EXEC_STATUS_REPORT|EXEC_GCODE_REPORT|EXEC_PID_REPORT|EXEC_TLO_REPORT|EXEC_RT_COMMAND); // clear requests already processed @@ -969,7 +943,7 @@ ISR_CODE bool ISR_FUNC(protocol_enqueue_realtime_command)(char c) case CMD_MPG_MODE_TOGGLE: // Switch off MPG mode if((drop = hal.stream.type == StreamType_MPG)) - protocol_enqueue_foreground_task(stream_mpg_set_mode, NULL); + task_add_immediate(stream_mpg_set_mode, NULL); break; case CMD_AUTO_REPORTING_TOGGLE: @@ -1058,63 +1032,6 @@ ISR_CODE bool ISR_FUNC(protocol_enqueue_realtime_command)(char c) return drop; } -static const uint32_t dummy_data = 0; - - -/*! \brief Enqueue a function to be called once by the foreground process. -\param fn pointer to a \a foreground_task_ptr type of function. -\param data pointer to data to be passed to the callee. -\returns true if successful, false otherwise. -*/ -ISR_CODE bool ISR_FUNC(protocol_enqueue_foreground_task)(fg_task_ptr fn, void *data) -{ - bool ok; - uint_fast8_t bptr = (realtime_queue.head + 1) & (RT_QUEUE_SIZE - 1); // Get next head pointer - - if((ok = bptr != realtime_queue.tail)) { // If not buffer full - realtime_queue.task[realtime_queue.head].data = data; - realtime_queue.task[realtime_queue.head].task = fn; // add function pointer to buffer, - realtime_queue.head = bptr; // update pointer and - if(sys.driver_started) - system_set_exec_state_flag(EXEC_RT_COMMAND); // flag it for execute - } - - return ok; -} - -/*! \brief Enqueue a function to be called once by the foreground process. -\param fn pointer to a \a on_execute_realtime_ptr type of function. -\returns true if successful, false otherwise. -__NOTE:__ Deprecated, use protocol_enqueue_foreground_task instead. -*/ -ISR_CODE bool ISR_FUNC(protocol_enqueue_rt_command)(on_execute_realtime_ptr fn) -{ - return protocol_enqueue_foreground_task(fn, (void *)&dummy_data); -} - -// Execute enqueued functions. -static void protocol_execute_rt_commands (sys_state_t state) -{ - while(realtime_queue.tail != realtime_queue.head) { - uint_fast8_t bptr = realtime_queue.tail; - if(realtime_queue.task[bptr].task.fn) { - if(realtime_queue.task[bptr].data == (void *)&dummy_data) { - on_execute_realtime_ptr call = realtime_queue.task[bptr].task.fn_deprecated; - realtime_queue.task[bptr].task.fn_deprecated = NULL; - call(state_get()); - } else { - foreground_task_ptr call = realtime_queue.task[bptr].task.fn; - realtime_queue.task[bptr].task.fn = NULL; - call(realtime_queue.task[bptr].data); - } - } - realtime_queue.tail = (bptr + 1) & (RT_QUEUE_SIZE - 1); - } - - if(!sys.driver_started) - while(true); -} - void protocol_execute_noop (sys_state_t state) { (void)state; diff --git a/protocol.h b/protocol.h index 7352069..f6413c2 100644 --- a/protocol.h +++ b/protocol.h @@ -3,7 +3,7 @@ Part of grblHAL - Copyright (c) 2016-2024 Terje Io + Copyright (c) 2016-2025 Terje Io Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC Copyright (c) 2009-2011 Simen Svale Skogsrud @@ -49,8 +49,9 @@ bool protocol_main_loop (void); bool protocol_execute_realtime (void); bool protocol_exec_rt_system (void); void protocol_execute_noop (uint_fast16_t state); -bool protocol_enqueue_rt_command (on_execute_realtime_ptr fn); -bool protocol_enqueue_foreground_task (fg_task_ptr fn, void *data); + +// Deprecated, to be deleted +#define protocol_enqueue_foreground_task(fn, data) task_run_on_startup(fn, data) // Executes the auto cycle feature, if enabled. void protocol_auto_cycle_start (void); diff --git a/settings.c b/settings.c index c7cac09..b847fa2 100644 --- a/settings.c +++ b/settings.c @@ -84,6 +84,8 @@ PROGMEM const settings_t defaults = { .flags.force_initialization_alarm = DEFAULT_FORCE_INITIALIZATION_ALARM, .flags.restore_overrides = DEFAULT_RESET_OVERRIDES, .flags.no_restore_position_after_M6 = DEFAULT_TOOLCHANGE_NO_RESTORE_POSITION, + .flags.tool_change_at_g30 = DEFAULT_TOOLCHANGE_AT_G30, + .flags.tool_change_fast_pulloff = DEFAULT_TOOLCHANGE_FAST_PROBE_PULLOFF, .flags.no_unlock_after_estop = DEFAULT_NO_UNLOCK_AFTER_ESTOP, .flags.keep_offsets_on_reset = DEFAULT_KEEP_OFFSETS_ON_RESET, @@ -1047,9 +1049,9 @@ static status_code_t set_probe_flags (setting_id_t id, uint_fast16_t int_value) static status_code_t set_tool_change_mode (setting_id_t id, uint_fast16_t int_value) { - if(!hal.driver_cap.atc && hal.stream.suspend_read && int_value <= ToolChange_FastSemiAutomatic) { + if(!hal.driver_cap.atc && hal.stream.suspend_read && int_value <= ToolChange_Ignore) { #if COMPATIBILITY_LEVEL > 1 - if((toolchange_mode_t)int_value == ToolChange_Manual_G59_3 || (toolchange_mode_t)int_value == ToolChange_SemiAutomatic || (toolchange_mode_t)int_value == ToolChange_FastSemiAutomatic) + if((toolchange_mode_t)int_value == ToolChange_Manual_G59_3 || (toolchange_mode_t)int_value == ToolChange_SemiAutomatic) return Status_InvalidStatement; #endif settings.tool_change.mode = (toolchange_mode_t)int_value; @@ -1070,12 +1072,14 @@ static status_code_t set_tool_change_probing_distance (setting_id_t id, float va return Status_OK; } -static status_code_t set_tool_restore_pos (setting_id_t id, uint_fast16_t int_value) +static status_code_t set_toolchange_flags (setting_id_t id, uint_fast16_t int_value) { if(hal.driver_cap.atc) return Status_InvalidStatement; - settings.flags.no_restore_position_after_M6 = int_value == 0; + settings.flags.no_restore_position_after_M6 = !(int_value & 0b001); + settings.flags.tool_change_at_g30 = !!(int_value & 0b010); + settings.flags.tool_change_fast_pulloff = !!(int_value & 0b100); return Status_OK; } @@ -1570,8 +1574,10 @@ static uint32_t get_int (setting_id_t id) value = settings.tool_change.mode; break; - case Setting_ToolChangeRestorePosition: - value = settings.flags.no_restore_position_after_M6 ? 0 : 1; + case Setting_ToolChangeOptions: + value = (settings.flags.no_restore_position_after_M6 ? 0b000 : 0b001) | + (settings.flags.tool_change_at_g30 ? 0b010 : 0b000) | + (settings.flags.tool_change_fast_pulloff ? 0b100 : 0b00); break; case Setting_DisableG92Persistence: @@ -1863,7 +1869,7 @@ static bool is_setting_available (const setting_detail_t *setting, uint_fast16_t case Setting_ToolChangeFeedRate: case Setting_ToolChangeSeekRate: case Setting_ToolChangePulloffRate: - case Setting_ToolChangeRestorePosition: + case Setting_ToolChangeOptions: available = !hal.driver_cap.atc; break; @@ -2139,12 +2145,12 @@ PROGMEM static const setting_detail_t setting_detail[] = { { Setting_AxisHomingFeedRate, Group_Axis0, "-axis homing locate feed rate", axis_rate, Format_Decimal, "###0", NULL, NULL, Setting_NonCoreFn, set_axis_setting, get_float, is_setting_available, AXIS_OPTS }, { Setting_AxisHomingSeekRate, Group_Axis0, "-axis homing search seek rate", axis_rate, Format_Decimal, "###0", NULL, NULL, Setting_NonCoreFn, set_axis_setting, get_float, is_setting_available, AXIS_OPTS }, { Setting_SpindleAtSpeedTolerance, Group_Spindle, "Spindle at speed tolerance", "percent", Format_Decimal, "##0.0", NULL, NULL, Setting_IsExtendedFn, set_float, get_float, is_setting_available }, - { Setting_ToolChangeMode, Group_Toolchange, "Tool change mode", NULL, Format_RadioButtons, "Normal,Manual touch off,Manual touch off @ G59.3,Automatic touch off @ G59.3,Ignore M6,Fast Automatic touch off @ G59.3", NULL, NULL, Setting_IsExtendedFn, set_tool_change_mode, get_int, is_setting_available }, + { Setting_ToolChangeMode, Group_Toolchange, "Tool change mode", NULL, Format_RadioButtons, "Normal,Manual touch off,Manual touch off @ G59.3,Automatic touch off @ G59.3,Ignore M6", NULL, NULL, Setting_IsExtendedFn, set_tool_change_mode, get_int, is_setting_available }, { Setting_ToolChangeProbingDistance, Group_Toolchange, "Tool change probing distance", "mm", Format_Decimal, "#####0.0", NULL, NULL, Setting_IsExtendedFn, set_tool_change_probing_distance, get_float, is_setting_available }, { Setting_ToolChangeFeedRate, Group_Toolchange, "Tool change locate feed rate", "mm/min", Format_Decimal, "#####0.0", NULL, NULL, Setting_IsExtended, &settings.tool_change.feed_rate, NULL, is_setting_available }, { Setting_ToolChangeSeekRate, Group_Toolchange, "Tool change search seek rate", "mm/min", Format_Decimal, "#####0.0", NULL, NULL, Setting_IsExtended, &settings.tool_change.seek_rate, NULL, is_setting_available }, { Setting_ToolChangePulloffRate, Group_Toolchange, "Tool change probe pull-off rate", "mm/min", Format_Decimal, "#####0.0", NULL, NULL, Setting_IsExtended, &settings.tool_change.pulloff_rate, NULL, is_setting_available }, - { Setting_ToolChangeRestorePosition, Group_Toolchange, "Restore position after M6", NULL, Format_Bool, NULL, NULL, NULL, Setting_IsExtendedFn, set_tool_restore_pos, get_int, is_setting_available }, + { Setting_ToolChangeOptions, Group_Toolchange, "Tool change options", NULL, Format_Bitfield, "Restore position after M6,Change tool at G30,Fast probe pull off", NULL, NULL, Setting_IsExtendedFn, set_toolchange_flags, get_int, is_setting_available }, { Setting_DualAxisLengthFailPercent, Group_Limits_DualAxis, "Dual axis length fail", "percent", Format_Decimal, "##0.0", "0", "100", Setting_IsExtended, &settings.homing.dual_axis.fail_length_percent, NULL, is_setting_available }, { Setting_DualAxisLengthFailMin, Group_Limits_DualAxis, "Dual axis length fail min", "mm", Format_Decimal, "#####0.000", NULL, NULL, Setting_IsExtended, &settings.homing.dual_axis.fail_distance_min, NULL, is_setting_available }, { Setting_DualAxisLengthFailMax, Group_Limits_DualAxis, "Dual axis length fail max", "mm", Format_Decimal, "#####0.000", NULL, NULL, Setting_IsExtended, &settings.homing.dual_axis.fail_distance_max, NULL, is_setting_available }, @@ -2331,17 +2337,21 @@ PROGMEM static const setting_descr_t setting_descr[] = { "NOTE: if the spindle on delay is set to 0 the timeout defaults to one minute." }, { Setting_ToolChangeMode, "Normal: allows jogging for manual touch off. Set new position manually.\\n\\n" - "Manual touch off: retracts tool axis to home position for tool change, use jogging or $TPW for touch off.\\n\\n" - "Manual touch off @ G59.3: retracts tool axis to home position then to G59.3 position for tool change, use jogging or $TPW for touch off.\\n\\n" - "Automatic touch off @ G59.3: retracts tool axis to home position for tool change, then to G59.3 position for automatic touch off.\\n\\n" - "Fast Automatic touch off @ G59.3: Same as automatic mode, except that it uses G38.4 style probing for faster touch off.\\n\\n" - "All modes except \"Normal\" and \"Ignore M6\" returns the tool (controlled point) to original position after touch off." + "Manual touch off: rapids to tool change position, use jogging or $TPW for touch off.\\n\\n" + "Manual touch off @ G59.3: rapids to tool change position, after change to G59.3 position for manual touch off. Use jogging or $TPW for touch off.\\n\\n" + "Automatic touch off @ G59.3: rapids to tool change position, after change to G59.3 position for automatic touch off.\\n\\n" + "Depending on settings the tool (controlled point) will be moved back to the to original position after touch off. " + "\"tool change position\" is either tool axis home, G59.3 or G30 position depending on settings." }, { Setting_ToolChangeProbingDistance, "Maximum probing distance for automatic or $TPW touch off." }, { Setting_ToolChangeFeedRate, "Feed rate to slowly engage tool change sensor to determine the tool offset accurately." }, { Setting_ToolChangeSeekRate, "Seek rate to quickly find the tool change sensor before the slower locating phase." }, { Setting_ToolChangePulloffRate, "Pull-off rate for the retract move before the slower locating phase." }, - { Setting_ToolChangeRestorePosition, "When set the spindle is moved so that the controlled point (tool tip) is the same as before the M6 command, if not the spindle is only moved to the Z home position." }, + { Setting_ToolChangeOptions, "Restore position after M6: when set the spindle is moved so that the controlled point (tool tip) is the same as before the M6 command," + "if not the spindle is only moved to the Z home position.\\n\\n" + "Change tool at G30: when set rapids to the G30 position via tool axis home. Requires axes to be homed.\\n\\n" + "Fast probe pulloff: use G38.4 style probing for faster touch off." + }, { Setting_DualAxisLengthFailPercent, "Dual axis length fail in percent of axis max travel." }, { Setting_DualAxisLengthFailMin, "Dual axis length fail minimum distance." }, { Setting_DualAxisLengthFailMax, "Dual axis length fail maximum distance." }, @@ -2475,6 +2485,9 @@ void settings_write_coord_data (coord_system_id_t id, float (*coord_data)[N_AXIS protocol_buffer_synchronize(); #endif + if(grbl.on_wco_saved) + grbl.on_wco_saved(id, (coord_data_t *)coord_data); + if(hal.nvs.type != NVS_None) hal.nvs.memcpy_to_nvs(NVS_ADDR_PARAMETERS + id * (sizeof(coord_data_t) + NVS_CRC_BYTES), (uint8_t *)coord_data, sizeof(coord_data_t), true); } @@ -2572,6 +2585,11 @@ static void sanity_check (void) } while(idx); } + if(settings.tool_change.mode > ToolChange_Ignore) { + settings.tool_change.mode = ToolChange_SemiAutomatic; + settings.flags.tool_change_fast_pulloff = On; + } + if(SLEEP_DURATION <= 0.0f) settings.flags.sleep_enable = Off; diff --git a/settings.h b/settings.h index 899ddb5..9312ea4 100644 --- a/settings.h +++ b/settings.h @@ -214,7 +214,7 @@ typedef enum { Setting_ToolChangeFeedRate = 343, Setting_ToolChangeSeekRate = 344, Setting_ToolChangePulloffRate = 345, - Setting_ToolChangeRestorePosition = 346, + Setting_ToolChangeOptions = 346, Setting_DualAxisLengthFailPercent = 347, Setting_DualAxisLengthFailMin = 348, @@ -592,7 +592,9 @@ typedef union { no_unlock_after_estop :1, settings_downgrade :1, keep_offsets_on_reset :1, - unassigned :14; + tool_change_at_g30 :1, + tool_change_fast_pulloff :1, + unassigned :12; }; } settingflags_t; @@ -815,8 +817,7 @@ typedef enum { ToolChange_Manual, ToolChange_Manual_G59_3, ToolChange_SemiAutomatic, - ToolChange_Ignore, - ToolChange_FastSemiAutomatic + ToolChange_Ignore } toolchange_mode_t; typedef struct { diff --git a/stream.c b/stream.c index abadd3b..2e8c145 100644 --- a/stream.c +++ b/stream.c @@ -339,7 +339,7 @@ static bool stream_select (const io_stream_t *stream, bool add) stream_mpg_enable(false); mpg.flags.mpg_control = On; } else if(mpg_enable) - protocol_enqueue_foreground_task(stream_mpg_set_mode, (void *)1); + task_add_immediate(stream_mpg_set_mode, (void *)1); memcpy(&hal.stream, stream, sizeof(io_stream_t)); @@ -750,7 +750,7 @@ bool debug_stream_init (void) if(stream_enumerate_streams(debug_claim_stream)) hal.debug.write(ASCII_EOL "UART debug active:" ASCII_EOL); else - protocol_enqueue_foreground_task(report_warning, "Failed to initialize debug stream!"); + task_run_on_startup(report_warning, "Failed to initialize debug stream!"); return hal.debug.write == debug_write; } diff --git a/stream_passthru.c b/stream_passthru.c index 78effe7..539fa61 100644 --- a/stream_passthru.c +++ b/stream_passthru.c @@ -168,7 +168,7 @@ void stream_passthru_init (uint8_t instance, uint32_t baud_rate, bool start) if((hal.stream.state.passthru = stream != NULL)) { - protocol_enqueue_foreground_task(passthru_start1, NULL); // enter passthrouh mode after finished booting grblHAL + task_run_on_startup(passthru_start1, NULL); // enter passthrouh mode after finished booting grblHAL memcpy(&dest, stream, sizeof(io_stream_t)); dest.set_enqueue_rt_handler(sink_uart_rx); diff --git a/task.h b/task.h index f9cccd3..3abc522 100644 --- a/task.h +++ b/task.h @@ -3,7 +3,7 @@ Part of grblHAL - Copyright (c) 2024 Terje Io + Copyright (c) 2024-2025 Terje Io grblHAL is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -26,6 +26,7 @@ typedef void (*foreground_task_ptr)(void *data); bool task_add_immediate (foreground_task_ptr fn, void *data); bool task_add_delayed (foreground_task_ptr fn, void *data, uint32_t delay_ms); +bool task_run_on_startup (foreground_task_ptr fn, void *data); void task_delete (foreground_task_ptr fn, void *data); bool task_add_systick (foreground_task_ptr fn, void *data); void task_delete_systick (foreground_task_ptr fn, void *data); diff --git a/tool_change.c b/tool_change.c index 410b4fa..06d90bb 100644 --- a/tool_change.c +++ b/tool_change.c @@ -37,21 +37,21 @@ #define TOOL_CHANGE_PROBE_RETRACT_DISTANCE 2.0f #endif -static bool block_cycle_start, probe_toolsetter; +static bool block_cycle_start, probe_toolsetter, change_at_g30; static volatile bool execute_posted = false; static volatile uint32_t spin_lock = 0; -static float tool_change_position; -static tool_data_t current_tool = {0}, *next_tool = NULL; +static tool_data_t current_tool = {}, *next_tool = NULL; static plane_t plane; -static coord_data_t target = {0}, previous; +static coord_data_t target = {}, previous; static driver_reset_ptr driver_reset = NULL; static enqueue_realtime_command_ptr enqueue_realtime_command = NULL; static control_signals_callback_ptr control_interrupt_callback = NULL; static on_homing_completed_ptr on_homing_completed = NULL; static on_probe_completed_ptr on_probe_completed; +static on_wco_saved_ptr on_wco_saved; // Clear tool length offset on homing -static void tc_on_homing_complete (axes_signals_t homing_cycle, bool success) +static void onHomingComplete (axes_signals_t homing_cycle, bool success) { if(on_homing_completed) on_homing_completed(homing_cycle, success); @@ -60,6 +60,15 @@ static void tc_on_homing_complete (axes_signals_t homing_cycle, bool success) system_clear_tlo_reference(homing_cycle); } +static void onWcoSaved (coord_system_id_t id, coord_data_t *offset) +{ + if(on_wco_saved) + on_wco_saved(id, offset); + + if(id == gc_state.modal.coord_system.id && block_cycle_start) + block_cycle_start = change_at_g30; +} + // Set tool offset on successful $TPW probe, prompt for retry on failure. // Called via probe completed event. static void onProbeCompleted (void) @@ -128,21 +137,25 @@ static void reset (void) // Restore coolant and spindle status, return controlled point to original position. static bool restore (void) { + bool ok; plan_line_data_t plan_data; plan_data_init(&plan_data); plan_data.condition.rapid_motion = On; - target.values[plane.axis_linear] = tool_change_position; - mc_line(target.values, &plan_data); + if(!(ok = (target.values[plane.axis_0] == previous.values[plane.axis_0] && + target.values[plane.axis_1] == previous.values[plane.axis_1]))) { - if(!settings.flags.no_restore_position_after_M6) { - memcpy(&target, &previous, sizeof(coord_data_t)); - target.values[plane.axis_linear] = tool_change_position; - mc_line(target.values, &plan_data); + target.values[plane.axis_linear] = sys.home_position[plane.axis_linear]; + + if((ok = mc_line(target.values, &plan_data)) && !settings.flags.no_restore_position_after_M6) { + memcpy(&target, &previous, sizeof(coord_data_t)); + target.values[plane.axis_linear] = sys.home_position[plane.axis_linear]; + ok = mc_line(target.values, &plan_data); + } } - if(protocol_buffer_synchronize()) { + if(ok && protocol_buffer_synchronize()) { sync_position(); @@ -164,6 +177,48 @@ static bool restore (void) return !ABORTED; } +static bool go_linear_home (plan_line_data_t *pl_data) +{ + system_convert_array_steps_to_mpos(target.values, sys.position); + + if(target.values[plane.axis_linear] != sys.home_position[plane.axis_linear]) { + + target.values[plane.axis_linear] = sys.home_position[plane.axis_linear]; + if(!mc_line(target.values, pl_data)) + return false; + } + + return true; +} +#if COMPATIBILITY_LEVEL <= 1 + +static bool go_toolsetter (plan_line_data_t *pl_data) +{ + // G59.3 contains offsets to toolsetter. + settings_read_coord_data(CoordinateSystem_G59_3, &target.values); + + float tmp_pos = target.values[plane.axis_linear]; + + target.values[plane.axis_linear] = sys.home_position[plane.axis_linear]; + + if(probe_toolsetter) + grbl.on_probe_toolsetter(next_tool, &target, false, true); + + if(!mc_line(target.values, pl_data)) + return false; + + target.values[plane.axis_linear] = tmp_pos; + if(!mc_line(target.values, pl_data)) + return false; + + if(probe_toolsetter) + grbl.on_probe_toolsetter(next_tool, NULL, true, true); + + return true; +} + +#endif + // Issue warning on cycle start event if touch off by $TPW is pending. // Used in Manual and Manual_G59_3 modes ($341=1 or $341=2). Called from the foreground process. static void execute_warning (void *data) @@ -171,6 +226,38 @@ static void execute_warning (void *data) grbl.report.feedback_message(Message_ExecuteTPW); } +// Execute restore position after tool change, either back to original or to toolsetter for touch off. +// Used when G30 position is used for changing the tool. Called from the foreground process. +static void execute_return_from_g30 (void *data) +{ + bool ok; + plan_line_data_t plan_data; + + plan_data_init(&plan_data); + plan_data.condition.rapid_motion = On; + + if((ok = go_linear_home(&plan_data))) { +#if COMPATIBILITY_LEVEL <= 1 + if(settings.tool_change.mode == ToolChange_Manual_G59_3) + ok = go_toolsetter(&plan_data); + else +#endif + { + // Rapid to original XY position. + target.values[plane.axis_0] = previous.values[plane.axis_0]; + target.values[plane.axis_1] = previous.values[plane.axis_1]; + ok = mc_line(target.values, &plan_data); + } + } + + if(ok) { + protocol_buffer_synchronize(); + sync_position(); + } + + change_at_g30 = execute_posted = false; +} + // Execute restore position after touch off (on cycle start event). // Used in Manual and Manual_G59_3 modes ($341=1 or $341=2). Called from the foreground process. static void execute_restore (void *data) @@ -205,7 +292,7 @@ static void execute_probe (void *data) bool ok; coord_data_t offset; plan_line_data_t plan_data; - gc_parser_flags_t flags = {0}; + gc_parser_flags_t flags = {}; // G59.3 contains offsets to position of TLS. settings_read_coord_data(CoordinateSystem_G59_3, &offset.values); @@ -213,13 +300,15 @@ static void execute_probe (void *data) plan_data_init(&plan_data); plan_data.condition.rapid_motion = On; + ok = !change_at_g30 || go_linear_home(&plan_data); + target.values[plane.axis_0] = offset.values[plane.axis_0]; target.values[plane.axis_1] = offset.values[plane.axis_1]; if(probe_toolsetter) grbl.on_probe_toolsetter(next_tool, &target, false, true); - if((ok = mc_line(target.values, &plan_data))) { + if((ok = ok && mc_line(target.values, &plan_data))) { target.values[plane.axis_linear] = offset.values[plane.axis_linear]; ok = mc_line(target.values, &plan_data); @@ -233,29 +322,24 @@ static void execute_probe (void *data) set_probe_target(&target, plane.axis_linear); - if((ok = ok && mc_probe_cycle(target.values, &plan_data, flags) == GCProbe_Found)) - { + if((ok = ok && mc_probe_cycle(target.values, &plan_data, flags) == GCProbe_Found)) { + system_convert_array_steps_to_mpos(target.values, sys.probe_position); - if(settings.tool_change.mode == ToolChange_FastSemiAutomatic){ - - // Retract slowly until contact lost. - plan_data.feed_rate = settings.tool_change.feed_rate; target.values[plane.axis_linear] += TOOL_CHANGE_PROBE_RETRACT_DISTANCE; - flags.probe_is_away = true; - ok = mc_probe_cycle(target.values, &plan_data, flags) == GCProbe_Found; - } else { - // Retract a bit and perform slow probe. - plan_data.feed_rate = settings.tool_change.pulloff_rate; - target.values[plane.axis_linear] += TOOL_CHANGE_PROBE_RETRACT_DISTANCE; - if((ok = mc_line(target.values, &plan_data))) { - plan_data.feed_rate = settings.tool_change.feed_rate; - target.values[plane.axis_linear] -= (TOOL_CHANGE_PROBE_RETRACT_DISTANCE + 2.0f); - ok = mc_probe_cycle(target.values, &plan_data, flags) == GCProbe_Found; + if((flags.probe_is_away = settings.flags.tool_change_fast_pulloff)) + plan_data.feed_rate = settings.tool_change.feed_rate; // Retract slowly until contact lost. + else { + // Retract a bit and perform slow probe. + plan_data.feed_rate = settings.tool_change.pulloff_rate; + if((ok = mc_line(target.values, &plan_data))) { + plan_data.feed_rate = settings.tool_change.feed_rate; + target.values[plane.axis_linear] -= (TOOL_CHANGE_PROBE_RETRACT_DISTANCE + 2.0f); + } } + ok = ok && mc_probe_cycle(target.values, &plan_data, flags) == GCProbe_Found; } - } if(ok) { if(!(sys.tlo_reference_set.mask & bit(plane.axis_linear))) { @@ -287,14 +371,13 @@ ISR_CODE static void ISR_FUNC(trap_control_cycle_start)(control_signals_t signal if(signals.cycle_start) { if(!execute_posted) { if(!block_cycle_start) - execute_posted = protocol_enqueue_foreground_task( - (settings.tool_change.mode == ToolChange_SemiAutomatic || - settings.tool_change.mode == ToolChange_FastSemiAutomatic) - ? execute_probe - : execute_restore, - NULL); + execute_posted = task_add_immediate(settings.tool_change.mode == ToolChange_SemiAutomatic + ? execute_probe + : execute_restore, NULL); + else if(change_at_g30) + execute_posted = task_add_immediate(execute_return_from_g30, NULL); else - protocol_enqueue_foreground_task(execute_warning, NULL); + task_add_immediate(execute_warning, NULL); } signals.cycle_start = Off; } else @@ -312,14 +395,13 @@ ISR_CODE static bool ISR_FUNC(trap_stream_cycle_start)(char c) if((drop = (c == CMD_CYCLE_START || c == CMD_CYCLE_START_LEGACY))) { if(!execute_posted) { if(!block_cycle_start) - execute_posted = protocol_enqueue_foreground_task( - (settings.tool_change.mode == ToolChange_SemiAutomatic || - settings.tool_change.mode == ToolChange_FastSemiAutomatic) - ? execute_probe - : execute_restore, - NULL); + execute_posted = task_add_immediate(settings.tool_change.mode == ToolChange_SemiAutomatic + ? execute_probe + : execute_restore, NULL); + else if(change_at_g30) + execute_posted = task_add_immediate(execute_return_from_g30, NULL); else - protocol_enqueue_foreground_task(execute_warning, NULL); + task_add_immediate(execute_warning, NULL); } } else drop = enqueue_realtime_command(c); @@ -356,7 +438,7 @@ static status_code_t tool_change (parser_state_t *parser_state) return Status_OK; #if COMPATIBILITY_LEVEL > 1 - if(settings.tool_change.mode == ToolChange_Manual_G59_3 || settings.tool_change.mode == ToolChange_SemiAutomatic || settings.tool_change.mode == ToolChange_FastSemiAutomatic) + if(settings.tool_change.mode == ToolChange_Manual_G59_3 || settings.tool_change.mode == ToolChange_SemiAutomatic) return Status_GcodeUnsupportedCommand; #endif @@ -381,12 +463,15 @@ static status_code_t tool_change (parser_state_t *parser_state) if((sys.homed.mask & homed_req) != homed_req) return Status_HomingRequired; - if(settings.tool_change.mode != ToolChange_SemiAutomatic && - settings.tool_change.mode != ToolChange_FastSemiAutomatic) - grbl.on_probe_completed = on_probe_completed; + plan_line_data_t plan_data; + coord_data_t change_position; - block_cycle_start = (settings.tool_change.mode != ToolChange_SemiAutomatic && - settings.tool_change.mode != ToolChange_FastSemiAutomatic); + if(settings.tool_change.mode != ToolChange_SemiAutomatic && grbl.on_probe_completed != onProbeCompleted) { + on_probe_completed = grbl.on_probe_completed; + grbl.on_probe_completed = onProbeCompleted; + } + + block_cycle_start = settings.tool_change.mode != ToolChange_SemiAutomatic; // Stop spindle and coolant. spindle_all_off(); @@ -396,8 +481,7 @@ static status_code_t tool_change (parser_state_t *parser_state) probe_toolsetter = grbl.on_probe_toolsetter != NULL && (settings.tool_change.mode == ToolChange_Manual || settings.tool_change.mode == ToolChange_Manual_G59_3 || - settings.tool_change.mode == ToolChange_SemiAutomatic || - settings.tool_change.mode == ToolChange_FastSemiAutomatic); + settings.tool_change.mode == ToolChange_SemiAutomatic); // Save current position. system_convert_array_steps_to_mpos(previous.values, sys.position); @@ -406,7 +490,7 @@ static status_code_t tool_change (parser_state_t *parser_state) previous.values[plane.axis_linear] -= gc_get_offset(plane.axis_linear, false); - plan_line_data_t plan_data; + memcpy(&change_position, &previous, sizeof(coord_data_t)); plan_data_init(&plan_data); plan_data.condition.rapid_motion = On; @@ -416,36 +500,39 @@ static status_code_t tool_change (parser_state_t *parser_state) // tool_change_position = ? //else - tool_change_position = sys.home_position[plane.axis_linear]; // - settings.homing.flags.force_set_origin ? LINEAR_AXIS_HOME_OFFSET : 0.0f; + if((change_at_g30 = (settings.flags.tool_change_at_g30) && (sys.homed.mask & (X_AXIS_BIT|Y_AXIS_BIT|Z_AXIS_BIT)) == (X_AXIS_BIT|Y_AXIS_BIT|Z_AXIS_BIT))) + settings_read_coord_data(CoordinateSystem_G30, &change_position.values); + else + change_position.values[plane.axis_linear] = sys.home_position[plane.axis_linear]; // - settings.homing.flags.force_set_origin ? LINEAR_AXIS_HOME_OFFSET : 0.0f; // Rapid to home position of linear axis. - memcpy(&target, &previous, sizeof(coord_data_t)); - target.values[plane.axis_linear] = tool_change_position; - if(!mc_line(target.values, &plan_data)) + if(!go_linear_home(&plan_data)) return Status_Reset; + if(change_at_g30) { + + // Rapid to G30 position. + if(!(target.values[plane.axis_0] == change_position.values[plane.axis_0] && + target.values[plane.axis_1] == change_position.values[plane.axis_1])) { + + target.values[plane.axis_0] = change_position.values[plane.axis_0]; + target.values[plane.axis_1] = change_position.values[plane.axis_1]; + if(!mc_line(target.values, &plan_data)) + return Status_Reset; + } + + if(target.values[plane.axis_linear] != change_position.values[plane.axis_linear]) { + + target.values[plane.axis_linear] = change_position.values[plane.axis_linear]; + if(!mc_line(target.values, &plan_data)) + return Status_Reset; + } + } + #if COMPATIBILITY_LEVEL <= 1 - if(settings.tool_change.mode == ToolChange_Manual_G59_3) { - - // G59.3 contains offsets to tool change position. - settings_read_coord_data(CoordinateSystem_G59_3, &target.values); - - float tmp_pos = target.values[plane.axis_linear]; - - target.values[plane.axis_linear] = tool_change_position; - - if(probe_toolsetter) - grbl.on_probe_toolsetter(next_tool, &target, false, true); - - if(!mc_line(target.values, &plan_data)) + else if(settings.tool_change.mode == ToolChange_Manual_G59_3) { + if(!go_toolsetter(&plan_data)) return Status_Reset; - - target.values[plane.axis_linear] = tmp_pos; - if(!mc_line(target.values, &plan_data)) - return Status_Reset; - - if(probe_toolsetter) - grbl.on_probe_toolsetter(next_tool, NULL, true, true); } #endif @@ -483,9 +570,14 @@ void tc_init (void) hal.tool.change = tool_change; grbl.on_toolchange_ack = on_toolchange_ack; if(!on_homing_subscribed) { + on_homing_subscribed = true; + on_homing_completed = grbl.on_homing_completed; - grbl.on_homing_completed = tc_on_homing_complete; + grbl.on_homing_completed = onHomingComplete; + + on_wco_saved = grbl.on_wco_saved; + grbl.on_wco_saved = onWcoSaved; } if(driver_reset == NULL) { driver_reset = hal.driver_reset; @@ -502,6 +594,11 @@ status_code_t tc_probe_workpiece (void) if(!(settings.tool_change.mode == ToolChange_Manual || settings.tool_change.mode == ToolChange_Manual_G59_3) || enqueue_realtime_command == NULL) return Status_InvalidStatement; + if(change_at_g30) { + grbl.report.feedback_message(Message_CycleStart2TouchOff); + return Status_OK; + } + // TODO: add check for reference offset set? bool ok; @@ -527,23 +624,28 @@ status_code_t tc_probe_workpiece (void) { system_convert_array_steps_to_mpos(target.values, sys.probe_position); - // Retract a bit and perform slow probe. - plan_data.feed_rate = settings.tool_change.pulloff_rate; target.values[plane.axis_linear] += TOOL_CHANGE_PROBE_RETRACT_DISTANCE; - if((ok = mc_line(target.values, &plan_data))) { - plan_data.feed_rate = settings.tool_change.feed_rate; - target.values[plane.axis_linear] -= (TOOL_CHANGE_PROBE_RETRACT_DISTANCE + 2.0f); - if((ok = mc_probe_cycle(target.values, &plan_data, flags) == GCProbe_Found)) { - // Retract a bit again so that any touch plate can be removed - system_convert_array_steps_to_mpos(target.values, sys.probe_position); - plan_data.feed_rate = settings.tool_change.seek_rate; - target.values[plane.axis_linear] += TOOL_CHANGE_PROBE_RETRACT_DISTANCE * 2.0f; - if(target.values[plane.axis_linear] > tool_change_position) - target.values[plane.axis_linear] = tool_change_position; - ok = mc_line(target.values, &plan_data); + if((flags.probe_is_away = settings.flags.tool_change_fast_pulloff)) + plan_data.feed_rate = settings.tool_change.feed_rate; // Retract slowly until contact lost. + else { + // Retract a bit before performing slow probe. + plan_data.feed_rate = settings.tool_change.pulloff_rate; + if((ok = mc_line(target.values, &plan_data))) { + plan_data.feed_rate = settings.tool_change.feed_rate; + target.values[plane.axis_linear] -= (TOOL_CHANGE_PROBE_RETRACT_DISTANCE + 2.0f); } } + + if((ok = ok && mc_probe_cycle(target.values, &plan_data, flags) == GCProbe_Found)) { + // Retract a bit again so that any touch plate can be removed + system_convert_array_steps_to_mpos(target.values, sys.probe_position); + plan_data.feed_rate = settings.tool_change.seek_rate; + target.values[plane.axis_linear] += TOOL_CHANGE_PROBE_RETRACT_DISTANCE * 2.0f; + if(target.values[plane.axis_linear] > sys.home_position[plane.axis_linear]) + target.values[plane.axis_linear] = sys.home_position[plane.axis_linear]; + ok = mc_line(target.values, &plan_data); + } } if(ok && protocol_buffer_synchronize()) {