From 2cee78634cc65b27f588284304e6821f0ad099c1 Mon Sep 17 00:00:00 2001 From: Miguel Risco-Castillo Date: Mon, 8 Dec 2025 20:58:13 -0500 Subject: [PATCH 01/17] =?UTF-8?q?=F0=9F=8E=A8=20e3v2=20->=20dwin=20(#28211?= =?UTF-8?q?)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/MarlinCore.cpp | 6 +++--- Marlin/src/gcode/bedlevel/abl/G29.cpp | 2 +- Marlin/src/gcode/calibrate/G28.cpp | 2 +- Marlin/src/gcode/feature/powerloss/M1000.cpp | 4 ++-- Marlin/src/gcode/stats/M75-M78.cpp | 2 +- Marlin/src/inc/Conditionals-2-LCD.h | 2 +- Marlin/src/lcd/{e3v2 => dwin}/README.md | 0 Marlin/src/lcd/{e3v2 => dwin}/common/dwin_api.cpp | 0 Marlin/src/lcd/{e3v2 => dwin}/common/dwin_api.h | 4 ++-- Marlin/src/lcd/{e3v2 => dwin}/common/dwin_color.h | 0 Marlin/src/lcd/{e3v2 => dwin}/common/dwin_font.h | 0 Marlin/src/lcd/{e3v2 => dwin}/common/dwin_set.h | 0 Marlin/src/lcd/{e3v2 => dwin}/common/encoder.cpp | 2 +- Marlin/src/lcd/{e3v2 => dwin}/common/encoder.h | 2 +- Marlin/src/lcd/{e3v2 => dwin}/common/limits.h | 2 +- Marlin/src/lcd/{e3v2 => dwin}/creality/dwin.cpp | 0 Marlin/src/lcd/{e3v2 => dwin}/creality/dwin.h | 0 Marlin/src/lcd/{e3v2 => dwin}/creality/dwin_lcd.cpp | 2 +- Marlin/src/lcd/{e3v2 => dwin}/creality/dwin_lcd.h | 0 Marlin/src/lcd/{e3v2 => dwin}/jyersui/dwin.cpp | 2 +- Marlin/src/lcd/{e3v2 => dwin}/jyersui/dwin.h | 2 +- Marlin/src/lcd/{e3v2 => dwin}/jyersui/dwin_lcd.cpp | 2 +- Marlin/src/lcd/{e3v2 => dwin}/jyersui/dwin_lcd.h | 2 +- Marlin/src/lcd/{e3v2 => dwin}/marlinui/dwin_lcd.cpp | 2 +- Marlin/src/lcd/{e3v2 => dwin}/marlinui/dwin_lcd.h | 2 +- Marlin/src/lcd/{e3v2 => dwin}/marlinui/dwin_string.cpp | 0 Marlin/src/lcd/{e3v2 => dwin}/marlinui/dwin_string.h | 0 Marlin/src/lcd/{e3v2 => dwin}/marlinui/game.cpp | 2 +- Marlin/src/lcd/{e3v2 => dwin}/marlinui/game.h | 0 .../src/lcd/{e3v2 => dwin}/marlinui/lcdprint_dwin.cpp | 2 +- Marlin/src/lcd/{e3v2 => dwin}/marlinui/lcdprint_dwin.h | 0 Marlin/src/lcd/{e3v2 => dwin}/marlinui/marlinui_dwin.h | 2 +- Marlin/src/lcd/{e3v2 => dwin}/marlinui/ui_common.cpp | 0 .../lcd/{e3v2 => dwin}/marlinui/ui_status_480x272.cpp | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/base64.h | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/bedlevel_tools.cpp | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/bedlevel_tools.h | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/dwin.cpp | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/dwin.h | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/dwin_defines.h | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/dwin_lcd.cpp | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/dwin_lcd.h | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/dwin_popup.cpp | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/dwin_popup.h | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/dwinui.cpp | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/dwinui.h | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/endstop_diag.cpp | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/endstop_diag.h | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/gcode_preview.cpp | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/gcode_preview.h | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/lockscreen.cpp | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/lockscreen.h | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/menus.cpp | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/menus.h | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/meshviewer.cpp | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/meshviewer.h | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/plot.cpp | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/plot.h | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/printstats.cpp | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/printstats.h | 0 Marlin/src/lcd/{e3v2 => dwin}/proui/proui_extui.cpp | 0 Marlin/src/lcd/lcdprint.h | 2 +- Marlin/src/lcd/marlinui.cpp | 4 ++-- Marlin/src/lcd/marlinui.h | 6 +++--- Marlin/src/lcd/menu/game/types.h | 2 +- Marlin/src/module/motion.h | 2 +- Marlin/src/module/settings.cpp | 6 +++--- Marlin/src/module/temperature.cpp | 2 +- Marlin/src/sd/cardreader.cpp | 2 +- ini/features.ini | 10 +++++----- platformio.ini | 2 +- 71 files changed, 43 insertions(+), 43 deletions(-) rename Marlin/src/lcd/{e3v2 => dwin}/README.md (100%) rename Marlin/src/lcd/{e3v2 => dwin}/common/dwin_api.cpp (100%) rename Marlin/src/lcd/{e3v2 => dwin}/common/dwin_api.h (99%) rename Marlin/src/lcd/{e3v2 => dwin}/common/dwin_color.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/common/dwin_font.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/common/dwin_set.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/common/encoder.cpp (99%) rename Marlin/src/lcd/{e3v2 => dwin}/common/encoder.h (98%) rename Marlin/src/lcd/{e3v2 => dwin}/common/limits.h (99%) rename Marlin/src/lcd/{e3v2 => dwin}/creality/dwin.cpp (100%) rename Marlin/src/lcd/{e3v2 => dwin}/creality/dwin.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/creality/dwin_lcd.cpp (98%) rename Marlin/src/lcd/{e3v2 => dwin}/creality/dwin_lcd.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/jyersui/dwin.cpp (99%) rename Marlin/src/lcd/{e3v2 => dwin}/jyersui/dwin.h (99%) rename Marlin/src/lcd/{e3v2 => dwin}/jyersui/dwin_lcd.cpp (98%) rename Marlin/src/lcd/{e3v2 => dwin}/jyersui/dwin_lcd.h (96%) rename Marlin/src/lcd/{e3v2 => dwin}/marlinui/dwin_lcd.cpp (98%) rename Marlin/src/lcd/{e3v2 => dwin}/marlinui/dwin_lcd.h (98%) rename Marlin/src/lcd/{e3v2 => dwin}/marlinui/dwin_string.cpp (100%) rename Marlin/src/lcd/{e3v2 => dwin}/marlinui/dwin_string.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/marlinui/game.cpp (99%) rename Marlin/src/lcd/{e3v2 => dwin}/marlinui/game.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/marlinui/lcdprint_dwin.cpp (99%) rename Marlin/src/lcd/{e3v2 => dwin}/marlinui/lcdprint_dwin.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/marlinui/marlinui_dwin.h (99%) rename Marlin/src/lcd/{e3v2 => dwin}/marlinui/ui_common.cpp (100%) rename Marlin/src/lcd/{e3v2 => dwin}/marlinui/ui_status_480x272.cpp (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/base64.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/bedlevel_tools.cpp (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/bedlevel_tools.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/dwin.cpp (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/dwin.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/dwin_defines.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/dwin_lcd.cpp (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/dwin_lcd.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/dwin_popup.cpp (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/dwin_popup.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/dwinui.cpp (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/dwinui.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/endstop_diag.cpp (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/endstop_diag.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/gcode_preview.cpp (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/gcode_preview.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/lockscreen.cpp (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/lockscreen.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/menus.cpp (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/menus.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/meshviewer.cpp (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/meshviewer.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/plot.cpp (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/plot.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/printstats.cpp (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/printstats.h (100%) rename Marlin/src/lcd/{e3v2 => dwin}/proui/proui_extui.cpp (100%) diff --git a/Marlin/src/MarlinCore.cpp b/Marlin/src/MarlinCore.cpp index 6610e90ab7..6d672e3b8c 100644 --- a/Marlin/src/MarlinCore.cpp +++ b/Marlin/src/MarlinCore.cpp @@ -74,11 +74,11 @@ #endif #if HAS_DWIN_E3V2 - #include "lcd/e3v2/common/encoder.h" + #include "lcd/dwin/common/encoder.h" #if ENABLED(DWIN_CREALITY_LCD) - #include "lcd/e3v2/creality/dwin.h" + #include "lcd/dwin/creality/dwin.h" #elif ENABLED(DWIN_CREALITY_LCD_JYERSUI) - #include "lcd/e3v2/jyersui/dwin.h" + #include "lcd/dwin/jyersui/dwin.h" #elif ENABLED(SOVOL_SV06_RTS) #include "lcd/sovol_rts/sovol_rts.h" #endif diff --git a/Marlin/src/gcode/bedlevel/abl/G29.cpp b/Marlin/src/gcode/bedlevel/abl/G29.cpp index 058d4ea4ab..800d51dac6 100644 --- a/Marlin/src/gcode/bedlevel/abl/G29.cpp +++ b/Marlin/src/gcode/bedlevel/abl/G29.cpp @@ -51,7 +51,7 @@ #if ENABLED(EXTENSIBLE_UI) #include "../../../lcd/extui/ui_api.h" #elif ENABLED(DWIN_CREALITY_LCD) - #include "../../../lcd/e3v2/creality/dwin.h" + #include "../../../lcd/dwin/creality/dwin.h" #elif ENABLED(SOVOL_SV06_RTS) #include "../../../lcd/sovol_rts/sovol_rts.h" #endif diff --git a/Marlin/src/gcode/calibrate/G28.cpp b/Marlin/src/gcode/calibrate/G28.cpp index 4d3d7841e7..4a0ecffc4b 100644 --- a/Marlin/src/gcode/calibrate/G28.cpp +++ b/Marlin/src/gcode/calibrate/G28.cpp @@ -61,7 +61,7 @@ #if ENABLED(EXTENSIBLE_UI) #include "../../lcd/extui/ui_api.h" #elif ENABLED(DWIN_CREALITY_LCD) - #include "../../lcd/e3v2/creality/dwin.h" + #include "../../lcd/dwin/creality/dwin.h" #elif ENABLED(SOVOL_SV06_RTS) #include "../../lcd/sovol_rts/sovol_rts.h" #endif diff --git a/Marlin/src/gcode/feature/powerloss/M1000.cpp b/Marlin/src/gcode/feature/powerloss/M1000.cpp index c85817cd3e..15597eac59 100644 --- a/Marlin/src/gcode/feature/powerloss/M1000.cpp +++ b/Marlin/src/gcode/feature/powerloss/M1000.cpp @@ -36,9 +36,9 @@ #if ENABLED(EXTENSIBLE_UI) #include "../../../lcd/extui/ui_api.h" #elif ENABLED(DWIN_CREALITY_LCD) - #include "../../../lcd/e3v2/creality/dwin.h" + #include "../../../lcd/dwin/creality/dwin.h" #elif ENABLED(DWIN_CREALITY_LCD_JYERSUI) - #include "../../../lcd/e3v2/jyersui/dwin.h" // Temporary fix until it can be better implemented + #include "../../../lcd/dwin/jyersui/dwin.h" // Temporary fix until it can be better implemented #endif #define DEBUG_OUT ENABLED(DEBUG_POWER_LOSS_RECOVERY) diff --git a/Marlin/src/gcode/stats/M75-M78.cpp b/Marlin/src/gcode/stats/M75-M78.cpp index 8c7378c977..b539bf30dc 100644 --- a/Marlin/src/gcode/stats/M75-M78.cpp +++ b/Marlin/src/gcode/stats/M75-M78.cpp @@ -30,7 +30,7 @@ #include "../../MarlinCore.h" // for startOrResumeJob #if ENABLED(DWIN_LCD_PROUI) - #include "../../lcd/e3v2/proui/dwin.h" + #include "../../lcd/dwin/proui/dwin.h" #endif /** diff --git a/Marlin/src/inc/Conditionals-2-LCD.h b/Marlin/src/inc/Conditionals-2-LCD.h index 49d8121f0c..72bad45c89 100644 --- a/Marlin/src/inc/Conditionals-2-LCD.h +++ b/Marlin/src/inc/Conditionals-2-LCD.h @@ -570,7 +570,7 @@ #define EXTENSIBLE_UI #endif -// E3V2 extras +// DWIN extras #if ANY(HAS_DWIN_E3V2, IS_DWIN_MARLINUI, SOVOL_SV06_RTS) #define SERIAL_CATCHALL 0 #define HAS_LCD_BRIGHTNESS 1 diff --git a/Marlin/src/lcd/e3v2/README.md b/Marlin/src/lcd/dwin/README.md similarity index 100% rename from Marlin/src/lcd/e3v2/README.md rename to Marlin/src/lcd/dwin/README.md diff --git a/Marlin/src/lcd/e3v2/common/dwin_api.cpp b/Marlin/src/lcd/dwin/common/dwin_api.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/common/dwin_api.cpp rename to Marlin/src/lcd/dwin/common/dwin_api.cpp diff --git a/Marlin/src/lcd/e3v2/common/dwin_api.h b/Marlin/src/lcd/dwin/common/dwin_api.h similarity index 99% rename from Marlin/src/lcd/e3v2/common/dwin_api.h rename to Marlin/src/lcd/dwin/common/dwin_api.h index bf50a78e2f..15c7ba3c94 100644 --- a/Marlin/src/lcd/e3v2/common/dwin_api.h +++ b/Marlin/src/lcd/dwin/common/dwin_api.h @@ -24,9 +24,9 @@ #include "../../../inc/MarlinConfig.h" // -// e3v2/common/dwin_api.h +// dwin/common/dwin_api.h // -// Included by: e3v2/*/dwin_lcd.h +// Included by: dwin/*/dwin_lcd.h // #if ENABLED(DWIN_MARLINUI_LANDSCAPE) diff --git a/Marlin/src/lcd/e3v2/common/dwin_color.h b/Marlin/src/lcd/dwin/common/dwin_color.h similarity index 100% rename from Marlin/src/lcd/e3v2/common/dwin_color.h rename to Marlin/src/lcd/dwin/common/dwin_color.h diff --git a/Marlin/src/lcd/e3v2/common/dwin_font.h b/Marlin/src/lcd/dwin/common/dwin_font.h similarity index 100% rename from Marlin/src/lcd/e3v2/common/dwin_font.h rename to Marlin/src/lcd/dwin/common/dwin_font.h diff --git a/Marlin/src/lcd/e3v2/common/dwin_set.h b/Marlin/src/lcd/dwin/common/dwin_set.h similarity index 100% rename from Marlin/src/lcd/e3v2/common/dwin_set.h rename to Marlin/src/lcd/dwin/common/dwin_set.h diff --git a/Marlin/src/lcd/e3v2/common/encoder.cpp b/Marlin/src/lcd/dwin/common/encoder.cpp similarity index 99% rename from Marlin/src/lcd/e3v2/common/encoder.cpp rename to Marlin/src/lcd/dwin/common/encoder.cpp index 2f050e92fd..8eea288d12 100644 --- a/Marlin/src/lcd/e3v2/common/encoder.cpp +++ b/Marlin/src/lcd/dwin/common/encoder.cpp @@ -21,7 +21,7 @@ */ /***************************************************************************** - * @file lcd/e3v2/common/encoder.cpp + * @file lcd/dwin/common/encoder.cpp * @brief Rotary encoder functions *****************************************************************************/ diff --git a/Marlin/src/lcd/e3v2/common/encoder.h b/Marlin/src/lcd/dwin/common/encoder.h similarity index 98% rename from Marlin/src/lcd/e3v2/common/encoder.h rename to Marlin/src/lcd/dwin/common/encoder.h index 428193ca65..8a8f2fcaa3 100644 --- a/Marlin/src/lcd/e3v2/common/encoder.h +++ b/Marlin/src/lcd/dwin/common/encoder.h @@ -22,7 +22,7 @@ #pragma once /***************************************************************************** - * @file lcd/e3v2/common/encoder.h + * @file lcd/dwin/common/encoder.h * @brief Rotary encoder functions ****************************************************************************/ diff --git a/Marlin/src/lcd/e3v2/common/limits.h b/Marlin/src/lcd/dwin/common/limits.h similarity index 99% rename from Marlin/src/lcd/e3v2/common/limits.h rename to Marlin/src/lcd/dwin/common/limits.h index c773240b28..1e2a77980c 100644 --- a/Marlin/src/lcd/e3v2/common/limits.h +++ b/Marlin/src/lcd/dwin/common/limits.h @@ -22,7 +22,7 @@ #pragma once /***************************************************************************** - * @file lcd/e3v2/common/limits.h + * @file lcd/dwin/common/limits.h * @brief Limits for UI values ****************************************************************************/ diff --git a/Marlin/src/lcd/e3v2/creality/dwin.cpp b/Marlin/src/lcd/dwin/creality/dwin.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/creality/dwin.cpp rename to Marlin/src/lcd/dwin/creality/dwin.cpp diff --git a/Marlin/src/lcd/e3v2/creality/dwin.h b/Marlin/src/lcd/dwin/creality/dwin.h similarity index 100% rename from Marlin/src/lcd/e3v2/creality/dwin.h rename to Marlin/src/lcd/dwin/creality/dwin.h diff --git a/Marlin/src/lcd/e3v2/creality/dwin_lcd.cpp b/Marlin/src/lcd/dwin/creality/dwin_lcd.cpp similarity index 98% rename from Marlin/src/lcd/e3v2/creality/dwin_lcd.cpp rename to Marlin/src/lcd/dwin/creality/dwin_lcd.cpp index 649e1b4771..60c49e8d96 100644 --- a/Marlin/src/lcd/e3v2/creality/dwin_lcd.cpp +++ b/Marlin/src/lcd/dwin/creality/dwin_lcd.cpp @@ -21,7 +21,7 @@ */ /******************************************************************************** - * @file lcd/e3v2/creality/dwin_lcd.cpp + * @file lcd/dwin/creality/dwin_lcd.cpp * @author LEO / Creality3D * @date 2019/07/18 * @version 2.0.1 diff --git a/Marlin/src/lcd/e3v2/creality/dwin_lcd.h b/Marlin/src/lcd/dwin/creality/dwin_lcd.h similarity index 100% rename from Marlin/src/lcd/e3v2/creality/dwin_lcd.h rename to Marlin/src/lcd/dwin/creality/dwin_lcd.h diff --git a/Marlin/src/lcd/e3v2/jyersui/dwin.cpp b/Marlin/src/lcd/dwin/jyersui/dwin.cpp similarity index 99% rename from Marlin/src/lcd/e3v2/jyersui/dwin.cpp rename to Marlin/src/lcd/dwin/jyersui/dwin.cpp index 2cede088ec..1e8cdb1a55 100644 --- a/Marlin/src/lcd/e3v2/jyersui/dwin.cpp +++ b/Marlin/src/lcd/dwin/jyersui/dwin.cpp @@ -21,7 +21,7 @@ */ /** - * lcd/e3v2/jyersui/dwin.cpp + * lcd/dwin/jyersui/dwin.cpp */ #include "../../../inc/MarlinConfigPre.h" diff --git a/Marlin/src/lcd/e3v2/jyersui/dwin.h b/Marlin/src/lcd/dwin/jyersui/dwin.h similarity index 99% rename from Marlin/src/lcd/e3v2/jyersui/dwin.h rename to Marlin/src/lcd/dwin/jyersui/dwin.h index 890cc79999..22a56b190c 100644 --- a/Marlin/src/lcd/e3v2/jyersui/dwin.h +++ b/Marlin/src/lcd/dwin/jyersui/dwin.h @@ -22,7 +22,7 @@ #pragma once /** - * lcd/e3v2/jyersui/dwin.h + * lcd/dwin/jyersui/dwin.h */ #include "dwin_lcd.h" diff --git a/Marlin/src/lcd/e3v2/jyersui/dwin_lcd.cpp b/Marlin/src/lcd/dwin/jyersui/dwin_lcd.cpp similarity index 98% rename from Marlin/src/lcd/e3v2/jyersui/dwin_lcd.cpp rename to Marlin/src/lcd/dwin/jyersui/dwin_lcd.cpp index 96518b8c21..717e17868d 100644 --- a/Marlin/src/lcd/e3v2/jyersui/dwin_lcd.cpp +++ b/Marlin/src/lcd/dwin/jyersui/dwin_lcd.cpp @@ -21,7 +21,7 @@ */ /******************************************************************************** - * @file lcd/e3v2/jyersui/dwin_lcd.cpp + * @file lcd/dwin/jyersui/dwin_lcd.cpp * @brief DWIN screen control functions ********************************************************************************/ diff --git a/Marlin/src/lcd/e3v2/jyersui/dwin_lcd.h b/Marlin/src/lcd/dwin/jyersui/dwin_lcd.h similarity index 96% rename from Marlin/src/lcd/e3v2/jyersui/dwin_lcd.h rename to Marlin/src/lcd/dwin/jyersui/dwin_lcd.h index a9335a4f23..124062535a 100644 --- a/Marlin/src/lcd/e3v2/jyersui/dwin_lcd.h +++ b/Marlin/src/lcd/dwin/jyersui/dwin_lcd.h @@ -22,7 +22,7 @@ #pragma once /******************************************************************************** - * @file lcd/e3v2/jyersui/dwin_lcd.h + * @file lcd/dwin/jyersui/dwin_lcd.h * @brief DWIN screen control functions ********************************************************************************/ diff --git a/Marlin/src/lcd/e3v2/marlinui/dwin_lcd.cpp b/Marlin/src/lcd/dwin/marlinui/dwin_lcd.cpp similarity index 98% rename from Marlin/src/lcd/e3v2/marlinui/dwin_lcd.cpp rename to Marlin/src/lcd/dwin/marlinui/dwin_lcd.cpp index 13c6de5480..49d660d843 100644 --- a/Marlin/src/lcd/e3v2/marlinui/dwin_lcd.cpp +++ b/Marlin/src/lcd/dwin/marlinui/dwin_lcd.cpp @@ -21,7 +21,7 @@ */ /******************************************************************************** - * @file lcd/e3v2/marlinui/dwin_lcd.cpp + * @file lcd/dwin/marlinui/dwin_lcd.cpp * @brief DWIN screen control functions ********************************************************************************/ diff --git a/Marlin/src/lcd/e3v2/marlinui/dwin_lcd.h b/Marlin/src/lcd/dwin/marlinui/dwin_lcd.h similarity index 98% rename from Marlin/src/lcd/e3v2/marlinui/dwin_lcd.h rename to Marlin/src/lcd/dwin/marlinui/dwin_lcd.h index ef81a7df77..3b308a7020 100644 --- a/Marlin/src/lcd/e3v2/marlinui/dwin_lcd.h +++ b/Marlin/src/lcd/dwin/marlinui/dwin_lcd.h @@ -22,7 +22,7 @@ #pragma once /******************************************************************************** - * @file lcd/e3v2/marlinui/dwin_lcd.h + * @file lcd/dwin/marlinui/dwin_lcd.h * @brief DWIN screen control functions ********************************************************************************/ diff --git a/Marlin/src/lcd/e3v2/marlinui/dwin_string.cpp b/Marlin/src/lcd/dwin/marlinui/dwin_string.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/marlinui/dwin_string.cpp rename to Marlin/src/lcd/dwin/marlinui/dwin_string.cpp diff --git a/Marlin/src/lcd/e3v2/marlinui/dwin_string.h b/Marlin/src/lcd/dwin/marlinui/dwin_string.h similarity index 100% rename from Marlin/src/lcd/e3v2/marlinui/dwin_string.h rename to Marlin/src/lcd/dwin/marlinui/dwin_string.h diff --git a/Marlin/src/lcd/e3v2/marlinui/game.cpp b/Marlin/src/lcd/dwin/marlinui/game.cpp similarity index 99% rename from Marlin/src/lcd/e3v2/marlinui/game.cpp rename to Marlin/src/lcd/dwin/marlinui/game.cpp index eb15bd8108..b77c72664f 100644 --- a/Marlin/src/lcd/e3v2/marlinui/game.cpp +++ b/Marlin/src/lcd/dwin/marlinui/game.cpp @@ -26,7 +26,7 @@ // Enable performance counters (draw call count, frame timing) for debugging //#define GAME_PERFORMANCE_COUNTERS -#include "../../menu/game/types.h" // includes e3v2/marlinui/game.h +#include "../../menu/game/types.h" // includes dwin/marlinui/game.h #include "../../lcdprint.h" #include "lcdprint_dwin.h" #include "marlinui_dwin.h" diff --git a/Marlin/src/lcd/e3v2/marlinui/game.h b/Marlin/src/lcd/dwin/marlinui/game.h similarity index 100% rename from Marlin/src/lcd/e3v2/marlinui/game.h rename to Marlin/src/lcd/dwin/marlinui/game.h diff --git a/Marlin/src/lcd/e3v2/marlinui/lcdprint_dwin.cpp b/Marlin/src/lcd/dwin/marlinui/lcdprint_dwin.cpp similarity index 99% rename from Marlin/src/lcd/e3v2/marlinui/lcdprint_dwin.cpp rename to Marlin/src/lcd/dwin/marlinui/lcdprint_dwin.cpp index d13ec2e779..a1c7ba2094 100644 --- a/Marlin/src/lcd/e3v2/marlinui/lcdprint_dwin.cpp +++ b/Marlin/src/lcd/dwin/marlinui/lcdprint_dwin.cpp @@ -21,7 +21,7 @@ */ /** - * lcd/e3v2/marlinui/lcdprint_dwin.cpp + * lcd/dwin/marlinui/lcdprint_dwin.cpp * * Due to DWIN hardware limitations simplified characters are used */ diff --git a/Marlin/src/lcd/e3v2/marlinui/lcdprint_dwin.h b/Marlin/src/lcd/dwin/marlinui/lcdprint_dwin.h similarity index 100% rename from Marlin/src/lcd/e3v2/marlinui/lcdprint_dwin.h rename to Marlin/src/lcd/dwin/marlinui/lcdprint_dwin.h diff --git a/Marlin/src/lcd/e3v2/marlinui/marlinui_dwin.h b/Marlin/src/lcd/dwin/marlinui/marlinui_dwin.h similarity index 99% rename from Marlin/src/lcd/e3v2/marlinui/marlinui_dwin.h rename to Marlin/src/lcd/dwin/marlinui/marlinui_dwin.h index b8913914dd..eb25ecd362 100644 --- a/Marlin/src/lcd/e3v2/marlinui/marlinui_dwin.h +++ b/Marlin/src/lcd/dwin/marlinui/marlinui_dwin.h @@ -22,7 +22,7 @@ #pragma once /** - * lcd/e3v2/marlinui/lcdprint_dwin.h + * lcd/dwin/marlinui/lcdprint_dwin.h */ #include "../../../inc/MarlinConfigPre.h" diff --git a/Marlin/src/lcd/e3v2/marlinui/ui_common.cpp b/Marlin/src/lcd/dwin/marlinui/ui_common.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/marlinui/ui_common.cpp rename to Marlin/src/lcd/dwin/marlinui/ui_common.cpp diff --git a/Marlin/src/lcd/e3v2/marlinui/ui_status_480x272.cpp b/Marlin/src/lcd/dwin/marlinui/ui_status_480x272.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/marlinui/ui_status_480x272.cpp rename to Marlin/src/lcd/dwin/marlinui/ui_status_480x272.cpp diff --git a/Marlin/src/lcd/e3v2/proui/base64.h b/Marlin/src/lcd/dwin/proui/base64.h similarity index 100% rename from Marlin/src/lcd/e3v2/proui/base64.h rename to Marlin/src/lcd/dwin/proui/base64.h diff --git a/Marlin/src/lcd/e3v2/proui/bedlevel_tools.cpp b/Marlin/src/lcd/dwin/proui/bedlevel_tools.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/proui/bedlevel_tools.cpp rename to Marlin/src/lcd/dwin/proui/bedlevel_tools.cpp diff --git a/Marlin/src/lcd/e3v2/proui/bedlevel_tools.h b/Marlin/src/lcd/dwin/proui/bedlevel_tools.h similarity index 100% rename from Marlin/src/lcd/e3v2/proui/bedlevel_tools.h rename to Marlin/src/lcd/dwin/proui/bedlevel_tools.h diff --git a/Marlin/src/lcd/e3v2/proui/dwin.cpp b/Marlin/src/lcd/dwin/proui/dwin.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/proui/dwin.cpp rename to Marlin/src/lcd/dwin/proui/dwin.cpp diff --git a/Marlin/src/lcd/e3v2/proui/dwin.h b/Marlin/src/lcd/dwin/proui/dwin.h similarity index 100% rename from Marlin/src/lcd/e3v2/proui/dwin.h rename to Marlin/src/lcd/dwin/proui/dwin.h diff --git a/Marlin/src/lcd/e3v2/proui/dwin_defines.h b/Marlin/src/lcd/dwin/proui/dwin_defines.h similarity index 100% rename from Marlin/src/lcd/e3v2/proui/dwin_defines.h rename to Marlin/src/lcd/dwin/proui/dwin_defines.h diff --git a/Marlin/src/lcd/e3v2/proui/dwin_lcd.cpp b/Marlin/src/lcd/dwin/proui/dwin_lcd.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/proui/dwin_lcd.cpp rename to Marlin/src/lcd/dwin/proui/dwin_lcd.cpp diff --git a/Marlin/src/lcd/e3v2/proui/dwin_lcd.h b/Marlin/src/lcd/dwin/proui/dwin_lcd.h similarity index 100% rename from Marlin/src/lcd/e3v2/proui/dwin_lcd.h rename to Marlin/src/lcd/dwin/proui/dwin_lcd.h diff --git a/Marlin/src/lcd/e3v2/proui/dwin_popup.cpp b/Marlin/src/lcd/dwin/proui/dwin_popup.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/proui/dwin_popup.cpp rename to Marlin/src/lcd/dwin/proui/dwin_popup.cpp diff --git a/Marlin/src/lcd/e3v2/proui/dwin_popup.h b/Marlin/src/lcd/dwin/proui/dwin_popup.h similarity index 100% rename from Marlin/src/lcd/e3v2/proui/dwin_popup.h rename to Marlin/src/lcd/dwin/proui/dwin_popup.h diff --git a/Marlin/src/lcd/e3v2/proui/dwinui.cpp b/Marlin/src/lcd/dwin/proui/dwinui.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/proui/dwinui.cpp rename to Marlin/src/lcd/dwin/proui/dwinui.cpp diff --git a/Marlin/src/lcd/e3v2/proui/dwinui.h b/Marlin/src/lcd/dwin/proui/dwinui.h similarity index 100% rename from Marlin/src/lcd/e3v2/proui/dwinui.h rename to Marlin/src/lcd/dwin/proui/dwinui.h diff --git a/Marlin/src/lcd/e3v2/proui/endstop_diag.cpp b/Marlin/src/lcd/dwin/proui/endstop_diag.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/proui/endstop_diag.cpp rename to Marlin/src/lcd/dwin/proui/endstop_diag.cpp diff --git a/Marlin/src/lcd/e3v2/proui/endstop_diag.h b/Marlin/src/lcd/dwin/proui/endstop_diag.h similarity index 100% rename from Marlin/src/lcd/e3v2/proui/endstop_diag.h rename to Marlin/src/lcd/dwin/proui/endstop_diag.h diff --git a/Marlin/src/lcd/e3v2/proui/gcode_preview.cpp b/Marlin/src/lcd/dwin/proui/gcode_preview.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/proui/gcode_preview.cpp rename to Marlin/src/lcd/dwin/proui/gcode_preview.cpp diff --git a/Marlin/src/lcd/e3v2/proui/gcode_preview.h b/Marlin/src/lcd/dwin/proui/gcode_preview.h similarity index 100% rename from Marlin/src/lcd/e3v2/proui/gcode_preview.h rename to Marlin/src/lcd/dwin/proui/gcode_preview.h diff --git a/Marlin/src/lcd/e3v2/proui/lockscreen.cpp b/Marlin/src/lcd/dwin/proui/lockscreen.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/proui/lockscreen.cpp rename to Marlin/src/lcd/dwin/proui/lockscreen.cpp diff --git a/Marlin/src/lcd/e3v2/proui/lockscreen.h b/Marlin/src/lcd/dwin/proui/lockscreen.h similarity index 100% rename from Marlin/src/lcd/e3v2/proui/lockscreen.h rename to Marlin/src/lcd/dwin/proui/lockscreen.h diff --git a/Marlin/src/lcd/e3v2/proui/menus.cpp b/Marlin/src/lcd/dwin/proui/menus.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/proui/menus.cpp rename to Marlin/src/lcd/dwin/proui/menus.cpp diff --git a/Marlin/src/lcd/e3v2/proui/menus.h b/Marlin/src/lcd/dwin/proui/menus.h similarity index 100% rename from Marlin/src/lcd/e3v2/proui/menus.h rename to Marlin/src/lcd/dwin/proui/menus.h diff --git a/Marlin/src/lcd/e3v2/proui/meshviewer.cpp b/Marlin/src/lcd/dwin/proui/meshviewer.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/proui/meshviewer.cpp rename to Marlin/src/lcd/dwin/proui/meshviewer.cpp diff --git a/Marlin/src/lcd/e3v2/proui/meshviewer.h b/Marlin/src/lcd/dwin/proui/meshviewer.h similarity index 100% rename from Marlin/src/lcd/e3v2/proui/meshviewer.h rename to Marlin/src/lcd/dwin/proui/meshviewer.h diff --git a/Marlin/src/lcd/e3v2/proui/plot.cpp b/Marlin/src/lcd/dwin/proui/plot.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/proui/plot.cpp rename to Marlin/src/lcd/dwin/proui/plot.cpp diff --git a/Marlin/src/lcd/e3v2/proui/plot.h b/Marlin/src/lcd/dwin/proui/plot.h similarity index 100% rename from Marlin/src/lcd/e3v2/proui/plot.h rename to Marlin/src/lcd/dwin/proui/plot.h diff --git a/Marlin/src/lcd/e3v2/proui/printstats.cpp b/Marlin/src/lcd/dwin/proui/printstats.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/proui/printstats.cpp rename to Marlin/src/lcd/dwin/proui/printstats.cpp diff --git a/Marlin/src/lcd/e3v2/proui/printstats.h b/Marlin/src/lcd/dwin/proui/printstats.h similarity index 100% rename from Marlin/src/lcd/e3v2/proui/printstats.h rename to Marlin/src/lcd/dwin/proui/printstats.h diff --git a/Marlin/src/lcd/e3v2/proui/proui_extui.cpp b/Marlin/src/lcd/dwin/proui/proui_extui.cpp similarity index 100% rename from Marlin/src/lcd/e3v2/proui/proui_extui.cpp rename to Marlin/src/lcd/dwin/proui/proui_extui.cpp diff --git a/Marlin/src/lcd/lcdprint.h b/Marlin/src/lcd/lcdprint.h index ef50fb823e..50f1074655 100644 --- a/Marlin/src/lcd/lcdprint.h +++ b/Marlin/src/lcd/lcdprint.h @@ -36,7 +36,7 @@ #if IS_DWIN_MARLINUI - #include "e3v2/marlinui/marlinui_dwin.h" + #include "dwin/marlinui/marlinui_dwin.h" // The DWIN lcd_moveto function uses row / column, not pixels #define LCD_COL_X(col) (col) diff --git a/Marlin/src/lcd/marlinui.cpp b/Marlin/src/lcd/marlinui.cpp index b9f7749c97..b22023a056 100644 --- a/Marlin/src/lcd/marlinui.cpp +++ b/Marlin/src/lcd/marlinui.cpp @@ -46,9 +46,9 @@ MarlinUI ui; #endif #if ENABLED(DWIN_CREALITY_LCD) - #include "e3v2/creality/dwin.h" + #include "dwin/creality/dwin.h" #elif ENABLED(DWIN_CREALITY_LCD_JYERSUI) - #include "e3v2/jyersui/dwin.h" + #include "dwin/jyersui/dwin.h" #elif ENABLED(SOVOL_SV06_RTS) #include "sovol_rts/sovol_rts.h" #endif diff --git a/Marlin/src/lcd/marlinui.h b/Marlin/src/lcd/marlinui.h index 94a333629a..a3a3ae28f0 100644 --- a/Marlin/src/lcd/marlinui.h +++ b/Marlin/src/lcd/marlinui.h @@ -48,13 +48,13 @@ #endif #if ENABLED(DWIN_CREALITY_LCD) - #include "e3v2/creality/dwin.h" + #include "dwin/creality/dwin.h" #elif ENABLED(DWIN_LCD_PROUI) - #include "e3v2/proui/dwin.h" + #include "dwin/proui/dwin.h" #endif #if ALL(HAS_STATUS_MESSAGE, IS_DWIN_MARLINUI) - #include "e3v2/marlinui/marlinui_dwin.h" // for LCD_WIDTH + #include "dwin/marlinui/marlinui_dwin.h" // for LCD_WIDTH #endif typedef bool (*statusResetFunc_t)(); diff --git a/Marlin/src/lcd/menu/game/types.h b/Marlin/src/lcd/menu/game/types.h index a12d134d38..c19c60dd69 100644 --- a/Marlin/src/lcd/menu/game/types.h +++ b/Marlin/src/lcd/menu/game/types.h @@ -27,7 +27,7 @@ #if HAS_MARLINUI_U8GLIB #include "../../dogm/game.h" #elif IS_DWIN_MARLINUI - #include "../../e3v2/marlinui/game.h" + #include "../../dwin/marlinui/game.h" #endif typedef struct { int8_t x, y; } pos_t; diff --git a/Marlin/src/module/motion.h b/Marlin/src/module/motion.h index fbce0166a9..8050e66448 100644 --- a/Marlin/src/module/motion.h +++ b/Marlin/src/module/motion.h @@ -31,7 +31,7 @@ #include "../inc/MarlinConfig.h" #if ALL(DWIN_LCD_PROUI, INDIVIDUAL_AXIS_HOMING_SUBMENU, MESH_BED_LEVELING) - #include "../lcd/e3v2/proui/dwin.h" // for Z_POST_CLEARANCE + #include "../lcd/dwin/proui/dwin.h" // for Z_POST_CLEARANCE #endif #if IS_SCARA diff --git a/Marlin/src/module/settings.cpp b/Marlin/src/module/settings.cpp index e7ce0204c7..e48b215b77 100644 --- a/Marlin/src/module/settings.cpp +++ b/Marlin/src/module/settings.cpp @@ -79,14 +79,14 @@ #endif #if ENABLED(DWIN_LCD_PROUI) - #include "../lcd/e3v2/proui/dwin.h" - #include "../lcd/e3v2/proui/bedlevel_tools.h" + #include "../lcd/dwin/proui/dwin.h" + #include "../lcd/dwin/proui/bedlevel_tools.h" #endif #if ENABLED(EXTENSIBLE_UI) #include "../lcd/extui/ui_api.h" #elif ENABLED(DWIN_CREALITY_LCD_JYERSUI) - #include "../lcd/e3v2/jyersui/dwin.h" + #include "../lcd/dwin/jyersui/dwin.h" #endif #if ENABLED(HOST_PROMPT_SUPPORT) diff --git a/Marlin/src/module/temperature.cpp b/Marlin/src/module/temperature.cpp index 819133f30d..f0ca49ef66 100644 --- a/Marlin/src/module/temperature.cpp +++ b/Marlin/src/module/temperature.cpp @@ -51,7 +51,7 @@ #endif #if ENABLED(DWIN_CREALITY_LCD) - #include "../lcd/e3v2/creality/dwin.h" + #include "../lcd/dwin/creality/dwin.h" #elif ENABLED(SOVOL_SV06_RTS) #include "../lcd/sovol_rts/sovol_rts.h" #endif diff --git a/Marlin/src/sd/cardreader.cpp b/Marlin/src/sd/cardreader.cpp index 1fb12f44a2..98b44e5d14 100644 --- a/Marlin/src/sd/cardreader.cpp +++ b/Marlin/src/sd/cardreader.cpp @@ -36,7 +36,7 @@ #include "../lcd/marlinui.h" #if ENABLED(DWIN_CREALITY_LCD) - #include "../lcd/e3v2/creality/dwin.h" + #include "../lcd/dwin/creality/dwin.h" #elif ENABLED(SOVOL_SV06_RTS) #include "../lcd/sovol_rts/sovol_rts.h" #endif diff --git a/ini/features.ini b/ini/features.ini index d9b0928237..03a9bdbba8 100644 --- a/ini/features.ini +++ b/ini/features.ini @@ -47,11 +47,11 @@ HAS_(FSMC|SPI|LTDC)_TFT = build_src_filter=+ I2C_EEPROM = build_src_filter=+ SOFT_I2C_EEPROM|U8G_USES_SW_I2C = SlowSoftI2CMaster, SlowSoftWire=https://github.com/felias-fogg/SlowSoftWire/archive/f34d777f39.zip SPI_EEPROM = build_src_filter=+ -HAS_DWIN_E3V2|IS_DWIN_MARLINUI = build_src_filter=+ -DWIN_CREALITY_LCD = build_src_filter=+ -DWIN_LCD_PROUI = build_src_filter=+ -DWIN_CREALITY_LCD_JYERSUI = build_src_filter=+ -IS_DWIN_MARLINUI = build_src_filter=+ +HAS_DWIN_E3V2|IS_DWIN_MARLINUI = build_src_filter=+ +DWIN_CREALITY_LCD = build_src_filter=+ +DWIN_LCD_PROUI = build_src_filter=+ +DWIN_CREALITY_LCD_JYERSUI = build_src_filter=+ +IS_DWIN_MARLINUI = build_src_filter=+ SOVOL_SV06_RTS = build_src_filter=+ HAS_GRAPHICAL_TFT = build_src_filter=+ - - HAS_UI_320X.+ = build_src_filter=+ diff --git a/platformio.ini b/platformio.ini index df5418cde4..995c29ca17 100644 --- a/platformio.ini +++ b/platformio.ini @@ -58,7 +58,7 @@ lib_deps = default_src_filter = + - - ; LCDs and Controllers - - - - - - - - - - - + - - - - - - ; Marlin HAL - From 0e5a4cacf8262e503fdc6c2347b7cd0aef9f0f86 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Mon, 8 Dec 2025 21:04:01 -0600 Subject: [PATCH 02/17] =?UTF-8?q?=F0=9F=A7=91=E2=80=8D=F0=9F=92=BB=20Remov?= =?UTF-8?q?e=20unused=20servo=20fields?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/HAL/TEENSY31_32/Servo.h | 2 -- Marlin/src/HAL/TEENSY35_36/Servo.h | 2 -- Marlin/src/HAL/TEENSY40_41/Servo.h | 2 -- 3 files changed, 6 deletions(-) diff --git a/Marlin/src/HAL/TEENSY31_32/Servo.h b/Marlin/src/HAL/TEENSY31_32/Servo.h index 82b601d956..2da74fd25d 100644 --- a/Marlin/src/HAL/TEENSY31_32/Servo.h +++ b/Marlin/src/HAL/TEENSY31_32/Servo.h @@ -31,7 +31,5 @@ class libServo : public Servo { void move(const int value); private: typedef Servo super; - uint16_t min_ticks; - uint16_t max_ticks; uint8_t servoIndex; // index into the channel data for this servo }; diff --git a/Marlin/src/HAL/TEENSY35_36/Servo.h b/Marlin/src/HAL/TEENSY35_36/Servo.h index 719011f102..c37567a813 100644 --- a/Marlin/src/HAL/TEENSY35_36/Servo.h +++ b/Marlin/src/HAL/TEENSY35_36/Servo.h @@ -35,7 +35,5 @@ class libServo : public Servo { void move(const int value); private: typedef Servo super; - uint16_t min_ticks; - uint16_t max_ticks; uint8_t servoIndex; // Index into the channel data for this servo }; diff --git a/Marlin/src/HAL/TEENSY40_41/Servo.h b/Marlin/src/HAL/TEENSY40_41/Servo.h index 699fd700c9..fceb9465ea 100644 --- a/Marlin/src/HAL/TEENSY40_41/Servo.h +++ b/Marlin/src/HAL/TEENSY40_41/Servo.h @@ -37,7 +37,5 @@ class libServo : public PWMServo { private: typedef PWMServo super; uint8_t servoPin; - uint16_t min_ticks; - uint16_t max_ticks; uint8_t servoIndex; // Index into the channel data for this servo }; From 6b18b1bb8a969766bd7fe91281e28c3e4b6510da Mon Sep 17 00:00:00 2001 From: thinkyhead Date: Tue, 9 Dec 2025 06:11:47 +0000 Subject: [PATCH 03/17] [cron] Bump distribution date (2025-12-09) --- Marlin/Version.h | 2 +- Marlin/src/inc/Version.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Marlin/Version.h b/Marlin/Version.h index ac5a3a0e0d..f23871ff04 100644 --- a/Marlin/Version.h +++ b/Marlin/Version.h @@ -41,7 +41,7 @@ * here we define this default string as the date where the latest release * version was tagged. */ -//#define STRING_DISTRIBUTION_DATE "2025-12-07" +//#define STRING_DISTRIBUTION_DATE "2025-12-09" /** * The protocol for communication to the host. Protocol indicates communication diff --git a/Marlin/src/inc/Version.h b/Marlin/src/inc/Version.h index 2538bc840e..63f0634f0a 100644 --- a/Marlin/src/inc/Version.h +++ b/Marlin/src/inc/Version.h @@ -42,7 +42,7 @@ * version was tagged. */ #ifndef STRING_DISTRIBUTION_DATE - #define STRING_DISTRIBUTION_DATE "2025-12-07" + #define STRING_DISTRIBUTION_DATE "2025-12-09" #endif /** From 9c29c634b7d61e55af109de6cafd0c3899ebafe4 Mon Sep 17 00:00:00 2001 From: Harald Wagener Date: Thu, 11 Dec 2025 01:09:05 +0100 Subject: [PATCH 04/17] =?UTF-8?q?=E2=9C=A8=20NO=5FSTANDARD=5FMOTION=20(#28?= =?UTF-8?q?212)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Scott Lahteine --- Marlin/Configuration_adv.h | 6 +- Marlin/src/HAL/ESP32/i2s.cpp | 51 +- Marlin/src/gcode/feature/ft_motion/M493.cpp | 20 +- Marlin/src/inc/Conditionals-4-adv.h | 64 +- Marlin/src/inc/SanityCheck.h | 19 + Marlin/src/lcd/menu/menu_motion.cpp | 6 +- Marlin/src/module/ft_motion.cpp | 6 +- Marlin/src/module/ft_motion.h | 80 +- Marlin/src/module/ft_motion/shaping.h | 1 + Marlin/src/module/planner.cpp | 297 +-- Marlin/src/module/planner.h | 17 +- Marlin/src/module/stepper.cpp | 2084 ++++++++++--------- Marlin/src/module/stepper.h | 41 +- buildroot/tests/rambo | 3 +- 14 files changed, 1398 insertions(+), 1297 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 1f56d184fc..f217923520 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1160,7 +1160,11 @@ #if ENABLED(FT_MOTION) //#define FTM_IS_DEFAULT_MOTION // Use FT Motion as the factory default? //#define FT_MOTION_MENU // Provide a MarlinUI menu to set M493 and M494 parameters - //#define FTM_HOME_AND_PROBE // Use FT Motion for homing / probing. Disable if FT Motion breaks these functions. + + //#define NO_STANDARD_MOTION // Disable the standard motion system entirely to save Flash and RAM + #if DISABLED(NO_STANDARD_MOTION) + //#define FTM_HOME_AND_PROBE // Use FT Motion for homing / probing. Disable if FT Motion breaks these functions. + #endif //#define FTM_DYNAMIC_FREQ // Enable for linear adjustment of XY shaping frequency according to Z or E #if ENABLED(FTM_DYNAMIC_FREQ) diff --git a/Marlin/src/HAL/ESP32/i2s.cpp b/Marlin/src/HAL/ESP32/i2s.cpp index 6aeeb0e3dc..f7c01730b3 100644 --- a/Marlin/src/HAL/ESP32/i2s.cpp +++ b/Marlin/src/HAL/ESP32/i2s.cpp @@ -156,38 +156,43 @@ void stepperTask(void *parameter) { while (dma.rw_pos < DMA_SAMPLE_COUNT) { - #if ENABLED(FT_MOTION) + if (using_ftMotion) { - if (using_ftMotion) { + #if ENABLED(FT_MOTION) if (!nextMainISR) stepper.ftMotion_stepper(); nextMainISR = 0; - } + #endif - #endif + } + else { - if (!using_ftMotion) { - if (!nextMainISR) { - stepper.pulse_phase_isr(); - nextMainISR = stepper.block_phase_isr(); - } - #if ENABLED(LIN_ADVANCE) - else if (!nextAdvanceISR) { - stepper.advance_isr(); - nextAdvanceISR = stepper.la_interval; + #if HAS_STANDARD_MOTION + + if (!nextMainISR) { + stepper.pulse_phase_isr(); + nextMainISR = stepper.block_phase_isr(); } - #endif - else - i2s_push_sample(); + #if ENABLED(LIN_ADVANCE) + else if (!nextAdvanceISR) { + stepper.advance_isr(); + nextAdvanceISR = stepper.la_interval; + } + #endif + else + i2s_push_sample(); - nextMainISR--; + nextMainISR--; - #if ENABLED(LIN_ADVANCE) - if (nextAdvanceISR == stepper.LA_ADV_NEVER) - nextAdvanceISR = stepper.la_interval; + #if ENABLED(LIN_ADVANCE) + if (nextAdvanceISR == stepper.LA_ADV_NEVER) + nextAdvanceISR = stepper.la_interval; + + if (nextAdvanceISR && nextAdvanceISR != stepper.LA_ADV_NEVER) + nextAdvanceISR--; + #endif + + #endif // HAS_STANDARD_MOTION - if (nextAdvanceISR && nextAdvanceISR != stepper.LA_ADV_NEVER) - nextAdvanceISR--; - #endif } } } diff --git a/Marlin/src/gcode/feature/ft_motion/M493.cpp b/Marlin/src/gcode/feature/ft_motion/M493.cpp index e3f4bea8cb..528102c868 100644 --- a/Marlin/src/gcode/feature/ft_motion/M493.cpp +++ b/Marlin/src/gcode/feature/ft_motion/M493.cpp @@ -151,7 +151,7 @@ void GcodeSuite::M493_report(const bool forReplay/*=true*/) { /** * M493: Set Fixed-time Motion Control parameters * - * S Set Fixed-Time motion mode on or off. + * S Set Fixed-Time motion mode on or off. Ignored with NO_STANDARD_MOTION. * 0: Fixed-Time Motion OFF (Standard Motion) * 1: Fixed-Time Motion ON * @@ -210,15 +210,17 @@ void GcodeSuite::M493() { ft_config_t &c = ftMotion.cfg; - // Parse 'S' mode parameter. - if (parser.seen('S')) { - const bool active = parser.value_bool(); - if (active != c.active) { - stepper.ftMotion_syncPosition(); - c.active = active; - flag.report = true; + #if HAS_STANDARD_MOTION + // Parse 'S' mode parameter. + if (parser.seen('S')) { + const bool active = parser.value_bool(); + if (active != c.active) { + stepper.ftMotion_syncPosition(); + c.active = active; + flag.report = true; + } } - } + #endif #if NUM_AXES_SHAPED > 0 diff --git a/Marlin/src/inc/Conditionals-4-adv.h b/Marlin/src/inc/Conditionals-4-adv.h index 41e2e914f1..f92ba2c079 100644 --- a/Marlin/src/inc/Conditionals-4-adv.h +++ b/Marlin/src/inc/Conditionals-4-adv.h @@ -346,10 +346,46 @@ #define HAS_CLASSIC_E_JERK 1 #endif -// Linear advance uses Jerk since E is an isolated axis -#if ALL(FT_MOTION, HAS_EXTRUDERS) - #define FTM_HAS_LIN_ADVANCE 1 +// Fixed-Time Motion +#if ENABLED(FT_MOTION) + #if HAS_X_AXIS + #define HAS_FTM_SHAPING 1 + #define FTM_SHAPER_X + #endif + #if HAS_Y_AXIS + #define FTM_SHAPER_Y + #endif + #if !HAS_Z_AXIS + #undef FTM_SHAPER_Z + #endif + #if HAS_EXTRUDERS + #define FTM_HAS_LIN_ADVANCE 1 + #else + #undef FTM_SHAPER_E + #endif + #if ENABLED(NO_STANDARD_MOTION) + #define FTM_HOME_AND_PROBE + #undef LIN_ADVANCE + #undef SMOOTH_LIN_ADVANCE + #undef S_CURVE_ACCELERATION + #undef ADAPTIVE_STEP_SMOOTHING + #undef INPUT_SHAPING_X + #undef INPUT_SHAPING_Y + #undef INPUT_SHAPING_E_SYNC + #undef MULTISTEPPING_LIMIT + #define MULTISTEPPING_LIMIT 1 + #endif #endif +#if DISABLED(NO_STANDARD_MOTION) + #define HAS_STANDARD_MOTION 1 +#endif + +// ZV Input shaping +#if ANY(INPUT_SHAPING_X, INPUT_SHAPING_Y, INPUT_SHAPING_Z) + #define HAS_ZV_SHAPING 1 +#endif + +// Linear advance uses Jerk since E is an isolated axis #if ANY(FTM_HAS_LIN_ADVANCE, LIN_ADVANCE) #define HAS_LIN_ADVANCE_K 1 #endif @@ -1521,28 +1557,6 @@ #define CANNOT_EMBED_CONFIGURATION defined(__AVR__) #endif -// Input shaping -#if ANY(INPUT_SHAPING_X, INPUT_SHAPING_Y, INPUT_SHAPING_Z) - #define HAS_ZV_SHAPING 1 -#endif - -// FT Motion: Shapers -#if ENABLED(FT_MOTION) - #if HAS_X_AXIS - #define HAS_FTM_SHAPING 1 - #define FTM_SHAPER_X - #endif - #if HAS_Y_AXIS - #define FTM_SHAPER_Y - #endif - #if !HAS_Z_AXIS - #undef FTM_SHAPER_Z - #endif - #if !HAS_EXTRUDERS - #undef FTM_SHAPER_E - #endif -#endif - // Multi-Stepping Limit #ifndef MULTISTEPPING_LIMIT #define MULTISTEPPING_LIMIT 128 diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index 868b0e9fb8..88d177cde3 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -4500,6 +4500,25 @@ static_assert(_PLUS_TEST(3), "DEFAULT_MAX_ACCELERATION values must be positive." #if ENABLED(FTM_RESONANCE_TEST) && DISABLED(EMERGENCY_PARSER) #error "EMERGENCY_PARSER is required with FTM_RESONANCE_TEST (to cancel the test)." #endif + #if !HAS_STANDARD_MOTION + #if ENABLED(NONLINEAR_EXTRUSION) + #error "NONLINEAR_EXTRUSION is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it." + #elif ENABLED(SMOOTH_LIN_ADVANCE) + #error "SMOOTH_LIN_ADVANCE is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it." + #elif ENABLED(MIXING_EXTRUDER) + #error "MIXING_EXTRUDER is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it." + #elif ENABLED(FREEZE_FEATURE) + #error "FREEZE_FEATURE is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it." + #elif ENABLED(DIRECT_STEPPING) + #error "DIRECT_STEPPING is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it." + #elif ENABLED(DIFFERENTIAL_EXTRUDER) + #error "DIFFERENTIAL_EXTRUDER is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it." + #elif ENABLED(LASER_FEATURE) + #error "LASER_FEATURE is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it." + #elif ENABLED(Z_LATE_ENABLE) + #error "Z_LATE_ENABLE is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it." + #endif + #endif #endif // Multi-Stepping Limit diff --git a/Marlin/src/lcd/menu/menu_motion.cpp b/Marlin/src/lcd/menu/menu_motion.cpp index 8f644361f4..bbd6b5beff 100644 --- a/Marlin/src/lcd/menu/menu_motion.cpp +++ b/Marlin/src/lcd/menu/menu_motion.cpp @@ -499,8 +499,10 @@ void menu_move() { START_MENU(); BACK_ITEM(MSG_MOTION); - bool show_state = c.active; - EDIT_ITEM(bool, MSG_FIXED_TIME_MOTION, &show_state, []{ (void)ftMotion.toggle(); }); + #if HAS_STANDARD_MOTION + bool show_state = c.active; + EDIT_ITEM(bool, MSG_FIXED_TIME_MOTION, &show_state, []{ (void)ftMotion.toggle(); }); + #endif // Show only when FT Motion is active (or optionally always show) if (c.active || ENABLED(FT_MOTION_NO_MENU_TOGGLE)) { diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index a2e0b8be70..d8cc2e5f45 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -375,12 +375,8 @@ bool FTMotion::plan_next_block() { const xyze_pos_t& moveDist = current_block->dist_mm; ratio = moveDist / totalLength; - const float mmps = totalLength / current_block->step_event_count, // (mm/step) Distance for each step - initial_speed = mmps * current_block->initial_rate, // (mm/s) Start feedrate - final_speed = mmps * current_block->final_rate; // (mm/s) End feedrate - // Plan the trajectory using the trajectory generator - currentGenerator->plan(initial_speed, final_speed, current_block->acceleration, current_block->nominal_speed, totalLength); + currentGenerator->plan(current_block->entry_speed, current_block->exit_speed, current_block->acceleration, current_block->nominal_speed, totalLength); endPos_prevBlock += moveDist; diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index a149057547..991d3efcef 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -58,7 +58,11 @@ * FTConfig - The active configured state of FT Motion */ typedef struct FTConfig { - bool active = ENABLED(FTM_IS_DEFAULT_MOTION); // Active (else standard motion) + #if HAS_STANDARD_MOTION + bool active = ENABLED(FTM_IS_DEFAULT_MOTION); // Active (else Standard Motion) + #else + static constexpr bool active = true; // Always active with NO_STANDARD_MOTION + #endif bool axis_sync_enabled = true; // Axis synchronization enabled #if HAS_FTM_SHAPING @@ -104,6 +108,33 @@ typedef struct FTConfig { constexpr bool goodBaseFreq(const float f) { return WITHIN(f, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2); } + void set_defaults() { + #if HAS_STANDARD_MOTION + active = ENABLED(FTM_IS_DEFAULT_MOTION); + #endif + + #if HAS_FTM_SHAPING + + #define _SET_CFG_DEFAULTS(A) do{ \ + shaper.A = FTM_DEFAULT_SHAPER_##A; \ + baseFreq.A = FTM_SHAPING_DEFAULT_FREQ_##A; \ + zeta.A = FTM_SHAPING_ZETA_##A; \ + vtol.A = FTM_SHAPING_V_TOL_##A; \ + }while(0); + + SHAPED_MAP(_SET_CFG_DEFAULTS); + #undef _SET_CFG_DEFAULTS + + #if HAS_DYNAMIC_FREQ + dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; + dynFreqK.reset(); + #endif + + #endif // HAS_FTM_SHAPING + + TERN_(FTM_POLYS, poly6_acceleration_overshoot = FTM_POLY6_ACCELERATION_OVERSHOOT); + } + } ft_config_t; /** @@ -122,31 +153,9 @@ class FTMotion { static bool busy; static void set_defaults() { - cfg.active = ENABLED(FTM_IS_DEFAULT_MOTION); + cfg.set_defaults(); - #if HAS_FTM_SHAPING - - #define _SET_CFG_DEFAULTS(A) do{ \ - cfg.shaper.A = FTM_DEFAULT_SHAPER_##A; \ - cfg.baseFreq.A = FTM_SHAPING_DEFAULT_FREQ_##A; \ - cfg.zeta.A = FTM_SHAPING_ZETA_##A; \ - cfg.vtol.A = FTM_SHAPING_V_TOL_##A; \ - }while(0); - - SHAPED_MAP(_SET_CFG_DEFAULTS); - #undef _SET_CFG_DEFAULTS - - #if HAS_DYNAMIC_FREQ - cfg.dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; - //ZERO(cfg.dynFreqK); - #define _DYN_RESET(A) cfg.dynFreqK.A = 0.0f; - SHAPED_MAP(_DYN_RESET); - #undef _DYN_RESET - #endif - - update_shaping_params(); - - #endif // HAS_FTM_SHAPING + TERN_(HAS_FTM_SHAPING, update_shaping_params()); #if ENABLED(FTM_SMOOTHING) #define _SET_SMOOTH(A) set_smoothing_time(_AXIS(A), FTM_SMOOTHING_TIME_##A); @@ -154,10 +163,7 @@ class FTMotion { #undef _SET_SMOOTH #endif - #if ENABLED(FTM_POLYS) - cfg.poly6_acceleration_overshoot = FTM_POLY6_ACCELERATION_OVERSHOOT; - setTrajectoryType(TrajectoryType::FTM_TRAJECTORY_TYPE); - #endif + TERN_(FTM_POLYS, setTrajectoryType(TrajectoryType::FTM_TRAJECTORY_TYPE)); reset(); } @@ -188,12 +194,14 @@ class FTMotion { static void reset(); // Reset all states of the fixed time conversion to defaults. // Safely toggle the active state of FT Motion - static bool toggle() { - stepper.ftMotion_syncPosition(); - FLIP(cfg.active); - update_shaping_params(); - return cfg.active; - } + #if ALL(FT_MOTION, HAS_STANDARD_MOTION) + static bool toggle() { + stepper.ftMotion_syncPosition(); + FLIP(cfg.active); + update_shaping_params(); + return cfg.active; + } + #endif // Trajectory generator selection static void setTrajectoryType(const TrajectoryType type); @@ -201,7 +209,7 @@ class FTMotion { static FSTR_P getTrajectoryName(); FORCE_INLINE static bool axis_is_moving(const AxisEnum axis) { - return cfg.active ? moving_axis_flags[axis] : stepper.axis_is_moving(axis); + return cfg.active ? moving_axis_flags[axis] : TERN0(HAS_STANDARD_MOTION, stepper.axis_is_moving(axis)); } FORCE_INLINE static bool motor_direction(const AxisEnum axis) { return cfg.active ? axis_move_dir[axis] : stepper.last_direction_bits[axis]; diff --git a/Marlin/src/module/ft_motion/shaping.h b/Marlin/src/module/ft_motion/shaping.h index 5b37c2930d..dacd0454fb 100644 --- a/Marlin/src/module/ft_motion/shaping.h +++ b/Marlin/src/module/ft_motion/shaping.h @@ -73,6 +73,7 @@ struct FTShapedAxes { T& operator[](const int axis) { return val[axis_to_index(axis)]; } + void reset() { ZERO(val); } private: static constexpr int axis_to_index(const int axis) { diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 62db79a907..f39a358457 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -797,141 +797,150 @@ block_t* Planner::get_future_block(const uint8_t offset) { */ void Planner::calculate_trapezoid_for_block(block_t * const block, const float entry_speed, const float exit_speed) { - const float spmm = block->steps_per_mm; - uint32_t initial_rate = entry_speed ? LROUND(entry_speed * spmm) : block->initial_rate, - final_rate = LROUND(exit_speed * spmm); - - NOLESS(initial_rate, stepper.minimal_step_rate); - NOLESS(final_rate, stepper.minimal_step_rate); - NOLESS(block->nominal_rate, stepper.minimal_step_rate); - - #if ANY(S_CURVE_ACCELERATION, LIN_ADVANCE) - // If we have some plateau time, the cruise rate will be the nominal rate - uint32_t cruise_rate = block->nominal_rate; + #if ENABLED(FT_MOTION) + block->entry_speed = entry_speed; + block->exit_speed = exit_speed; #endif - // Steps for acceleration, plateau and deceleration - int32_t plateau_steps = block->step_event_count, - accelerate_steps = 0, - decelerate_steps = 0; + #if HAS_STANDARD_MOTION - const int32_t accel = block->acceleration_steps_per_s2; - float inverse_accel = 0.0f; - if (accel != 0) { - inverse_accel = 1.0f / accel; - const float half_inverse_accel = 0.5f * inverse_accel, - nominal_rate_sq = FLOAT_SQ(block->nominal_rate), - // Steps required for acceleration, deceleration to/from nominal rate - decelerate_steps_float = half_inverse_accel * (nominal_rate_sq - FLOAT_SQ(final_rate)), - accelerate_steps_float = half_inverse_accel * (nominal_rate_sq - FLOAT_SQ(initial_rate)); - // Aims to fully reach nominal and final rates - accelerate_steps = CEIL(accelerate_steps_float); - decelerate_steps = CEIL(decelerate_steps_float); + const float spmm = block->steps_per_mm; + uint32_t initial_rate = entry_speed ? LROUND(entry_speed * spmm) : block->initial_rate, + final_rate = LROUND(exit_speed * spmm); - // Steps between acceleration and deceleration, if any - plateau_steps -= accelerate_steps + decelerate_steps; + NOLESS(initial_rate, stepper.minimal_step_rate); + NOLESS(final_rate, stepper.minimal_step_rate); + NOLESS(block->nominal_rate, stepper.minimal_step_rate); - // Does accelerate_steps + decelerate_steps exceed step_event_count? - // Then we can't possibly reach the nominal rate, there will be no cruising. - // Calculate accel / braking time in order to reach the final_rate exactly - // at the end of this block. - if (plateau_steps < 0) { - accelerate_steps = LROUND((block->step_event_count + accelerate_steps_float - decelerate_steps_float) * 0.5f); - LIMIT(accelerate_steps, 0, int32_t(block->step_event_count)); - decelerate_steps = block->step_event_count - accelerate_steps; + #if ANY(S_CURVE_ACCELERATION, LIN_ADVANCE) + // If we have some plateau time, the cruise rate will be the nominal rate + uint32_t cruise_rate = block->nominal_rate; + #endif - #if ANY(S_CURVE_ACCELERATION, LIN_ADVANCE) - // We won't reach the cruising rate. Let's calculate the speed we will reach - NOMORE(cruise_rate, final_speed(initial_rate, accel, accelerate_steps)); - #endif - } - } + // Steps for acceleration, plateau and deceleration + int32_t plateau_steps = block->step_event_count, + accelerate_steps = 0, + decelerate_steps = 0; - #if ANY(S_CURVE_ACCELERATION, SMOOTH_LIN_ADVANCE) - const float rate_factor = inverse_accel * (STEPPER_TIMER_RATE); - // Jerk controlled speed requires to express speed versus time, NOT steps - uint32_t acceleration_time = rate_factor * float(cruise_rate - initial_rate), - deceleration_time = rate_factor * float(cruise_rate - final_rate); - #endif - #if ENABLED(S_CURVE_ACCELERATION) - // And to offload calculations from the ISR, we also calculate the inverse of those times here - uint32_t acceleration_time_inverse = get_period_inverse(acceleration_time), - deceleration_time_inverse = get_period_inverse(deceleration_time); - #endif + const int32_t accel = block->acceleration_steps_per_s2; + float inverse_accel = 0.0f; + if (accel != 0) { + inverse_accel = 1.0f / accel; + const float half_inverse_accel = 0.5f * inverse_accel, + nominal_rate_sq = FLOAT_SQ(block->nominal_rate), + // Steps required for acceleration, deceleration to/from nominal rate + decelerate_steps_float = half_inverse_accel * (nominal_rate_sq - FLOAT_SQ(final_rate)), + accelerate_steps_float = half_inverse_accel * (nominal_rate_sq - FLOAT_SQ(initial_rate)); + // Aims to fully reach nominal and final rates + accelerate_steps = CEIL(accelerate_steps_float); + decelerate_steps = CEIL(decelerate_steps_float); - // Store new block parameters - block->accelerate_before = accelerate_steps; - block->decelerate_start = block->step_event_count - decelerate_steps; - block->initial_rate = initial_rate; - block->final_rate = final_rate; + // Steps between acceleration and deceleration, if any + plateau_steps -= accelerate_steps + decelerate_steps; - #if ANY(S_CURVE_ACCELERATION, SMOOTH_LIN_ADVANCE) - block->acceleration_time = acceleration_time; - block->deceleration_time = deceleration_time; - block->cruise_rate = cruise_rate; - #endif - #if ENABLED(S_CURVE_ACCELERATION) - block->acceleration_time_inverse = acceleration_time_inverse; - block->deceleration_time_inverse = deceleration_time_inverse; - #endif + // Does accelerate_steps + decelerate_steps exceed step_event_count? + // Then we can't possibly reach the nominal rate, there will be no cruising. + // Calculate accel / braking time in order to reach the final_rate exactly + // at the end of this block. + if (plateau_steps < 0) { + accelerate_steps = LROUND((block->step_event_count + accelerate_steps_float - decelerate_steps_float) * 0.5f); + LIMIT(accelerate_steps, 0, int32_t(block->step_event_count)); + decelerate_steps = block->step_event_count - accelerate_steps; - #if ENABLED(SMOOTH_LIN_ADVANCE) - block->cruise_time = plateau_steps > 0 ? float(plateau_steps) * float(STEPPER_TIMER_RATE) / float(cruise_rate) : 0; - #elif HAS_ROUGH_LIN_ADVANCE - if (block->la_advance_rate) { - const float comp = get_advance_k(block->extruder) * block->steps.e / block->step_event_count; - block->max_adv_steps = cruise_rate * comp; - block->final_adv_steps = final_rate * comp; - } - #endif - - #if ENABLED(LASER_POWER_TRAP) - /** - * Laser Trapezoid Calculations - * - * Approximate the trapezoid with the laser, incrementing the power every `trap_ramp_entry_incr` - * steps while accelerating, and decrementing the power every `trap_ramp_exit_decr` while decelerating, - * to keep power proportional to feedrate. Laser power trap will reduce the initial power to no less - * than the laser_power_floor value. Based on the number of calculated accel/decel steps the power is - * distributed over the trapezoid entry- and exit-ramp steps. - * - * trap_ramp_active_pwr - The active power is initially set at a reduced level factor of initial - * power / accel steps and will be additively incremented using a trap_ramp_entry_incr value for each - * accel step processed later in the stepper code. The trap_ramp_exit_decr value is calculated as - * power / decel steps and is also adjusted to no less than the power floor. - * - * If the power == 0 the inline mode variables need to be set to zero to prevent stepper processing. - * The method allows for simpler non-powered moves like G0 or G28. - * - * Laser Trap Power works for all Jerk and Curve modes; however Arc-based moves will have issues since - * the segments are usually too small. - */ - if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS - && planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled - ) { - if (block->laser.power > 0) { - NOLESS(block->laser.power, laser_power_floor); - block->laser.trap_ramp_active_pwr = (block->laser.power - laser_power_floor) * (initial_rate / float(block->nominal_rate)) + laser_power_floor; - block->laser.trap_ramp_entry_incr = (block->laser.power - block->laser.trap_ramp_active_pwr) / accelerate_steps; - float laser_pwr = block->laser.power * (final_rate / float(block->nominal_rate)); - NOLESS(laser_pwr, laser_power_floor); - block->laser.trap_ramp_exit_decr = (block->laser.power - laser_pwr) / decelerate_steps; - #if ENABLED(DEBUG_LASER_TRAP) - SERIAL_ECHO_MSG("lp:", block->laser.power); - SERIAL_ECHO_MSG("as:", accelerate_steps); - SERIAL_ECHO_MSG("ds:", decelerate_steps); - SERIAL_ECHO_MSG("p.trap:", block->laser.trap_ramp_active_pwr); - SERIAL_ECHO_MSG("p.incr:", block->laser.trap_ramp_entry_incr); - SERIAL_ECHO_MSG("p.decr:", block->laser.trap_ramp_exit_decr); + #if ANY(S_CURVE_ACCELERATION, LIN_ADVANCE) + // We won't reach the cruising rate. Let's calculate the speed we will reach + NOMORE(cruise_rate, final_speed(initial_rate, accel, accelerate_steps)); #endif } - else { - block->laser.trap_ramp_active_pwr = 0; - block->laser.trap_ramp_entry_incr = 0; - block->laser.trap_ramp_exit_decr = 0; - } } - #endif // LASER_POWER_TRAP + + #if ANY(S_CURVE_ACCELERATION, SMOOTH_LIN_ADVANCE) + const float rate_factor = inverse_accel * (STEPPER_TIMER_RATE); + // Jerk controlled speed requires to express speed versus time, NOT steps + uint32_t acceleration_time = rate_factor * float(cruise_rate - initial_rate), + deceleration_time = rate_factor * float(cruise_rate - final_rate); + #endif + #if ENABLED(S_CURVE_ACCELERATION) + // And to offload calculations from the ISR, we also calculate the inverse of those times here + uint32_t acceleration_time_inverse = get_period_inverse(acceleration_time), + deceleration_time_inverse = get_period_inverse(deceleration_time); + #endif + + // Store new block parameters + block->accelerate_before = accelerate_steps; + block->decelerate_start = block->step_event_count - decelerate_steps; + block->initial_rate = initial_rate; + block->final_rate = final_rate; + + #if ANY(S_CURVE_ACCELERATION, SMOOTH_LIN_ADVANCE) + block->acceleration_time = acceleration_time; + block->deceleration_time = deceleration_time; + block->cruise_rate = cruise_rate; + #endif + #if ENABLED(S_CURVE_ACCELERATION) + block->acceleration_time_inverse = acceleration_time_inverse; + block->deceleration_time_inverse = deceleration_time_inverse; + #endif + + #if ENABLED(SMOOTH_LIN_ADVANCE) + block->cruise_time = plateau_steps > 0 ? float(plateau_steps) * float(STEPPER_TIMER_RATE) / float(cruise_rate) : 0; + #elif HAS_ROUGH_LIN_ADVANCE + if (block->la_advance_rate) { + const float comp = get_advance_k(block->extruder) * block->steps.e / block->step_event_count; + block->max_adv_steps = cruise_rate * comp; + block->final_adv_steps = final_rate * comp; + } + #endif + + #if ENABLED(LASER_POWER_TRAP) + /** + * Laser Trapezoid Calculations + * + * Approximate the trapezoid with the laser, incrementing the power every `trap_ramp_entry_incr` + * steps while accelerating, and decrementing the power every `trap_ramp_exit_decr` while decelerating, + * to keep power proportional to feedrate. Laser power trap will reduce the initial power to no less + * than the laser_power_floor value. Based on the number of calculated accel/decel steps the power is + * distributed over the trapezoid entry- and exit-ramp steps. + * + * trap_ramp_active_pwr - The active power is initially set at a reduced level factor of initial + * power / accel steps and will be additively incremented using a trap_ramp_entry_incr value for each + * accel step processed later in the stepper code. The trap_ramp_exit_decr value is calculated as + * power / decel steps and is also adjusted to no less than the power floor. + * + * If the power == 0 the inline mode variables need to be set to zero to prevent stepper processing. + * The method allows for simpler non-powered moves like G0 or G28. + * + * Laser Trap Power works for all Jerk and Curve modes; however Arc-based moves will have issues since + * the segments are usually too small. + */ + if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS + && planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled + ) { + if (block->laser.power > 0) { + NOLESS(block->laser.power, laser_power_floor); + block->laser.trap_ramp_active_pwr = (block->laser.power - laser_power_floor) * (initial_rate / float(block->nominal_rate)) + laser_power_floor; + block->laser.trap_ramp_entry_incr = (block->laser.power - block->laser.trap_ramp_active_pwr) / accelerate_steps; + float laser_pwr = block->laser.power * (final_rate / float(block->nominal_rate)); + NOLESS(laser_pwr, laser_power_floor); + block->laser.trap_ramp_exit_decr = (block->laser.power - laser_pwr) / decelerate_steps; + #if ENABLED(DEBUG_LASER_TRAP) + SERIAL_ECHO_MSG("lp:", block->laser.power); + SERIAL_ECHO_MSG("as:", accelerate_steps); + SERIAL_ECHO_MSG("ds:", decelerate_steps); + SERIAL_ECHO_MSG("p.trap:", block->laser.trap_ramp_active_pwr); + SERIAL_ECHO_MSG("p.incr:", block->laser.trap_ramp_entry_incr); + SERIAL_ECHO_MSG("p.decr:", block->laser.trap_ramp_exit_decr); + #endif + } + else { + block->laser.trap_ramp_active_pwr = 0; + block->laser.trap_ramp_entry_incr = 0; + block->laser.trap_ramp_exit_decr = 0; + } + } + #endif // LASER_POWER_TRAP + + #endif // HAS_STANDARD_MOTION } /** @@ -1131,12 +1140,18 @@ void Planner::recalculate_trapezoids(const float safe_exit_speed_sqr) { if (stepper.is_block_busy(block)) { // Block is BUSY so we can't change the exit speed. Revert any reverse pass change. next->entry_speed_sqr = next->min_entry_speed_sqr; - if (!next->initial_rate) { - // 'next' was never calculated. Planner is falling behind so for maximum efficiency - // set next's stepping speed directly and forgo checking against min_entry_speed_sqr. - // calculate_trapezoid_for_block() can handle it, albeit sub-optimally. - next->initial_rate = block->final_rate; - } + + // If 'next' was never calculated the Planner is falling behind, so for maximum efficiency + // set next's stepping speed directly and forego checking against min_entry_speed_sqr. + // calculate_trapezoid_for_block() can handle it, albeit sub-optimally. + + #if HAS_STANDARD_MOTION + if (!next->initial_rate) next->initial_rate = block->final_rate; + #endif + #if ENABLED(FT_MOTION) + if (!next->entry_speed) next->entry_speed = block->exit_speed; + #endif + // Note that at this point next_entry_speed is (still) 0. } else { @@ -2108,7 +2123,7 @@ bool Planner::_populate_block( NUM_AXIS_CODE( if (block->steps.x) stepper.enable_axis(X_AXIS), if (block->steps.y) stepper.enable_axis(Y_AXIS), - if (TERN(Z_LATE_ENABLE, 0, block->steps.z)) stepper.enable_axis(Z_AXIS), + if (TERN(Z_LATE_ENABLE, false, block->steps.z)) stepper.enable_axis(Z_AXIS), if (block->steps.i) stepper.enable_axis(I_AXIS), if (block->steps.j) stepper.enable_axis(J_AXIS), if (block->steps.k) stepper.enable_axis(K_AXIS), @@ -2224,8 +2239,10 @@ bool Planner::_populate_block( if (was_enabled) stepper.wake_up(); #endif - block->nominal_speed = block->millimeters * inverse_secs; // (mm/sec) Always > 0 - block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0 + block->nominal_speed = block->millimeters * inverse_secs; // (mm/sec) Always > 0 + #if HAS_STANDARD_MOTION + block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0 + #endif #if ENABLED(FILAMENT_WIDTH_SENSOR) if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM) // Only for extruder with filament sensor @@ -2317,7 +2334,7 @@ bool Planner::_populate_block( // Correct the speed if (speed_factor < 1.0f) { current_speed *= speed_factor; - block->nominal_rate *= speed_factor; + TERN_(HAS_STANDARD_MOTION, block->nominal_rate *= speed_factor); block->nominal_speed *= speed_factor; } @@ -2409,11 +2426,13 @@ bool Planner::_populate_block( ); } } - block->acceleration_steps_per_s2 = accel; - block->acceleration = accel / steps_per_mm; - #if DISABLED(S_CURVE_ACCELERATION) - block->acceleration_rate = uint32_t(accel * (float(_BV32(24)) / (STEPPER_TIMER_RATE))); + #if HAS_STANDARD_MOTION + block->acceleration_steps_per_s2 = accel; + #if DISABLED(S_CURVE_ACCELERATION) + block->acceleration_rate = uint32_t(accel * (float(_BV32(24)) / (STEPPER_TIMER_RATE))); + #endif #endif + block->acceleration = accel / steps_per_mm; #if HAS_ROUGH_LIN_ADVANCE block->la_advance_rate = 0; @@ -2724,8 +2743,10 @@ bool Planner::_populate_block( block->entry_speed_sqr = minimum_planner_speed_sqr; // Set min entry speed. Rarely it could be higher than the previous nominal speed but that's ok. block->min_entry_speed_sqr = minimum_planner_speed_sqr; - // Zero the initial_rate to indicate that calculate_trapezoid_for_block() hasn't been called yet. - block->initial_rate = 0; + + // Zero initial_rate and/or entry_speed to indicate calculate_trapezoid_for_block() needs a call. + TERN_(HAS_STANDARD_MOTION, block->initial_rate = 0); + TERN_(FT_MOTION, block->entry_speed = 0); block->flag.recalculate = true; diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index 2afec3b4ab..2838fb90c3 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -254,7 +254,7 @@ typedef struct PlannerBlock { #if ENABLED(S_CURVE_ACCELERATION) uint32_t acceleration_time_inverse, // Inverse of acceleration and deceleration periods, expressed as integer. Scale depends on CPU being used deceleration_time_inverse; - #else + #elif HAS_STANDARD_MOTION uint32_t acceleration_rate; // Acceleration rate in (2^24 steps)/timer_ticks*s #endif @@ -277,10 +277,17 @@ typedef struct PlannerBlock { #endif #endif - uint32_t nominal_rate, // The nominal step rate for this block in step_events/sec - initial_rate, // The jerk-adjusted step rate at start of block - final_rate, // The minimal rate at exit - acceleration_steps_per_s2; // acceleration steps/sec^2 + #if ENABLED(FT_MOTION) + float entry_speed, // Block entry speed in steps units + exit_speed; // Block exit speed in steps units + #endif + + #if HAS_STANDARD_MOTION + uint32_t nominal_rate, // The nominal step rate for this block in step_events/sec + initial_rate, // The jerk-adjusted step rate at start of block + final_rate, // The minimal rate at exit + acceleration_steps_per_s2; // acceleration steps/sec^2 + #endif #if ENABLED(DIRECT_STEPPING) page_idx_t page_idx; // Page index used for direct stepping diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index d79a5edcc0..947a74f96a 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -138,8 +138,10 @@ stepper_flags_t Stepper::axis_enabled; // {0} block_t* Stepper::current_block; // (= nullptr) A pointer to the block currently being traced -AxisBits Stepper::last_direction_bits, // = 0 - Stepper::axis_did_move; // = 0 +#if HAS_STANDARD_MOTION + AxisBits Stepper::axis_did_move; // = 0 +#endif +AxisBits Stepper::last_direction_bits; // = 0 bool Stepper::abort_current_block; @@ -516,6 +518,9 @@ xyze_int8_t Stepper::count_direction{0}; #define W_APPLY_STEP(STATE,Q) W_STEP_WRITE(STATE) #endif +#define _APPLY_STEP(AXIS, STATE, ALWAYS) AXIS ##_APPLY_STEP(STATE, ALWAYS) +#define _STEP_STATE(AXIS) STEP_STATE_## AXIS + //#define E0_APPLY_DIR(FWD) do{ (FWD) ? FWD_E_DIR(0) : REV_E_DIR(0); }while(0) //#define E1_APPLY_DIR(FWD) do{ (FWD) ? FWD_E_DIR(1) : REV_E_DIR(1); }while(0) //#define E2_APPLY_DIR(FWD) do{ (FWD) ? FWD_E_DIR(2) : REV_E_DIR(2); }while(0) @@ -1583,11 +1588,11 @@ void Stepper::isr() { #if ENABLED(FT_MOTION) static uint32_t ftMotion_nextStepperISR = 0U; // Storage for the next ISR for stepping. - const bool using_ftMotion = ftMotion.cfg.active; - #else - constexpr bool using_ftMotion = false; #endif + // FT Motion can be toggled if Standard Motion is also active + const bool using_ftMotion = ENABLED(NO_STANDARD_MOTION) || TERN0(FT_MOTION, ftMotion.cfg.active); + // We need this variable here to be able to use it in the following loop hal_timer_t min_ticks; do { @@ -1628,72 +1633,76 @@ void Stepper::isr() { ftMotion_nextStepperISR -= interval; } - #endif + #endif // FT_MOTION - if (!using_ftMotion) { + #if HAS_STANDARD_MOTION - TERN_(HAS_ZV_SHAPING, shaping_isr()); // Do Shaper stepping, if needed + if (!using_ftMotion) { - if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses + TERN_(HAS_ZV_SHAPING, shaping_isr()); // Do Shaper stepping, if needed - #if ENABLED(LIN_ADVANCE) - if (!nextAdvanceISR) { // 0 = Do Linear Advance E Stepper pulses - advance_isr(); - nextAdvanceISR = la_interval; - } - else if (nextAdvanceISR > la_interval) // Start/accelerate LA steps if necessary - nextAdvanceISR = la_interval; - #endif + if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses - #if ENABLED(BABYSTEPPING) - // Time to run babystepping and apply STEP/DIR pulses? - // babystepping_isr -> babystep.task -> [ babystep.step_axis(*) -> stepper.do_babystep ] - const bool is_babystep = (nextBabystepISR == 0); // 0 = Do Babystepping (XY)Z pulses - if (is_babystep) nextBabystepISR = babystepping_isr(); - #endif + #if ENABLED(LIN_ADVANCE) + if (!nextAdvanceISR) { // 0 = Do Linear Advance E Stepper pulses + advance_isr(); + nextAdvanceISR = la_interval; + } + else if (nextAdvanceISR > la_interval) // Start/accelerate LA steps if necessary + nextAdvanceISR = la_interval; + #endif - // Enable ISRs to reduce latency for higher priority ISRs, or all ISRs if no prioritization. - hal.isr_on(); + #if ENABLED(BABYSTEPPING) + // Time to run babystepping and apply STEP/DIR pulses? + // babystepping_isr -> babystep.task -> [ babystep.step_axis(*) -> stepper.do_babystep ] + const bool is_babystep = (nextBabystepISR == 0); // 0 = Do Babystepping (XY)Z pulses + if (is_babystep) nextBabystepISR = babystepping_isr(); + #endif - // ^== Time critical. NOTHING besides pulse generation should be above here!!! + // Enable ISRs to reduce latency for higher priority ISRs, or all ISRs if no prioritization. + hal.isr_on(); - if (!nextMainISR) nextMainISR = block_phase_isr(); // Manage acc/deceleration, get next block - #if ENABLED(SMOOTH_LIN_ADVANCE) - if (!smoothLinAdvISR) smoothLinAdvISR = smooth_lin_adv_isr(); // Manage la - #endif + // ^== Time critical. NOTHING besides pulse generation should be above here!!! - #if ENABLED(BABYSTEPPING) - if (is_babystep) // Avoid ANY stepping too soon after baby-stepping - NOLESS(nextMainISR, (BABYSTEP_TICKS) / 8); // FULL STOP for 125µs after a baby-step + if (!nextMainISR) nextMainISR = block_phase_isr(); // Manage acc/deceleration, get next block + #if ENABLED(SMOOTH_LIN_ADVANCE) + if (!smoothLinAdvISR) smoothLinAdvISR = smooth_lin_adv_isr(); // Manage la + #endif - if (nextBabystepISR != BABYSTEP_NEVER) // Avoid baby-stepping too close to axis Stepping - NOLESS(nextBabystepISR, nextMainISR / 2); // TODO: Only look at axes enabled for baby-stepping - #endif + #if ENABLED(BABYSTEPPING) + if (is_babystep) // Avoid ANY stepping too soon after baby-stepping + NOLESS(nextMainISR, (BABYSTEP_TICKS) / 8); // FULL STOP for 125µs after a baby-step - // Get the interval to the next ISR call - interval = hal_timer_t(STEPPER_TIMER_RATE * 0.03); // Max wait of 30ms regardless of stepper timer frequency - NOMORE(interval, nextMainISR); // Time until the next Pulse / Block phase - TERN_(INPUT_SHAPING_X, NOMORE(interval, ShapingQueue::peek_x())); // Time until next input shaping echo for X - TERN_(INPUT_SHAPING_Y, NOMORE(interval, ShapingQueue::peek_y())); // Time until next input shaping echo for Y - TERN_(INPUT_SHAPING_Z, NOMORE(interval, ShapingQueue::peek_z())); // Time until next input shaping echo for Z - TERN_(LIN_ADVANCE, NOMORE(interval, nextAdvanceISR)); // Come back early for Linear Advance? - TERN_(SMOOTH_LIN_ADVANCE, NOMORE(interval, smoothLinAdvISR)); // Come back early for Linear Advance rate update? - TERN_(BABYSTEPPING, NOMORE(interval, nextBabystepISR)); // Come back early for Babystepping? + if (nextBabystepISR != BABYSTEP_NEVER) // Avoid baby-stepping too close to axis Stepping + NOLESS(nextBabystepISR, nextMainISR / 2); // TODO: Only look at axes enabled for baby-stepping + #endif - // - // Compute remaining time for each ISR phase - // NEVER : The phase is idle - // Zero : The phase will occur on the next ISR call - // Non-zero : The phase will occur on a future ISR call - // + // Get the interval to the next ISR call + interval = hal_timer_t(STEPPER_TIMER_RATE * 0.03); // Max wait of 30ms regardless of stepper timer frequency + NOMORE(interval, nextMainISR); // Time until the next Pulse / Block phase + TERN_(INPUT_SHAPING_X, NOMORE(interval, ShapingQueue::peek_x())); // Time until next input shaping echo for X + TERN_(INPUT_SHAPING_Y, NOMORE(interval, ShapingQueue::peek_y())); // Time until next input shaping echo for Y + TERN_(INPUT_SHAPING_Z, NOMORE(interval, ShapingQueue::peek_z())); // Time until next input shaping echo for Z + TERN_(LIN_ADVANCE, NOMORE(interval, nextAdvanceISR)); // Come back early for Linear Advance? + TERN_(SMOOTH_LIN_ADVANCE, NOMORE(interval, smoothLinAdvISR)); // Come back early for Linear Advance rate update? + TERN_(BABYSTEPPING, NOMORE(interval, nextBabystepISR)); // Come back early for Babystepping? - nextMainISR -= interval; - TERN_(HAS_ZV_SHAPING, ShapingQueue::decrement_delays(interval)); - TERN_(LIN_ADVANCE, if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval); - TERN_(SMOOTH_LIN_ADVANCE, if (smoothLinAdvISR != LA_ADV_NEVER) smoothLinAdvISR -= interval); - TERN_(BABYSTEPPING, if (nextBabystepISR != BABYSTEP_NEVER) nextBabystepISR -= interval); + // + // Compute remaining time for each ISR phase + // NEVER : The phase is idle + // Zero : The phase will occur on the next ISR call + // Non-zero : The phase will occur on a future ISR call + // - } // standard motion control + nextMainISR -= interval; + TERN_(HAS_ZV_SHAPING, ShapingQueue::decrement_delays(interval)); + TERN_(LIN_ADVANCE, if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval); + TERN_(SMOOTH_LIN_ADVANCE, if (smoothLinAdvISR != LA_ADV_NEVER) smoothLinAdvISR -= interval); + TERN_(BABYSTEPPING, if (nextBabystepISR != BABYSTEP_NEVER) nextBabystepISR -= interval); + + } + + #endif // HAS_STANDARD_MOTION /** * This needs to avoid a race-condition caused by interleaving @@ -1798,335 +1807,335 @@ void Stepper::isr() { #define ISR_MULTI_STEPS 1 #endif -/** - * This phase of the ISR should ONLY create the pulses for the steppers. - * This prevents jitter caused by the interval between the start of the - * interrupt and the start of the pulses. DON'T add any logic ahead of the - * call to this method that might cause variation in the timing. The aim - * is to keep pulse timing as regular as possible. - */ -void Stepper::pulse_phase_isr() { +#if HAS_STANDARD_MOTION + /** + * This phase of the ISR should ONLY create the pulses for the steppers. + * This prevents jitter caused by the interval between the start of the + * interrupt and the start of the pulses. DON'T add any logic ahead of the + * call to this method that might cause variation in the timing. The aim + * is to keep pulse timing as regular as possible. + */ + void Stepper::pulse_phase_isr() { - // If we must abort the current block, do so! - if (abort_current_block) { - abort_current_block = false; - if (current_block) { - discard_current_block(); - #if HAS_ZV_SHAPING - ShapingQueue::purge(); - #if ENABLED(INPUT_SHAPING_X) - shaping_x.delta_error = 0; - shaping_x.last_block_end_pos = count_position.x; + // If we must abort the current block, do so! + if (abort_current_block) { + abort_current_block = false; + if (current_block) { + discard_current_block(); + #if HAS_ZV_SHAPING + ShapingQueue::purge(); + #if ENABLED(INPUT_SHAPING_X) + shaping_x.delta_error = 0; + shaping_x.last_block_end_pos = count_position.x; + #endif + #if ENABLED(INPUT_SHAPING_Y) + shaping_y.delta_error = 0; + shaping_y.last_block_end_pos = count_position.y; + #endif + #if ENABLED(INPUT_SHAPING_Z) + shaping_z.delta_error = 0; + shaping_z.last_block_end_pos = count_position.z; + #endif #endif - #if ENABLED(INPUT_SHAPING_Y) - shaping_y.delta_error = 0; - shaping_y.last_block_end_pos = count_position.y; - #endif - #if ENABLED(INPUT_SHAPING_Z) - shaping_z.delta_error = 0; - shaping_z.last_block_end_pos = count_position.z; - #endif - #endif + } } + + // If there is no current block, do nothing + if (!current_block || step_events_completed >= step_event_count) return; + + // Skipping step processing causes motion to freeze + if (TERN0(FREEZE_FEATURE, frozen)) return; + + // Count of pending loops and events for this iteration + const uint32_t pending_events = step_event_count - step_events_completed; + uint8_t events_to_do = _MIN(pending_events, steps_per_isr); + + // Just update the value we will get at the end of the loop + step_events_completed += events_to_do; + + TERN_(ISR_PULSE_CONTROL, USING_TIMED_PULSE()); + + // Take multiple steps per interrupt. For high speed moves. + #if ENABLED(ISR_MULTI_STEPS) + bool firstStep = true; + #endif + + // Direct Stepping page? + const bool is_page = current_block->is_page(); + + do { + AxisFlags step_needed{0}; + + // Determine if a pulse is needed using Bresenham + #define PULSE_PREP(AXIS) do{ \ + int32_t de = delta_error[_AXIS(AXIS)] + advance_dividend[_AXIS(AXIS)]; \ + if (de >= 0) { \ + step_needed.set(_AXIS(AXIS)); \ + de -= advance_divisor_cached; \ + } \ + delta_error[_AXIS(AXIS)] = de; \ + }while(0) + + // With input shaping, direction changes can happen with almost only + // AWAIT_LOW_PULSE() and DIR_WAIT_BEFORE() between steps. To work around + // the TMC2208 / TMC2225 shutdown bug (#16076), add a half step hysteresis + // in each direction. This results in the position being off by half an + // average half step during travel but correct at the end of each segment. + #if AXIS_DRIVER_TYPE_X(TMC2208) || AXIS_DRIVER_TYPE_X(TMC2208_STANDALONE) || \ + AXIS_DRIVER_TYPE_X(TMC5160) || AXIS_DRIVER_TYPE_X(TMC5160_STANDALONE) + #define HYSTERESIS_X 64 + #else + #define HYSTERESIS_X 0 + #endif + #if AXIS_DRIVER_TYPE_Y(TMC2208) || AXIS_DRIVER_TYPE_Y(TMC2208_STANDALONE) || \ + AXIS_DRIVER_TYPE_Y(TMC5160) || AXIS_DRIVER_TYPE_Y(TMC5160_STANDALONE) + #define HYSTERESIS_Y 64 + #else + #define HYSTERESIS_Y 0 + #endif + #if AXIS_DRIVER_TYPE_Z(TMC2208) || AXIS_DRIVER_TYPE_Z(TMC2208_STANDALONE) || \ + AXIS_DRIVER_TYPE_Z(TMC5160) || AXIS_DRIVER_TYPE_Z(TMC5160_STANDALONE) + #define HYSTERESIS_Z 64 + #else + #define HYSTERESIS_Z 0 + #endif + #define _HYSTERESIS(AXIS) HYSTERESIS_##AXIS + #define HYSTERESIS(AXIS) _HYSTERESIS(AXIS) + + #define PULSE_PREP_SHAPING(AXIS, DELTA_ERROR, DIVIDEND) do{ \ + int16_t de = DELTA_ERROR + (DIVIDEND); \ + const bool step_fwd = de >= (64 + HYSTERESIS(AXIS)), \ + step_bak = de <= -(64 + HYSTERESIS(AXIS)); \ + if (step_fwd || step_bak) { \ + de += step_fwd ? -128 : 128; \ + if ((MAXDIR(AXIS) && step_bak) || (MINDIR(AXIS) && step_fwd)) { \ + { USING_TIMED_PULSE(); START_TIMED_PULSE(); AWAIT_LOW_PULSE(); } \ + last_direction_bits.toggle(_AXIS(AXIS)); \ + DIR_WAIT_BEFORE(); \ + SET_STEP_DIR(AXIS); \ + TERN_(FT_MOTION, last_set_direction = last_direction_bits); \ + DIR_WAIT_AFTER(); \ + } \ + } \ + else \ + step_needed.clear(_AXIS(AXIS)); \ + DELTA_ERROR = de; \ + }while(0) + + // Start an active pulse if needed + #define PULSE_START(AXIS) do{ \ + if (step_needed.test(_AXIS(AXIS))) { \ + count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \ + _APPLY_STEP(AXIS, _STEP_STATE(AXIS), false); \ + } \ + }while(0) + + // Stop an active pulse if needed + #define PULSE_STOP(AXIS) do { \ + if (step_needed.test(_AXIS(AXIS))) { \ + _APPLY_STEP(AXIS, !_STEP_STATE(AXIS), false); \ + } \ + }while(0) + + #if ENABLED(DIRECT_STEPPING) + // Direct stepping is currently not ready for HAS_I_AXIS + if (is_page) { + + #if STEPPER_PAGE_FORMAT == SP_4x4D_128 + + #define PAGE_SEGMENT_UPDATE(AXIS, VALUE) do{ \ + if ((VALUE) < 7) dm[_AXIS(AXIS)] = false; \ + else if ((VALUE) > 7) dm[_AXIS(AXIS)] = true; \ + page_step_state.sd[_AXIS(AXIS)] = VALUE; \ + page_step_state.bd[_AXIS(AXIS)] += VALUE; \ + }while(0) + + #define PAGE_PULSE_PREP(AXIS) do{ \ + step_needed.set(_AXIS(AXIS), \ + pgm_read_byte(&segment_table[page_step_state.sd[_AXIS(AXIS)]][page_step_state.segment_steps & 0x7])); \ + }while(0) + + switch (page_step_state.segment_steps) { + case DirectStepping::Config::SEGMENT_STEPS: + page_step_state.segment_idx += 2; + page_step_state.segment_steps = 0; + // fallthru + case 0: { + const uint8_t low = page_step_state.page[page_step_state.segment_idx], + high = page_step_state.page[page_step_state.segment_idx + 1]; + const AxisBits dm = last_direction_bits; + + PAGE_SEGMENT_UPDATE(X, low >> 4); + PAGE_SEGMENT_UPDATE(Y, low & 0xF); + PAGE_SEGMENT_UPDATE(Z, high >> 4); + PAGE_SEGMENT_UPDATE(E, high & 0xF); + + if (dm != last_direction_bits) set_directions(dm); + + } break; + + default: break; + } + + PAGE_PULSE_PREP(X); + PAGE_PULSE_PREP(Y); + PAGE_PULSE_PREP(Z); + TERN_(HAS_EXTRUDERS, PAGE_PULSE_PREP(E)); + + page_step_state.segment_steps++; + + #elif STEPPER_PAGE_FORMAT == SP_4x2_256 + + #define PAGE_SEGMENT_UPDATE(AXIS, VALUE) \ + page_step_state.sd[_AXIS(AXIS)] = VALUE; \ + page_step_state.bd[_AXIS(AXIS)] += VALUE; + + #define PAGE_PULSE_PREP(AXIS) do{ \ + step_needed.set(_AXIS(AXIS), \ + pgm_read_byte(&segment_table[page_step_state.sd[_AXIS(AXIS)]][page_step_state.segment_steps & 0x3])); \ + }while(0) + + switch (page_step_state.segment_steps) { + case DirectStepping::Config::SEGMENT_STEPS: + page_step_state.segment_idx++; + page_step_state.segment_steps = 0; + // fallthru + case 0: { + const uint8_t b = page_step_state.page[page_step_state.segment_idx]; + PAGE_SEGMENT_UPDATE(X, (b >> 6) & 0x3); + PAGE_SEGMENT_UPDATE(Y, (b >> 4) & 0x3); + PAGE_SEGMENT_UPDATE(Z, (b >> 2) & 0x3); + PAGE_SEGMENT_UPDATE(E, (b >> 0) & 0x3); + } break; + default: break; + } + + PAGE_PULSE_PREP(X); + PAGE_PULSE_PREP(Y); + PAGE_PULSE_PREP(Z); + TERN_(HAS_EXTRUDERS, PAGE_PULSE_PREP(E)); + + page_step_state.segment_steps++; + + #elif STEPPER_PAGE_FORMAT == SP_4x1_512 + + #define PAGE_PULSE_PREP(AXIS, NBIT) do{ \ + step_needed.set(_AXIS(AXIS), TEST(steps, NBIT)); \ + if (step_needed.test(_AXIS(AXIS))) \ + page_step_state.bd[_AXIS(AXIS)]++; \ + }while(0) + + uint8_t steps = page_step_state.page[page_step_state.segment_idx >> 1]; + if (page_step_state.segment_idx & 0x1) steps >>= 4; + + PAGE_PULSE_PREP(X, 3); + PAGE_PULSE_PREP(Y, 2); + PAGE_PULSE_PREP(Z, 1); + PAGE_PULSE_PREP(E, 0); + + page_step_state.segment_idx++; + + #else + #error "Unknown direct stepping page format!" + #endif + } + + #endif // DIRECT_STEPPING + + if (!is_page) { + // Give the compiler a clue to store advance_divisor in registers for what follows + const uint32_t advance_divisor_cached = advance_divisor; + + // Determine if pulses are needed + #define _PULSE_PREP(A) TERF(HAS_##A##_STEP, PULSE_PREP)(A); + MAIN_AXIS_MAP(_PULSE_PREP); + + #if ANY(HAS_E0_STEP, MIXING_EXTRUDER) + PULSE_PREP(E); + #endif + + #if HAS_ROUGH_LIN_ADVANCE + if (la_active && step_needed.e) { + // Don't actually step here, but do subtract movements steps + // from the linear advance step count + step_needed.e = false; + la_advance_steps--; + } + #elif ENABLED(SMOOTH_LIN_ADVANCE) + // Extruder steps are exclusively managed by the LA isr + step_needed.e = false; + #endif + + #if HAS_ZV_SHAPING + // Record an echo if a step is needed in the primary Bresenham + const bool x_step = TERN0(INPUT_SHAPING_X, step_needed.x && shaping_x.enabled), + y_step = TERN0(INPUT_SHAPING_Y, step_needed.y && shaping_y.enabled), + z_step = TERN0(INPUT_SHAPING_Z, step_needed.z && shaping_z.enabled); + if (x_step || y_step || z_step) + ShapingQueue::enqueue(x_step, TERN0(INPUT_SHAPING_X, shaping_x.forward), y_step, TERN0(INPUT_SHAPING_Y, shaping_y.forward), z_step, TERN0(INPUT_SHAPING_Z, shaping_z.forward)); + + // Do the first part of the secondary Bresenham + #if ENABLED(INPUT_SHAPING_X) + if (x_step) + PULSE_PREP_SHAPING(X, shaping_x.delta_error, shaping_x.forward ? shaping_x.factor1 : -shaping_x.factor1); + #endif + #if ENABLED(INPUT_SHAPING_Y) + if (y_step) + PULSE_PREP_SHAPING(Y, shaping_y.delta_error, shaping_y.forward ? shaping_y.factor1 : -shaping_y.factor1); + #endif + #if ENABLED(INPUT_SHAPING_Z) + if (z_step) + PULSE_PREP_SHAPING(Z, shaping_z.delta_error, shaping_z.forward ? shaping_z.factor1 : -shaping_z.factor1); + #endif + #endif + } + + #if ISR_MULTI_STEPS + if (firstStep) + firstStep = false; + else + AWAIT_LOW_PULSE(); + #endif + + // Pulse start + #define _PULSE_START(A) TERF(HAS_##A##_STEP, PULSE_START)(A); + MAIN_AXIS_MAP(_PULSE_START); + + #if ENABLED(MIXING_EXTRUDER) + if (step_needed.e) { + count_position.e += count_direction.e; + E_STEP_WRITE(mixer.get_next_stepper(), STEP_STATE_E); + } + #elif HAS_E0_STEP + PULSE_START(E); + #endif + + TERN_(I2S_STEPPER_STREAM, i2s_push_sample()); + + // TODO: need to deal with MINIMUM_STEPPER_PULSE_NS over i2s + #if ISR_PULSE_CONTROL + START_TIMED_PULSE(); + AWAIT_HIGH_PULSE(); + #endif + + // Pulse stop + #define _PULSE_STOP(A) TERF(HAS_##A##_STEP, PULSE_STOP)(A); + MAIN_AXIS_MAP(_PULSE_STOP); + + #if ENABLED(MIXING_EXTRUDER) + if (step_needed.e) E_STEP_WRITE(mixer.get_stepper(), !STEP_STATE_E); + #elif HAS_E0_STEP + PULSE_STOP(E); + #endif + + #if ISR_MULTI_STEPS + if (events_to_do) START_TIMED_PULSE(); + #endif + + } while (--events_to_do); } - // If there is no current block, do nothing - if (!current_block || step_events_completed >= step_event_count) return; - - // Skipping step processing causes motion to freeze - if (TERN0(FREEZE_FEATURE, frozen)) return; - - // Count of pending loops and events for this iteration - const uint32_t pending_events = step_event_count - step_events_completed; - uint8_t events_to_do = _MIN(pending_events, steps_per_isr); - - // Just update the value we will get at the end of the loop - step_events_completed += events_to_do; - - TERN_(ISR_PULSE_CONTROL, USING_TIMED_PULSE()); - - // Take multiple steps per interrupt. For high speed moves. - #if ENABLED(ISR_MULTI_STEPS) - bool firstStep = true; - #endif - - // Direct Stepping page? - const bool is_page = current_block->is_page(); - - do { - AxisFlags step_needed{0}; - - #define _APPLY_STEP(AXIS, STATE, ALWAYS) AXIS ##_APPLY_STEP(STATE, ALWAYS) - #define _STEP_STATE(AXIS) STEP_STATE_## AXIS - - // Determine if a pulse is needed using Bresenham - #define PULSE_PREP(AXIS) do{ \ - int32_t de = delta_error[_AXIS(AXIS)] + advance_dividend[_AXIS(AXIS)]; \ - if (de >= 0) { \ - step_needed.set(_AXIS(AXIS)); \ - de -= advance_divisor_cached; \ - } \ - delta_error[_AXIS(AXIS)] = de; \ - }while(0) - - // With input shaping, direction changes can happen with almost only - // AWAIT_LOW_PULSE() and DIR_WAIT_BEFORE() between steps. To work around - // the TMC2208 / TMC2225 shutdown bug (#16076), add a half step hysteresis - // in each direction. This results in the position being off by half an - // average half step during travel but correct at the end of each segment. - #if AXIS_DRIVER_TYPE_X(TMC2208) || AXIS_DRIVER_TYPE_X(TMC2208_STANDALONE) || \ - AXIS_DRIVER_TYPE_X(TMC5160) || AXIS_DRIVER_TYPE_X(TMC5160_STANDALONE) - #define HYSTERESIS_X 64 - #else - #define HYSTERESIS_X 0 - #endif - #if AXIS_DRIVER_TYPE_Y(TMC2208) || AXIS_DRIVER_TYPE_Y(TMC2208_STANDALONE) || \ - AXIS_DRIVER_TYPE_Y(TMC5160) || AXIS_DRIVER_TYPE_Y(TMC5160_STANDALONE) - #define HYSTERESIS_Y 64 - #else - #define HYSTERESIS_Y 0 - #endif - #if AXIS_DRIVER_TYPE_Z(TMC2208) || AXIS_DRIVER_TYPE_Z(TMC2208_STANDALONE) || \ - AXIS_DRIVER_TYPE_Z(TMC5160) || AXIS_DRIVER_TYPE_Z(TMC5160_STANDALONE) - #define HYSTERESIS_Z 64 - #else - #define HYSTERESIS_Z 0 - #endif - #define _HYSTERESIS(AXIS) HYSTERESIS_##AXIS - #define HYSTERESIS(AXIS) _HYSTERESIS(AXIS) - - #define PULSE_PREP_SHAPING(AXIS, DELTA_ERROR, DIVIDEND) do{ \ - int16_t de = DELTA_ERROR + (DIVIDEND); \ - const bool step_fwd = de >= (64 + HYSTERESIS(AXIS)), \ - step_bak = de <= -(64 + HYSTERESIS(AXIS)); \ - if (step_fwd || step_bak) { \ - de += step_fwd ? -128 : 128; \ - if ((MAXDIR(AXIS) && step_bak) || (MINDIR(AXIS) && step_fwd)) { \ - { USING_TIMED_PULSE(); START_TIMED_PULSE(); AWAIT_LOW_PULSE(); } \ - last_direction_bits.toggle(_AXIS(AXIS)); \ - DIR_WAIT_BEFORE(); \ - SET_STEP_DIR(AXIS); \ - TERN_(FT_MOTION, last_set_direction = last_direction_bits); \ - DIR_WAIT_AFTER(); \ - } \ - } \ - else \ - step_needed.clear(_AXIS(AXIS)); \ - DELTA_ERROR = de; \ - }while(0) - - // Start an active pulse if needed - #define PULSE_START(AXIS) do{ \ - if (step_needed.test(_AXIS(AXIS))) { \ - count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \ - _APPLY_STEP(AXIS, _STEP_STATE(AXIS), 0); \ - } \ - }while(0) - - // Stop an active pulse if needed - #define PULSE_STOP(AXIS) do { \ - if (step_needed.test(_AXIS(AXIS))) { \ - _APPLY_STEP(AXIS, !_STEP_STATE(AXIS), 0); \ - } \ - }while(0) - - #if ENABLED(DIRECT_STEPPING) - // Direct stepping is currently not ready for HAS_I_AXIS - if (is_page) { - - #if STEPPER_PAGE_FORMAT == SP_4x4D_128 - - #define PAGE_SEGMENT_UPDATE(AXIS, VALUE) do{ \ - if ((VALUE) < 7) dm[_AXIS(AXIS)] = false; \ - else if ((VALUE) > 7) dm[_AXIS(AXIS)] = true; \ - page_step_state.sd[_AXIS(AXIS)] = VALUE; \ - page_step_state.bd[_AXIS(AXIS)] += VALUE; \ - }while(0) - - #define PAGE_PULSE_PREP(AXIS) do{ \ - step_needed.set(_AXIS(AXIS), \ - pgm_read_byte(&segment_table[page_step_state.sd[_AXIS(AXIS)]][page_step_state.segment_steps & 0x7])); \ - }while(0) - - switch (page_step_state.segment_steps) { - case DirectStepping::Config::SEGMENT_STEPS: - page_step_state.segment_idx += 2; - page_step_state.segment_steps = 0; - // fallthru - case 0: { - const uint8_t low = page_step_state.page[page_step_state.segment_idx], - high = page_step_state.page[page_step_state.segment_idx + 1]; - const AxisBits dm = last_direction_bits; - - PAGE_SEGMENT_UPDATE(X, low >> 4); - PAGE_SEGMENT_UPDATE(Y, low & 0xF); - PAGE_SEGMENT_UPDATE(Z, high >> 4); - PAGE_SEGMENT_UPDATE(E, high & 0xF); - - if (dm != last_direction_bits) set_directions(dm); - - } break; - - default: break; - } - - PAGE_PULSE_PREP(X); - PAGE_PULSE_PREP(Y); - PAGE_PULSE_PREP(Z); - TERN_(HAS_EXTRUDERS, PAGE_PULSE_PREP(E)); - - page_step_state.segment_steps++; - - #elif STEPPER_PAGE_FORMAT == SP_4x2_256 - - #define PAGE_SEGMENT_UPDATE(AXIS, VALUE) \ - page_step_state.sd[_AXIS(AXIS)] = VALUE; \ - page_step_state.bd[_AXIS(AXIS)] += VALUE; - - #define PAGE_PULSE_PREP(AXIS) do{ \ - step_needed.set(_AXIS(AXIS), \ - pgm_read_byte(&segment_table[page_step_state.sd[_AXIS(AXIS)]][page_step_state.segment_steps & 0x3])); \ - }while(0) - - switch (page_step_state.segment_steps) { - case DirectStepping::Config::SEGMENT_STEPS: - page_step_state.segment_idx++; - page_step_state.segment_steps = 0; - // fallthru - case 0: { - const uint8_t b = page_step_state.page[page_step_state.segment_idx]; - PAGE_SEGMENT_UPDATE(X, (b >> 6) & 0x3); - PAGE_SEGMENT_UPDATE(Y, (b >> 4) & 0x3); - PAGE_SEGMENT_UPDATE(Z, (b >> 2) & 0x3); - PAGE_SEGMENT_UPDATE(E, (b >> 0) & 0x3); - } break; - default: break; - } - - PAGE_PULSE_PREP(X); - PAGE_PULSE_PREP(Y); - PAGE_PULSE_PREP(Z); - TERN_(HAS_EXTRUDERS, PAGE_PULSE_PREP(E)); - - page_step_state.segment_steps++; - - #elif STEPPER_PAGE_FORMAT == SP_4x1_512 - - #define PAGE_PULSE_PREP(AXIS, NBIT) do{ \ - step_needed.set(_AXIS(AXIS), TEST(steps, NBIT)); \ - if (step_needed.test(_AXIS(AXIS))) \ - page_step_state.bd[_AXIS(AXIS)]++; \ - }while(0) - - uint8_t steps = page_step_state.page[page_step_state.segment_idx >> 1]; - if (page_step_state.segment_idx & 0x1) steps >>= 4; - - PAGE_PULSE_PREP(X, 3); - PAGE_PULSE_PREP(Y, 2); - PAGE_PULSE_PREP(Z, 1); - PAGE_PULSE_PREP(E, 0); - - page_step_state.segment_idx++; - - #else - #error "Unknown direct stepping page format!" - #endif - } - - #endif // DIRECT_STEPPING - - if (!is_page) { - // Give the compiler a clue to store advance_divisor in registers for what follows - const uint32_t advance_divisor_cached = advance_divisor; - - // Determine if pulses are needed - #define _PULSE_PREP(A) TERF(HAS_##A##_STEP, PULSE_PREP)(A); - MAIN_AXIS_MAP(_PULSE_PREP); - - #if ANY(HAS_E0_STEP, MIXING_EXTRUDER) - PULSE_PREP(E); - #endif - - #if HAS_ROUGH_LIN_ADVANCE - if (la_active && step_needed.e) { - // Don't actually step here, but do subtract movements steps - // from the linear advance step count - step_needed.e = false; - la_advance_steps--; - } - #elif ENABLED(SMOOTH_LIN_ADVANCE) - // Extruder steps are exclusively managed by the LA isr - step_needed.e = false; - #endif - - #if HAS_ZV_SHAPING - // Record an echo if a step is needed in the primary Bresenham - const bool x_step = TERN0(INPUT_SHAPING_X, step_needed.x && shaping_x.enabled), - y_step = TERN0(INPUT_SHAPING_Y, step_needed.y && shaping_y.enabled), - z_step = TERN0(INPUT_SHAPING_Z, step_needed.z && shaping_z.enabled); - if (x_step || y_step || z_step) - ShapingQueue::enqueue(x_step, TERN0(INPUT_SHAPING_X, shaping_x.forward), y_step, TERN0(INPUT_SHAPING_Y, shaping_y.forward), z_step, TERN0(INPUT_SHAPING_Z, shaping_z.forward)); - - // Do the first part of the secondary Bresenham - #if ENABLED(INPUT_SHAPING_X) - if (x_step) - PULSE_PREP_SHAPING(X, shaping_x.delta_error, shaping_x.forward ? shaping_x.factor1 : -shaping_x.factor1); - #endif - #if ENABLED(INPUT_SHAPING_Y) - if (y_step) - PULSE_PREP_SHAPING(Y, shaping_y.delta_error, shaping_y.forward ? shaping_y.factor1 : -shaping_y.factor1); - #endif - #if ENABLED(INPUT_SHAPING_Z) - if (z_step) - PULSE_PREP_SHAPING(Z, shaping_z.delta_error, shaping_z.forward ? shaping_z.factor1 : -shaping_z.factor1); - #endif - #endif - } - - #if ISR_MULTI_STEPS - if (firstStep) - firstStep = false; - else - AWAIT_LOW_PULSE(); - #endif - - // Pulse start - #define _PULSE_START(A) TERF(HAS_##A##_STEP, PULSE_START)(A); - MAIN_AXIS_MAP(_PULSE_START); - - #if ENABLED(MIXING_EXTRUDER) - if (step_needed.e) { - count_position.e += count_direction.e; - E_STEP_WRITE(mixer.get_next_stepper(), STEP_STATE_E); - } - #elif HAS_E0_STEP - PULSE_START(E); - #endif - - TERN_(I2S_STEPPER_STREAM, i2s_push_sample()); - - // TODO: need to deal with MINIMUM_STEPPER_PULSE_NS over i2s - #if ISR_PULSE_CONTROL - START_TIMED_PULSE(); - AWAIT_HIGH_PULSE(); - #endif - - // Pulse stop - #define _PULSE_STOP(A) TERF(HAS_##A##_STEP, PULSE_STOP)(A); - MAIN_AXIS_MAP(_PULSE_STOP); - - #if ENABLED(MIXING_EXTRUDER) - if (step_needed.e) E_STEP_WRITE(mixer.get_stepper(), !STEP_STATE_E); - #elif HAS_E0_STEP - PULSE_STOP(E); - #endif - - #if ISR_MULTI_STEPS - if (events_to_do) START_TIMED_PULSE(); - #endif - - } while (--events_to_do); -} +#endif // HAS_STANDARD_MOTION #if HAS_ZV_SHAPING @@ -2195,737 +2204,742 @@ void Stepper::pulse_phase_isr() { #endif // HAS_ZV_SHAPING -// Calculate timer interval, with all limits applied. -hal_timer_t Stepper::calc_timer_interval(uint32_t step_rate) { +#if HAS_STANDARD_MOTION - #ifdef CPU_32_BIT + // Calculate timer interval, with all limits applied. + hal_timer_t Stepper::calc_timer_interval(uint32_t step_rate) { - // A fast processor can just do integer division - return step_rate > minimal_step_rate ? uint32_t(STEPPER_TIMER_RATE) / step_rate : HAL_TIMER_TYPE_MAX; + #ifdef CPU_32_BIT - #else + // A fast processor can just do integer division + return step_rate > minimal_step_rate ? uint32_t(STEPPER_TIMER_RATE) / step_rate : HAL_TIMER_TYPE_MAX; - if (step_rate >= 0x0800) { // Higher step rate - // AVR is able to keep up at around 65kHz Stepping ISR rate at most. - // So values for step_rate > 65535 might as well be truncated. - // Handle it as quickly as possible. i.e., assume highest byte is zero - // because non-zero would represent a step rate far beyond AVR capabilities. - if (uint8_t(step_rate >> 16)) - return uint32_t(STEPPER_TIMER_RATE) / 0x10000; + #else - const uintptr_t table_address = uintptr_t(&speed_lookuptable_fast[uint8_t(step_rate >> 8)]); - const uint16_t base = uint16_t(pgm_read_word(table_address)); - const uint8_t gain = uint8_t(pgm_read_byte(table_address + 2)); - return base - MultiU8X8toH8(uint8_t(step_rate & 0x00FF), gain); - } - else if (step_rate > minimal_step_rate) { // Lower step rates - step_rate -= minimal_step_rate; // Correct for minimal speed - const uintptr_t table_address = uintptr_t(&speed_lookuptable_slow[uint8_t(step_rate >> 3)]); - return uint16_t(pgm_read_word(table_address)) - - ((uint16_t(pgm_read_word(table_address + 2)) * uint8_t(step_rate & 0x0007)) >> 3); - } + if (step_rate >= 0x0800) { // Higher step rate + // AVR is able to keep up at around 65kHz Stepping ISR rate at most. + // So values for step_rate > 65535 might as well be truncated. + // Handle it as quickly as possible. i.e., assume highest byte is zero + // because non-zero would represent a step rate far beyond AVR capabilities. + if (uint8_t(step_rate >> 16)) + return uint32_t(STEPPER_TIMER_RATE) / 0x10000; - return uint16_t(pgm_read_word(uintptr_t(speed_lookuptable_slow))); + const uintptr_t table_address = uintptr_t(&speed_lookuptable_fast[uint8_t(step_rate >> 8)]); + const uint16_t base = uint16_t(pgm_read_word(table_address)); + const uint8_t gain = uint8_t(pgm_read_byte(table_address + 2)); + return base - MultiU8X8toH8(uint8_t(step_rate & 0x00FF), gain); + } + else if (step_rate > minimal_step_rate) { // Lower step rates + step_rate -= minimal_step_rate; // Correct for minimal speed + const uintptr_t table_address = uintptr_t(&speed_lookuptable_slow[uint8_t(step_rate >> 3)]); + return uint16_t(pgm_read_word(table_address)) + - ((uint16_t(pgm_read_word(table_address + 2)) * uint8_t(step_rate & 0x0007)) >> 3); + } - #endif // !CPU_32_BIT -} + return uint16_t(pgm_read_word(uintptr_t(speed_lookuptable_slow))); -#if NONLINEAR_EXTRUSION_Q24 - void Stepper::calc_nonlinear_e(const uint32_t step_rate) { - const uint32_t velocity_q24 = ne.scale_q24 * step_rate; // Scale step_rate first so all intermediate values stay in range of 8.24 fixed point math - int32_t vd_q24 = ((((int64_t(ne.q24.A) * velocity_q24) >> 24) * velocity_q24) >> 24) + ((int64_t(ne.q24.B) * velocity_q24) >> 24); - NOLESS(vd_q24, 0); - - advance_dividend.e = (uint64_t(ne.q24.C + vd_q24) * ne.edividend) >> 24; + #endif // !CPU_32_BIT } -#endif -// Get the timer interval and the number of loops to perform per tick -hal_timer_t Stepper::calc_multistep_timer_interval(uint32_t step_rate) { + #if NONLINEAR_EXTRUSION_Q24 + void Stepper::calc_nonlinear_e(const uint32_t step_rate) { + const uint32_t velocity_q24 = ne.scale_q24 * step_rate; // Scale step_rate first so all intermediate values stay in range of 8.24 fixed point math + int32_t vd_q24 = ((((int64_t(ne.q24.A) * velocity_q24) >> 24) * velocity_q24) >> 24) + ((int64_t(ne.q24.B) * velocity_q24) >> 24); + NOLESS(vd_q24, 0); - #if ENABLED(OLD_ADAPTIVE_MULTISTEPPING) + advance_dividend.e = (uint64_t(ne.q24.C + vd_q24) * ne.edividend) >> 24; + } + #endif - #if MULTISTEPPING_LIMIT == 1 + // Get the timer interval and the number of loops to perform per tick + hal_timer_t Stepper::calc_multistep_timer_interval(uint32_t step_rate) { - // Just make sure the step rate is doable - NOMORE(step_rate, uint32_t(MAX_STEP_ISR_FREQUENCY_1X)); + #if ENABLED(OLD_ADAPTIVE_MULTISTEPPING) + #if MULTISTEPPING_LIMIT == 1 + + // Just make sure the step rate is doable + NOMORE(step_rate, uint32_t(MAX_STEP_ISR_FREQUENCY_1X)); + + #else + + // The stepping frequency limits for each multistepping rate + static const uint32_t limit[] PROGMEM = { + max_step_isr_frequency_sh(0) + , max_step_isr_frequency_sh(1) + #if MULTISTEPPING_LIMIT >= 4 + , max_step_isr_frequency_sh(2) + #endif + #if MULTISTEPPING_LIMIT >= 8 + , max_step_isr_frequency_sh(3) + #endif + #if MULTISTEPPING_LIMIT >= 16 + , max_step_isr_frequency_sh(4) + #endif + #if MULTISTEPPING_LIMIT >= 32 + , max_step_isr_frequency_sh(5) + #endif + #if MULTISTEPPING_LIMIT >= 64 + , max_step_isr_frequency_sh(6) + #endif + #if MULTISTEPPING_LIMIT >= 128 + , max_step_isr_frequency_sh(7) + #endif + }; + + // Find a doable step rate using multistepping + uint8_t multistep = 1; + for (uint8_t i = 0; i < COUNT(limit) && step_rate > uint32_t(pgm_read_dword(&limit[i])); ++i) { + step_rate >>= 1; + multistep <<= 1; + } + steps_per_isr = multistep; + + #endif + + #elif MULTISTEPPING_LIMIT > 1 + + uint8_t loops = steps_per_isr; + if (MULTISTEPPING_LIMIT >= 16 && loops >= 16) { step_rate >>= 4; loops >>= 4; } + if (MULTISTEPPING_LIMIT >= 4 && loops >= 4) { step_rate >>= 2; loops >>= 2; } + if (MULTISTEPPING_LIMIT >= 2 && loops >= 2) { step_rate >>= 1; } + + #endif + + return calc_timer_interval(step_rate); + } + + // Method to get all moving axes (for proper endstop handling) + void Stepper::set_axis_moved_for_current_block() { + + #if IS_CORE + // Define conditions for checking endstops + #define S_(N) current_block->steps[CORE_AXIS_##N] + #define D_(N) current_block->direction_bits[CORE_AXIS_##N] + #endif + + #if CORE_IS_XY || CORE_IS_XZ + /** + * Head direction in -X axis for CoreXY and CoreXZ bots. + * + * If steps differ, both axes are moving. + * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z, handled below) + * If DeltaA == DeltaB, the movement is only in the 1st axis (X) + */ + #if ANY(COREXY, COREXZ) + #define X_CMP(A,B) ((A)==(B)) + #else + #define X_CMP(A,B) ((A)!=(B)) + #endif + #define X_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && X_CMP(D_(1),D_(2))) ) + #elif ENABLED(MARKFORGED_XY) + #define X_MOVE_TEST (current_block->steps.a != current_block->steps.b) #else - - // The stepping frequency limits for each multistepping rate - static const uint32_t limit[] PROGMEM = { - max_step_isr_frequency_sh(0) - , max_step_isr_frequency_sh(1) - #if MULTISTEPPING_LIMIT >= 4 - , max_step_isr_frequency_sh(2) - #endif - #if MULTISTEPPING_LIMIT >= 8 - , max_step_isr_frequency_sh(3) - #endif - #if MULTISTEPPING_LIMIT >= 16 - , max_step_isr_frequency_sh(4) - #endif - #if MULTISTEPPING_LIMIT >= 32 - , max_step_isr_frequency_sh(5) - #endif - #if MULTISTEPPING_LIMIT >= 64 - , max_step_isr_frequency_sh(6) - #endif - #if MULTISTEPPING_LIMIT >= 128 - , max_step_isr_frequency_sh(7) - #endif - }; - - // Find a doable step rate using multistepping - uint8_t multistep = 1; - for (uint8_t i = 0; i < COUNT(limit) && step_rate > uint32_t(pgm_read_dword(&limit[i])); ++i) { - step_rate >>= 1; - multistep <<= 1; - } - steps_per_isr = multistep; - + #define X_MOVE_TEST !!current_block->steps.a #endif - #elif MULTISTEPPING_LIMIT > 1 - - uint8_t loops = steps_per_isr; - if (MULTISTEPPING_LIMIT >= 16 && loops >= 16) { step_rate >>= 4; loops >>= 4; } - if (MULTISTEPPING_LIMIT >= 4 && loops >= 4) { step_rate >>= 2; loops >>= 2; } - if (MULTISTEPPING_LIMIT >= 2 && loops >= 2) { step_rate >>= 1; } - - #endif - - return calc_timer_interval(step_rate); -} - -// Method to get all moving axes (for proper endstop handling) -void Stepper::set_axis_moved_for_current_block() { - - #if IS_CORE - // Define conditions for checking endstops - #define S_(N) current_block->steps[CORE_AXIS_##N] - #define D_(N) current_block->direction_bits[CORE_AXIS_##N] - #endif - - #if CORE_IS_XY || CORE_IS_XZ - /** - * Head direction in -X axis for CoreXY and CoreXZ bots. - * - * If steps differ, both axes are moving. - * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z, handled below) - * If DeltaA == DeltaB, the movement is only in the 1st axis (X) - */ - #if ANY(COREXY, COREXZ) - #define X_CMP(A,B) ((A)==(B)) + #if CORE_IS_XY || CORE_IS_YZ + /** + * Head direction in -Y axis for CoreXY / CoreYZ bots. + * + * If steps differ, both axes are moving + * If DeltaA == DeltaB, the movement is only in the 1st axis (X or Y) + * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z) + */ + #if ANY(COREYX, COREYZ) + #define Y_CMP(A,B) ((A)==(B)) + #else + #define Y_CMP(A,B) ((A)!=(B)) + #endif + #define Y_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && Y_CMP(D_(1),D_(2))) ) + #elif ENABLED(MARKFORGED_YX) + #define Y_MOVE_TEST (current_block->steps.a != current_block->steps.b) #else - #define X_CMP(A,B) ((A)!=(B)) + #define Y_MOVE_TEST !!current_block->steps.b #endif - #define X_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && X_CMP(D_(1),D_(2))) ) - #elif ENABLED(MARKFORGED_XY) - #define X_MOVE_TEST (current_block->steps.a != current_block->steps.b) - #else - #define X_MOVE_TEST !!current_block->steps.a - #endif - #if CORE_IS_XY || CORE_IS_YZ - /** - * Head direction in -Y axis for CoreXY / CoreYZ bots. - * - * If steps differ, both axes are moving - * If DeltaA == DeltaB, the movement is only in the 1st axis (X or Y) - * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z) - */ - #if ANY(COREYX, COREYZ) - #define Y_CMP(A,B) ((A)==(B)) + #if CORE_IS_XZ || CORE_IS_YZ + /** + * Head direction in -Z axis for CoreXZ or CoreYZ bots. + * + * If steps differ, both axes are moving + * If DeltaA == DeltaB, the movement is only in the 1st axis (X or Y, already handled above) + * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Z) + */ + #if ANY(COREZX, COREZY) + #define Z_CMP(A,B) ((A)==(B)) + #else + #define Z_CMP(A,B) ((A)!=(B)) + #endif + #define Z_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && Z_CMP(D_(1),D_(2))) ) #else - #define Y_CMP(A,B) ((A)!=(B)) + #define Z_MOVE_TEST !!current_block->steps.c #endif - #define Y_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && Y_CMP(D_(1),D_(2))) ) - #elif ENABLED(MARKFORGED_YX) - #define Y_MOVE_TEST (current_block->steps.a != current_block->steps.b) - #else - #define Y_MOVE_TEST !!current_block->steps.b - #endif - #if CORE_IS_XZ || CORE_IS_YZ - /** - * Head direction in -Z axis for CoreXZ or CoreYZ bots. - * - * If steps differ, both axes are moving - * If DeltaA == DeltaB, the movement is only in the 1st axis (X or Y, already handled above) - * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Z) - */ - #if ANY(COREZX, COREZY) - #define Z_CMP(A,B) ((A)==(B)) - #else - #define Z_CMP(A,B) ((A)!=(B)) - #endif - #define Z_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && Z_CMP(D_(1),D_(2))) ) - #else - #define Z_MOVE_TEST !!current_block->steps.c - #endif + // Set flags for all axes that move in this block + // These are set per-axis, not per-stepper + AxisBits didmove; + NUM_AXIS_CODE( + if (X_MOVE_TEST) didmove.a = true, // Cartesian X or Kinematic A + if (Y_MOVE_TEST) didmove.b = true, // Cartesian Y or Kinematic B + if (Z_MOVE_TEST) didmove.c = true, // Cartesian Z or Kinematic C + if (!!current_block->steps.i) didmove.i = true, + if (!!current_block->steps.j) didmove.j = true, + if (!!current_block->steps.k) didmove.k = true, + if (!!current_block->steps.u) didmove.u = true, + if (!!current_block->steps.v) didmove.v = true, + if (!!current_block->steps.w) didmove.w = true + ); + axis_did_move = didmove; + } - // Set flags for all axes that move in this block - // These are set per-axis, not per-stepper - AxisBits didmove; - NUM_AXIS_CODE( - if (X_MOVE_TEST) didmove.a = true, // Cartesian X or Kinematic A - if (Y_MOVE_TEST) didmove.b = true, // Cartesian Y or Kinematic B - if (Z_MOVE_TEST) didmove.c = true, // Cartesian Z or Kinematic C - if (!!current_block->steps.i) didmove.i = true, - if (!!current_block->steps.j) didmove.j = true, - if (!!current_block->steps.k) didmove.k = true, - if (!!current_block->steps.u) didmove.u = true, - if (!!current_block->steps.v) didmove.v = true, - if (!!current_block->steps.w) didmove.w = true - ); - axis_did_move = didmove; -} - -/** - * This last phase of the stepper interrupt processes and properly - * schedules planner blocks. This is executed after the step pulses - * have been done, so it is less time critical. - */ -hal_timer_t Stepper::block_phase_isr() { - #if DISABLED(OLD_ADAPTIVE_MULTISTEPPING) - // If the ISR uses < 50% of MPU time, halve multi-stepping - const hal_timer_t time_spent = HAL_timer_get_count(MF_TIMER_STEP); - #if MULTISTEPPING_LIMIT > 1 - if (steps_per_isr > 1 && time_spent_out_isr >= time_spent_in_isr + time_spent) { - steps_per_isr >>= 1; - // ticks_nominal will need to be recalculated if we are in cruise phase - ticks_nominal = 0; - } - #endif - time_spent_in_isr = -time_spent; // Unsigned but guaranteed to be +ve when needed - time_spent_out_isr = 0; - #endif - - // If no queued movements, just wait 1ms for the next block - hal_timer_t interval = (STEPPER_TIMER_RATE) / 1000UL; - - // If there is a current block - if (current_block) { - // If current block is finished, reset pointer and finalize state - if (step_events_completed >= step_event_count) { - #if ENABLED(DIRECT_STEPPING) - // Direct stepping is currently not ready for HAS_I_AXIS - #if STEPPER_PAGE_FORMAT == SP_4x4D_128 - #define PAGE_SEGMENT_UPDATE_POS(AXIS) \ - count_position[_AXIS(AXIS)] += page_step_state.bd[_AXIS(AXIS)] - 128 * 7; - #elif STEPPER_PAGE_FORMAT == SP_4x1_512 || STEPPER_PAGE_FORMAT == SP_4x2_256 - #define PAGE_SEGMENT_UPDATE_POS(AXIS) \ - count_position[_AXIS(AXIS)] += page_step_state.bd[_AXIS(AXIS)] * count_direction[_AXIS(AXIS)]; - #endif - - if (current_block->is_page()) { - PAGE_SEGMENT_UPDATE_POS(X); - PAGE_SEGMENT_UPDATE_POS(Y); - PAGE_SEGMENT_UPDATE_POS(Z); - PAGE_SEGMENT_UPDATE_POS(E); + /** + * This last phase of the stepper interrupt processes and properly + * schedules planner blocks. This is executed after the step pulses + * have been done, so it is less time critical. + */ + hal_timer_t Stepper::block_phase_isr() { + #if DISABLED(OLD_ADAPTIVE_MULTISTEPPING) + // If the ISR uses < 50% of MPU time, halve multi-stepping + const hal_timer_t time_spent = HAL_timer_get_count(MF_TIMER_STEP); + #if MULTISTEPPING_LIMIT > 1 + if (steps_per_isr > 1 && time_spent_out_isr >= time_spent_in_isr + time_spent) { + steps_per_isr >>= 1; + // ticks_nominal will need to be recalculated if we are in cruise phase + ticks_nominal = 0; } #endif - TERN_(HAS_FILAMENT_RUNOUT_DISTANCE, runout.block_completed(current_block)); - discard_current_block(); - } - else { - // Step events not completed yet... + time_spent_in_isr = -time_spent; // Unsigned but guaranteed to be +ve when needed + time_spent_out_isr = 0; + #endif - // Are we in acceleration phase ? - if (step_events_completed < accelerate_before) { // Calculate new timer value + // If no queued movements, just wait 1ms for the next block + hal_timer_t interval = (STEPPER_TIMER_RATE) / 1000UL; - #if ENABLED(S_CURVE_ACCELERATION) - // Get the next speed to use (Jerk limited!) - uint32_t acc_step_rate = acceleration_time < current_block->acceleration_time - ? _eval_bezier_curve(acceleration_time) - : current_block->cruise_rate; - #else - acc_step_rate = STEP_MULTIPLY(acceleration_time, current_block->acceleration_rate) + current_block->initial_rate; - NOMORE(acc_step_rate, current_block->nominal_rate); - #endif - - // acc_step_rate is in steps/second - - // step_rate to timer interval and steps per stepper isr - interval = calc_multistep_timer_interval(acc_step_rate << oversampling_factor); - acceleration_time += interval; - deceleration_time = 0; // Reset since we're doing acceleration first. - - // Apply Nonlinear Extrusion, if enabled - calc_nonlinear_e(acc_step_rate << oversampling_factor); - - #if HAS_ROUGH_LIN_ADVANCE - if (la_active) { - const uint32_t la_step_rate = la_advance_steps < current_block->max_adv_steps ? current_block->la_advance_rate : 0; - la_interval = calc_timer_interval((acc_step_rate + la_step_rate) >> current_block->la_scaling); - } - #endif - - /** - * Adjust Laser Power - Accelerating - * - * isPowered - True when a move is powered. - * isEnabled - laser power is active. - * - * Laser power variables are calculated and stored in this block by the planner code. - * trap_ramp_active_pwr - the active power in this block across accel or decel trap steps. - * trap_ramp_entry_incr - holds the precalculated value to increase the current power per accel step. - */ - #if ENABLED(LASER_POWER_TRAP) - if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { - if (planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled) { - if (current_block->laser.trap_ramp_entry_incr > 0) { - cutter.apply_power(current_block->laser.trap_ramp_active_pwr); - current_block->laser.trap_ramp_active_pwr += current_block->laser.trap_ramp_entry_incr * steps_per_isr; - } - } - // Not a powered move. - else cutter.apply_power(0); - } - #endif - TERN_(SMOOTH_LIN_ADVANCE, curr_step_rate = acc_step_rate); - } - // Are we in Deceleration phase ? - else if (step_events_completed >= decelerate_start) { - uint32_t step_rate; - - #if ENABLED(S_CURVE_ACCELERATION) - // If this is the 1st time we process the 2nd half of the trapezoid... - if (!bezier_2nd_half) { - // Initialize the Bézier speed curve - _calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse); - bezier_2nd_half = true; - } - // Calculate the next speed to use - step_rate = deceleration_time < current_block->deceleration_time - ? _eval_bezier_curve(deceleration_time) - : current_block->final_rate; - #else - // Using the old trapezoidal control - step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate); - if (step_rate < acc_step_rate) { - step_rate = acc_step_rate - step_rate; - NOLESS(step_rate, current_block->final_rate); - } - else - step_rate = current_block->final_rate; - - #endif - - // step_rate to timer interval and steps per stepper isr - interval = calc_multistep_timer_interval(step_rate << oversampling_factor); - deceleration_time += interval; - - // Apply Nonlinear Extrusion, if enabled - calc_nonlinear_e(step_rate << oversampling_factor); - - #if HAS_ROUGH_LIN_ADVANCE - if (la_active) { - const uint32_t la_step_rate = la_advance_steps > current_block->final_adv_steps ? current_block->la_advance_rate : 0; - if (la_step_rate != step_rate) { - const bool forward_e = la_step_rate < step_rate; - la_interval = calc_timer_interval((forward_e ? step_rate - la_step_rate : la_step_rate - step_rate) >> current_block->la_scaling); - - if (forward_e != motor_direction(E_AXIS)) { - last_direction_bits.toggle(E_AXIS); - count_direction.e *= -1; - - DIR_WAIT_BEFORE(); - - E_APPLY_DIR(forward_e, false); - - TERN_(FT_MOTION, last_set_direction = last_direction_bits); - - DIR_WAIT_AFTER(); - } - } - else - la_interval = LA_ADV_NEVER; - } - #endif // LIN_ADVANCE - - // Adjust Laser Power - Decelerating - #if ENABLED(LASER_POWER_TRAP) - if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { - if (planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled) { - if (current_block->laser.trap_ramp_exit_decr > 0) { - current_block->laser.trap_ramp_active_pwr -= current_block->laser.trap_ramp_exit_decr * steps_per_isr; - cutter.apply_power(current_block->laser.trap_ramp_active_pwr); - } - // Not a powered move. - else cutter.apply_power(0); - } - } - #endif - TERN_(SMOOTH_LIN_ADVANCE, curr_step_rate = step_rate); - } - else { // Must be in cruise phase otherwise - - // Calculate the ticks_nominal for this nominal speed, if not done yet - if (ticks_nominal == 0) { - // step_rate to timer interval and loops for the nominal speed - ticks_nominal = calc_multistep_timer_interval(current_block->nominal_rate << oversampling_factor); - // Prepare for deceleration - IF_DISABLED(S_CURVE_ACCELERATION, acc_step_rate = current_block->nominal_rate); - TERN_(SMOOTH_LIN_ADVANCE, curr_step_rate = current_block->nominal_rate); - deceleration_time = ticks_nominal / 2; - - // Apply Nonlinear Extrusion, if enabled - calc_nonlinear_e(current_block->nominal_rate << oversampling_factor); - - #if HAS_ROUGH_LIN_ADVANCE - if (la_active) - la_interval = calc_timer_interval(current_block->nominal_rate >> current_block->la_scaling); + // If there is a current block + if (current_block) { + // If current block is finished, reset pointer and finalize state + if (step_events_completed >= step_event_count) { + #if ENABLED(DIRECT_STEPPING) + // Direct stepping is currently not ready for HAS_I_AXIS + #if STEPPER_PAGE_FORMAT == SP_4x4D_128 + #define PAGE_SEGMENT_UPDATE_POS(AXIS) \ + count_position[_AXIS(AXIS)] += page_step_state.bd[_AXIS(AXIS)] - 128 * 7; + #elif STEPPER_PAGE_FORMAT == SP_4x1_512 || STEPPER_PAGE_FORMAT == SP_4x2_256 + #define PAGE_SEGMENT_UPDATE_POS(AXIS) \ + count_position[_AXIS(AXIS)] += page_step_state.bd[_AXIS(AXIS)] * count_direction[_AXIS(AXIS)]; #endif - // Adjust Laser Power - Cruise + if (current_block->is_page()) { + PAGE_SEGMENT_UPDATE_POS(X); + PAGE_SEGMENT_UPDATE_POS(Y); + PAGE_SEGMENT_UPDATE_POS(Z); + PAGE_SEGMENT_UPDATE_POS(E); + } + #endif + TERN_(HAS_FILAMENT_RUNOUT_DISTANCE, runout.block_completed(current_block)); + discard_current_block(); + } + else { + // Step events not completed yet... + + // Are we in acceleration phase ? + if (step_events_completed < accelerate_before) { // Calculate new timer value + + #if ENABLED(S_CURVE_ACCELERATION) + // Get the next speed to use (Jerk limited!) + uint32_t acc_step_rate = acceleration_time < current_block->acceleration_time + ? _eval_bezier_curve(acceleration_time) + : current_block->cruise_rate; + #else + acc_step_rate = STEP_MULTIPLY(acceleration_time, current_block->acceleration_rate) + current_block->initial_rate; + NOMORE(acc_step_rate, current_block->nominal_rate); + #endif + + // acc_step_rate is in steps/second + + // step_rate to timer interval and steps per stepper isr + interval = calc_multistep_timer_interval(acc_step_rate << oversampling_factor); + acceleration_time += interval; + deceleration_time = 0; // Reset since we're doing acceleration first. + + // Apply Nonlinear Extrusion, if enabled + calc_nonlinear_e(acc_step_rate << oversampling_factor); + + #if HAS_ROUGH_LIN_ADVANCE + if (la_active) { + const uint32_t la_step_rate = la_advance_steps < current_block->max_adv_steps ? current_block->la_advance_rate : 0; + la_interval = calc_timer_interval((acc_step_rate + la_step_rate) >> current_block->la_scaling); + } + #endif + + /** + * Adjust Laser Power - Accelerating + * + * isPowered - True when a move is powered. + * isEnabled - laser power is active. + * + * Laser power variables are calculated and stored in this block by the planner code. + * trap_ramp_active_pwr - the active power in this block across accel or decel trap steps. + * trap_ramp_entry_incr - holds the precalculated value to increase the current power per accel step. + */ #if ENABLED(LASER_POWER_TRAP) if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { if (planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled) { if (current_block->laser.trap_ramp_entry_incr > 0) { - current_block->laser.trap_ramp_active_pwr = current_block->laser.power; - cutter.apply_power(current_block->laser.power); + cutter.apply_power(current_block->laser.trap_ramp_active_pwr); + current_block->laser.trap_ramp_active_pwr += current_block->laser.trap_ramp_entry_incr * steps_per_isr; } } // Not a powered move. else cutter.apply_power(0); } #endif + TERN_(SMOOTH_LIN_ADVANCE, curr_step_rate = acc_step_rate); } + // Are we in Deceleration phase ? + else if (step_events_completed >= decelerate_start) { + uint32_t step_rate; - // The timer interval is just the nominal value for the nominal speed - interval = ticks_nominal; + #if ENABLED(S_CURVE_ACCELERATION) + // If this is the 1st time we process the 2nd half of the trapezoid... + if (!bezier_2nd_half) { + // Initialize the Bézier speed curve + _calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse); + bezier_2nd_half = true; + } + // Calculate the next speed to use + step_rate = deceleration_time < current_block->deceleration_time + ? _eval_bezier_curve(deceleration_time) + : current_block->final_rate; + #else + // Using the old trapezoidal control + step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate); + if (step_rate < acc_step_rate) { + step_rate = acc_step_rate - step_rate; + NOLESS(step_rate, current_block->final_rate); + } + else + step_rate = current_block->final_rate; + + #endif + + // step_rate to timer interval and steps per stepper isr + interval = calc_multistep_timer_interval(step_rate << oversampling_factor); + deceleration_time += interval; + + // Apply Nonlinear Extrusion, if enabled + calc_nonlinear_e(step_rate << oversampling_factor); + + #if HAS_ROUGH_LIN_ADVANCE + if (la_active) { + const uint32_t la_step_rate = la_advance_steps > current_block->final_adv_steps ? current_block->la_advance_rate : 0; + if (la_step_rate != step_rate) { + const bool forward_e = la_step_rate < step_rate; + la_interval = calc_timer_interval((forward_e ? step_rate - la_step_rate : la_step_rate - step_rate) >> current_block->la_scaling); + + if (forward_e != motor_direction(E_AXIS)) { + last_direction_bits.toggle(E_AXIS); + count_direction.e *= -1; + + DIR_WAIT_BEFORE(); + + E_APPLY_DIR(forward_e, false); + + TERN_(FT_MOTION, last_set_direction = last_direction_bits); + + DIR_WAIT_AFTER(); + } + } + else + la_interval = LA_ADV_NEVER; + } + #endif // LIN_ADVANCE + + // Adjust Laser Power - Decelerating + #if ENABLED(LASER_POWER_TRAP) + if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { + if (planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled) { + if (current_block->laser.trap_ramp_exit_decr > 0) { + current_block->laser.trap_ramp_active_pwr -= current_block->laser.trap_ramp_exit_decr * steps_per_isr; + cutter.apply_power(current_block->laser.trap_ramp_active_pwr); + } + // Not a powered move. + else cutter.apply_power(0); + } + } + #endif + TERN_(SMOOTH_LIN_ADVANCE, curr_step_rate = step_rate); + } + else { // Must be in cruise phase otherwise + + // Calculate the ticks_nominal for this nominal speed, if not done yet + if (ticks_nominal == 0) { + // step_rate to timer interval and loops for the nominal speed + ticks_nominal = calc_multistep_timer_interval(current_block->nominal_rate << oversampling_factor); + deceleration_time = ticks_nominal / 2; + + // Prepare for deceleration + IF_DISABLED(S_CURVE_ACCELERATION, acc_step_rate = current_block->nominal_rate); + TERN_(SMOOTH_LIN_ADVANCE, curr_step_rate = current_block->nominal_rate); + + // Apply Nonlinear Extrusion, if enabled + calc_nonlinear_e(current_block->nominal_rate << oversampling_factor); + + #if HAS_ROUGH_LIN_ADVANCE + if (la_active) + la_interval = calc_timer_interval(current_block->nominal_rate >> current_block->la_scaling); + #endif + + // Adjust Laser Power - Cruise + #if ENABLED(LASER_POWER_TRAP) + if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { + if (planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled) { + if (current_block->laser.trap_ramp_entry_incr > 0) { + current_block->laser.trap_ramp_active_pwr = current_block->laser.power; + cutter.apply_power(current_block->laser.power); + } + } + // Not a powered move. + else cutter.apply_power(0); + } + #endif + } + + // The timer interval is just the nominal value for the nominal speed + interval = ticks_nominal; + } } + + #if ENABLED(LASER_FEATURE) + /** + * CUTTER_MODE_DYNAMIC is experimental and developing. + * Super-fast method to dynamically adjust the laser power OCR value based on the input feedrate in mm-per-minute. + * TODO: Set up Min/Max OCR offsets to allow tuning and scaling of various lasers. + * TODO: Integrate accel/decel +-rate into the dynamic laser power calc. + */ + if (cutter.cutter_mode == CUTTER_MODE_DYNAMIC + && planner.laser_inline.status.isPowered // isPowered flag set on any parsed G1, G2, G3, or G5 move; cleared on any others. + && current_block // Block may not be available if steps completed (see discard_current_block() above) + && cutter.last_block_power != current_block->laser.power // Prevent constant update without change + ) { + cutter.apply_power(current_block->laser.power); + cutter.last_block_power = current_block->laser.power; + } + #endif + } + else { // !current_block + #if ENABLED(LASER_FEATURE) + if (cutter.cutter_mode == CUTTER_MODE_DYNAMIC) + cutter.apply_power(0); // No movement in dynamic mode so turn Laser off + #endif } - #if ENABLED(LASER_FEATURE) - /** - * CUTTER_MODE_DYNAMIC is experimental and developing. - * Super-fast method to dynamically adjust the laser power OCR value based on the input feedrate in mm-per-minute. - * TODO: Set up Min/Max OCR offsets to allow tuning and scaling of various lasers. - * TODO: Integrate accel/decel +-rate into the dynamic laser power calc. - */ - if (cutter.cutter_mode == CUTTER_MODE_DYNAMIC - && planner.laser_inline.status.isPowered // isPowered flag set on any parsed G1, G2, G3, or G5 move; cleared on any others. - && current_block // Block may not be available if steps completed (see discard_current_block() above) - && cutter.last_block_power != current_block->laser.power // Prevent constant update without change - ) { - cutter.apply_power(current_block->laser.power); - cutter.last_block_power = current_block->laser.power; - } - #endif - } - else { // !current_block - #if ENABLED(LASER_FEATURE) - if (cutter.cutter_mode == CUTTER_MODE_DYNAMIC) - cutter.apply_power(0); // No movement in dynamic mode so turn Laser off - #endif - } + // If there is no current block at this point, attempt to pop one from the buffer + // and prepare its movement + if (!current_block) { - // If there is no current block at this point, attempt to pop one from the buffer - // and prepare its movement - if (!current_block) { + // Anything in the buffer? + if ((current_block = planner.get_current_block())) { - // Anything in the buffer? - if ((current_block = planner.get_current_block())) { + // Run through all sync blocks + while (current_block->is_sync()) { - // Run through all sync blocks - while (current_block->is_sync()) { + // Set laser power + #if ENABLED(LASER_POWER_SYNC) + if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { + if (current_block->is_sync_pwr()) { + planner.laser_inline.status.isSyncPower = true; + cutter.apply_power(current_block->laser.power); + } + } + #endif - // Set laser power - #if ENABLED(LASER_POWER_SYNC) - if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { - if (current_block->is_sync_pwr()) { - planner.laser_inline.status.isSyncPower = true; - cutter.apply_power(current_block->laser.power); + // Set "fan speeds" for a laser module + #if ENABLED(LASER_SYNCHRONOUS_M106_M107) + if (current_block->is_sync_fan()) planner.sync_fan_speeds(current_block->fan_speed); + #endif + + // Set position + if (current_block->is_sync_pos()) _set_position(current_block->position); + + // Done with this block + discard_current_block(); + + // Try to get a new block. Exit if there are no more. + if (!(current_block = planner.get_current_block())) + return interval; // No more queued movements! + } + + // For non-inline cutter, grossly apply power + #if HAS_CUTTER + if (cutter.cutter_mode == CUTTER_MODE_STANDARD) { + cutter.apply_power(current_block->cutter_power); + } + #endif + + #if ENABLED(POWER_LOSS_RECOVERY) + recovery.info.sdpos = current_block->sdpos; + recovery.info.current_position = current_block->start_position; + #endif + + #if ENABLED(DIRECT_STEPPING) + if (current_block->is_page()) { + page_step_state.segment_steps = 0; + page_step_state.segment_idx = 0; + page_step_state.page = page_manager.get_page(current_block->page_idx); + page_step_state.bd.reset(); + + if (DirectStepping::Config::DIRECTIONAL) + current_block->direction_bits = last_direction_bits; + + if (!page_step_state.page) { + discard_current_block(); + return interval; } } #endif - // Set "fan speeds" for a laser module - #if ENABLED(LASER_SYNCHRONOUS_M106_M107) - if (current_block->is_sync_fan()) planner.sync_fan_speeds(current_block->fan_speed); + // Set flags for all moving axes, accounting for kinematics + set_axis_moved_for_current_block(); + + #if ENABLED(ADAPTIVE_STEP_SMOOTHING) + oversampling_factor = 0; + + // Decide if axis smoothing is possible + if (adaptive_step_smoothing_enabled) { + uint32_t max_rate = current_block->nominal_rate; // Get the step event rate + while (max_rate < min_step_isr_frequency) { // As long as more ISRs are possible... + max_rate <<= 1; // Try to double the rate + if (max_rate < min_step_isr_frequency) // Don't exceed the estimated ISR limit + ++oversampling_factor; // Increase the oversampling (used for left-shift) + } + } #endif - // Set position - if (current_block->is_sync_pos()) _set_position(current_block->position); + // Based on the oversampling factor, do the calculations + step_event_count = current_block->step_event_count << oversampling_factor; - // Done with this block - discard_current_block(); + #if ENABLED(DIFFERENTIAL_EXTRUDER) - // Try to get a new block. Exit if there are no more. - if (!(current_block = planner.get_current_block())) - return interval; // No more queued movements! - } + // X and E work together as a differential mechanism: + // When X and E move in the same direction they move the print carriage, + // and when X and E move in opposite directions they cause extrusion. - // For non-inline cutter, grossly apply power - #if HAS_CUTTER - if (cutter.cutter_mode == CUTTER_MODE_STANDARD) { - cutter.apply_power(current_block->cutter_power); - } - #endif + // NOTE: All calculations performed per block, preserving Bresenham timing coordination + if (current_block->steps.x > 0 || current_block->steps.e > 0) { + // Calculate signed step counts based on directions + const int32_t x_signed_steps = current_block->direction_bits.x ? current_block->steps.x : -int32_t(current_block->steps.x), + e_signed_steps = current_block->direction_bits.e ? current_block->steps.e : -int32_t(current_block->steps.e); - #if ENABLED(POWER_LOSS_RECOVERY) - recovery.info.sdpos = current_block->sdpos; - recovery.info.current_position = current_block->start_position; - #endif + #if ENABLED(BALANCED_DIFFERENTIAL_EXTRUDER) - #if ENABLED(DIRECT_STEPPING) - if (current_block->is_page()) { - page_step_state.segment_steps = 0; - page_step_state.segment_idx = 0; - page_step_state.page = page_manager.get_page(current_block->page_idx); - page_step_state.bd.reset(); + // BALANCED DIFFERENTIAL EXTRUDER: X stepper drives one loop belt + // and E stepper drives another loop belt. Two belts mesh at the extruder + // X stepper: X - E/2 (carriage movement - half extrusion) + // E stepper: X + E/2 (carriage movement + half extrusion) + // Net extrusion: (X + E/2) - (X - E/2) = E + // (This will work great once I figure out what to do when E/2 is not integer!) - if (DirectStepping::Config::DIRECTIONAL) - current_block->direction_bits = last_direction_bits; + // Split extrusion 50/50 between X and E steppers + const int32_t half_e_steps = e_signed_steps / 2; + + // X stepper: X movement - half E extrusion + const int32_t new_x_steps = x_signed_steps - half_e_steps; + current_block->steps.x = ABS(new_x_steps); + + // Update X axis direction + current_block->direction_bits.x = new_x_steps >= 0; + + // E stepper: X movement + half E extrusion + const int32_t new_e_steps = x_signed_steps + half_e_steps; + current_block->steps.e = ABS(new_e_steps); + + // Update E axis direction + current_block->direction_bits.e = new_e_steps >= 0; + + #else // !BALANCED_DIFFERENTIAL_EXTRUDER + + // SIMPLE SINGLE-LOOP DIFFERENTIAL EXTRUDER: X stepper drives the carriage as usual, + // while E stepper drives a loop belt that's meshed around the extruder. + // X stepper: X only (carriage movement) + // E stepper: X + E (carriage movement + extrusion) + // Net extrusion: (X + E) - X = E + + // Calculate net E steps (X movement + extrusion) + const int32_t net_e_steps = x_signed_steps + e_signed_steps; + + // Update the block with new E step count + current_block->steps.e = ABS(net_e_steps); + + // Update direction bit for E axis + current_block->direction_bits.e = net_e_steps >= 0; + + #endif // !BALANCED_DIFFERENTIAL_EXTRUDER + + // NOTE: DO NOT modify the step_event_count! This would change XYZ timing! + // Bresenham distributes X and E steps over the time base. - if (!page_step_state.page) { - discard_current_block(); - return interval; } - } - #endif - // Set flags for all moving axes, accounting for kinematics - set_axis_moved_for_current_block(); + #endif // DIFFERENTIAL_EXTRUDER - #if ENABLED(ADAPTIVE_STEP_SMOOTHING) - oversampling_factor = 0; + // Initialize Bresenham delta errors to 1/2 + delta_error = -int32_t(step_event_count); + TERN_(HAS_ROUGH_LIN_ADVANCE, la_delta_error = delta_error); - // Decide if axis smoothing is possible - if (adaptive_step_smoothing_enabled) { - uint32_t max_rate = current_block->nominal_rate; // Get the step event rate - while (max_rate < min_step_isr_frequency) { // As long as more ISRs are possible... - max_rate <<= 1; // Try to double the rate - if (max_rate < min_step_isr_frequency) // Don't exceed the estimated ISR limit - ++oversampling_factor; // Increase the oversampling (used for left-shift) + // Calculate Bresenham dividends and divisors + advance_dividend = (current_block->steps << 1).asInt32(); + advance_divisor = step_event_count << 1; + + #if ENABLED(INPUT_SHAPING_X) + if (shaping_x.enabled) { + const int64_t steps = current_block->direction_bits.x ? int64_t(current_block->steps.x) : -int64_t(current_block->steps.x); + shaping_x.last_block_end_pos += steps; + + // If there are any remaining echos unprocessed, then direction change must + // be delayed and processed in PULSE_PREP_SHAPING. This will cause half a step + // to be missed, which will need recovering and this can be done through shaping_x.remainder. + shaping_x.forward = current_block->direction_bits.x; + if (!ShapingQueue::empty_x()) current_block->direction_bits.x = last_direction_bits.x; } - } - #endif - - // Based on the oversampling factor, do the calculations - step_event_count = current_block->step_event_count << oversampling_factor; - - #if ENABLED(DIFFERENTIAL_EXTRUDER) - - // X and E work together as a differential mechanism: - // When X and E move in the same direction they move the print carriage, - // and when X and E move in opposite directions they cause extrusion. - - // NOTE: All calculations performed per block, preserving Bresenham timing coordination - if (current_block->steps.x > 0 || current_block->steps.e > 0) { - // Calculate signed step counts based on directions - const int32_t x_signed_steps = current_block->direction_bits.x ? current_block->steps.x : -int32_t(current_block->steps.x), - e_signed_steps = current_block->direction_bits.e ? current_block->steps.e : -int32_t(current_block->steps.e); - - #if ENABLED(BALANCED_DIFFERENTIAL_EXTRUDER) - - // BALANCED DIFFERENTIAL EXTRUDER: X stepper drives one loop belt - // and E stepper drives another loop belt. Two belts mesh at the extruder - // X stepper: X - E/2 (carriage movement - half extrusion) - // E stepper: X + E/2 (carriage movement + half extrusion) - // Net extrusion: (X + E/2) - (X - E/2) = E - // (This will work great once I figure out what to do when E/2 is not integer!) - - // Split extrusion 50/50 between X and E steppers - const int32_t half_e_steps = e_signed_steps / 2; - - // X stepper: X movement - half E extrusion - const int32_t new_x_steps = x_signed_steps - half_e_steps; - current_block->steps.x = ABS(new_x_steps); - - // Update X axis direction - current_block->direction_bits.x = new_x_steps >= 0; - - // E stepper: X movement + half E extrusion - const int32_t new_e_steps = x_signed_steps + half_e_steps; - current_block->steps.e = ABS(new_e_steps); - - // Update E axis direction - current_block->direction_bits.e = new_e_steps >= 0; - - #else // !BALANCED_DIFFERENTIAL_EXTRUDER - - // SIMPLE SINGLE-LOOP DIFFERENTIAL EXTRUDER: X stepper drives the carriage as usual, - // while E stepper drives a loop belt that's meshed around the extruder. - // X stepper: X only (carriage movement) - // E stepper: X + E (carriage movement + extrusion) - // Net extrusion: (X + E) - X = E - - // Calculate net E steps (X movement + extrusion) - const int32_t net_e_steps = x_signed_steps + e_signed_steps; - - // Update the block with new E step count - current_block->steps.e = ABS(net_e_steps); - - // Update direction bit for E axis - current_block->direction_bits.e = net_e_steps >= 0; - - #endif // !BALANCED_DIFFERENTIAL_EXTRUDER - - // NOTE: DO NOT modify the step_event_count! This would change XYZ timing! - // Bresenham distributes X and E steps over the time base. - - } - - #endif // DIFFERENTIAL_EXTRUDER - - // Initialize Bresenham delta errors to 1/2 - delta_error = -int32_t(step_event_count); - TERN_(HAS_ROUGH_LIN_ADVANCE, la_delta_error = delta_error); - - // Calculate Bresenham dividends and divisors - advance_dividend = (current_block->steps << 1).asInt32(); - advance_divisor = step_event_count << 1; - - #if ENABLED(INPUT_SHAPING_X) - if (shaping_x.enabled) { - const int64_t steps = current_block->direction_bits.x ? int64_t(current_block->steps.x) : -int64_t(current_block->steps.x); - shaping_x.last_block_end_pos += steps; - - // If there are any remaining echos unprocessed, then direction change must - // be delayed and processed in PULSE_PREP_SHAPING. This will cause half a step - // to be missed, which will need recovering and this can be done through shaping_x.remainder. - shaping_x.forward = current_block->direction_bits.x; - if (!ShapingQueue::empty_x()) current_block->direction_bits.x = last_direction_bits.x; - } - #endif - - // Y and Z follow the same logic as X (but the comments aren't repeated) - #if ENABLED(INPUT_SHAPING_Y) - if (shaping_y.enabled) { - const int64_t steps = current_block->direction_bits.y ? int64_t(current_block->steps.y) : -int64_t(current_block->steps.y); - shaping_y.last_block_end_pos += steps; - shaping_y.forward = current_block->direction_bits.y; - if (!ShapingQueue::empty_y()) current_block->direction_bits.y = last_direction_bits.y; - } - #endif - - #if ENABLED(INPUT_SHAPING_Z) - if (shaping_z.enabled) { - const int64_t steps = current_block->direction_bits.z ? int64_t(current_block->steps.z) : -int64_t(current_block->steps.z); - shaping_z.last_block_end_pos += steps; - shaping_z.forward = current_block->direction_bits.z; - if (!ShapingQueue::empty_z()) current_block->direction_bits.z = last_direction_bits.z; - } - #endif - - // No step events completed so far - step_events_completed = 0; - - // Compute the acceleration and deceleration points - accelerate_before = current_block->accelerate_before << oversampling_factor; - decelerate_start = current_block->decelerate_start << oversampling_factor; - - TERN_(MIXING_EXTRUDER, mixer.stepper_setup(current_block->b_color)); - - E_TERN_(stepper_extruder = current_block->extruder); - - // Initialize the trapezoid generator from the current block. - #if HAS_ROUGH_LIN_ADVANCE - #if DISABLED(MIXING_EXTRUDER) && E_STEPPERS > 1 - // If the now active extruder wasn't in use during the last move, its pressure is most likely gone. - if (stepper_extruder != last_moved_extruder) la_advance_steps = 0; #endif - la_active = (current_block->la_advance_rate != 0); - if (la_active) { - // Apply LA scaling and discount the effect of frequency scaling - la_dividend = (advance_dividend.e << current_block->la_scaling) << oversampling_factor; - } - #endif - if ( ENABLED(DUAL_X_CARRIAGE) // TODO: Find out why this fixes "jittery" small circles - || current_block->direction_bits != last_direction_bits - || TERN(MIXING_EXTRUDER, false, stepper_extruder != last_moved_extruder) - ) { - E_TERN_(last_moved_extruder = stepper_extruder); - set_directions(current_block->direction_bits); - } - - #if ENABLED(LASER_FEATURE) - if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { // Planner controls the laser - if (planner.laser_inline.status.isSyncPower) - // If the previous block was a M3 sync power then skip the trap power init otherwise it will 0 the sync power. - planner.laser_inline.status.isSyncPower = false; // Clear the flag to process subsequent trap calc's. - else if (current_block->laser.status.isEnabled) { - #if ENABLED(LASER_POWER_TRAP) - TERN_(DEBUG_LASER_TRAP, SERIAL_ECHO_MSG("InitTrapPwr:", current_block->laser.trap_ramp_active_pwr)); - cutter.apply_power(current_block->laser.status.isPowered ? current_block->laser.trap_ramp_active_pwr : 0); - #else - TERN_(DEBUG_CUTTER_POWER, SERIAL_ECHO_MSG("InlinePwr:", current_block->laser.power)); - cutter.apply_power(current_block->laser.status.isPowered ? current_block->laser.power : 0); - #endif + // Y and Z follow the same logic as X (but the comments aren't repeated) + #if ENABLED(INPUT_SHAPING_Y) + if (shaping_y.enabled) { + const int64_t steps = current_block->direction_bits.y ? int64_t(current_block->steps.y) : -int64_t(current_block->steps.y); + shaping_y.last_block_end_pos += steps; + shaping_y.forward = current_block->direction_bits.y; + if (!ShapingQueue::empty_y()) current_block->direction_bits.y = last_direction_bits.y; } - } - #endif // LASER_FEATURE + #endif - // If the endstop is already pressed, endstop interrupts won't invoke - // endstop_triggered and the move will grind. So check here for a - // triggered endstop, which marks the block for discard on the next ISR. - endstops.update(); + #if ENABLED(INPUT_SHAPING_Z) + if (shaping_z.enabled) { + const int64_t steps = current_block->direction_bits.z ? int64_t(current_block->steps.z) : -int64_t(current_block->steps.z); + shaping_z.last_block_end_pos += steps; + shaping_z.forward = current_block->direction_bits.z; + if (!ShapingQueue::empty_z()) current_block->direction_bits.z = last_direction_bits.z; + } + #endif - #if ENABLED(Z_LATE_ENABLE) - // If delayed Z enable, enable it now. This option will severely interfere with - // timing between pulses when chaining motion between blocks, and it could lead - // to lost steps in both X and Y axis, so avoid using it unless strictly necessary!! - if (current_block->steps.z) enable_axis(Z_AXIS); - #endif + // No step events completed so far + step_events_completed = 0; - // Mark ticks_nominal as not-yet-calculated - ticks_nominal = 0; + // Compute the acceleration and deceleration points + accelerate_before = current_block->accelerate_before << oversampling_factor; + decelerate_start = current_block->decelerate_start << oversampling_factor; - #if ENABLED(S_CURVE_ACCELERATION) - // Initialize the Bézier speed curve - _calc_bezier_curve_coeffs(current_block->initial_rate, current_block->cruise_rate, current_block->acceleration_time_inverse); - // We haven't started the 2nd half of the trapezoid - bezier_2nd_half = false; - #else - // Set as deceleration point the initial rate of the block - acc_step_rate = current_block->initial_rate; - #endif + TERN_(MIXING_EXTRUDER, mixer.stepper_setup(current_block->b_color)); - // Calculate Nonlinear Extrusion fixed-point quotients - #if NONLINEAR_EXTRUSION_Q24 - ne.edividend = advance_dividend.e; - const float scale = (float(ne.edividend) / advance_divisor) * planner.mm_per_step[E_AXIS_N(current_block->extruder)]; - ne.scale_q24 = _BV32(24) * scale; - if (ne.settings.enabled && current_block->direction_bits.e && ANY_AXIS_MOVES(current_block)) { - ne.q24.A = _BV32(24) * ne.settings.coeff.A; - ne.q24.B = _BV32(24) * ne.settings.coeff.B; - ne.q24.C = _BV32(24) * ne.settings.coeff.C; - } - else { - ne.q24.A = ne.q24.B = 0; - ne.q24.C = _BV32(24); - } - #endif + E_TERN_(stepper_extruder = current_block->extruder); - // Calculate the initial timer interval - interval = calc_multistep_timer_interval(current_block->initial_rate << oversampling_factor); - // Initialize ac/deceleration time as if half the time passed. - acceleration_time = deceleration_time = interval / 2; - - // Apply Nonlinear Extrusion, if enabled - calc_nonlinear_e(current_block->initial_rate << oversampling_factor); - - #if ENABLED(LIN_ADVANCE) - #if ENABLED(SMOOTH_LIN_ADVANCE) - curr_timer_tick = 0; - #else + // Initialize the trapezoid generator from the current block. + #if HAS_ROUGH_LIN_ADVANCE + #if DISABLED(MIXING_EXTRUDER) && E_STEPPERS > 1 + // If the now active extruder wasn't in use during the last move, its pressure is most likely gone. + if (stepper_extruder != last_moved_extruder) la_advance_steps = 0; + #endif + la_active = (current_block->la_advance_rate != 0); if (la_active) { - const uint32_t la_step_rate = la_advance_steps < current_block->max_adv_steps ? current_block->la_advance_rate : 0; - la_interval = calc_timer_interval((current_block->initial_rate + la_step_rate) >> current_block->la_scaling); + // Apply LA scaling and discount the effect of frequency scaling + la_dividend = (advance_dividend.e << current_block->la_scaling) << oversampling_factor; } #endif - #endif - } - } // !current_block - // Return the interval to wait - return interval; -} + if ( ENABLED(DUAL_X_CARRIAGE) // TODO: Find out why this fixes "jittery" small circles + || current_block->direction_bits != last_direction_bits + || TERN(MIXING_EXTRUDER, false, stepper_extruder != last_moved_extruder) + ) { + E_TERN_(last_moved_extruder = stepper_extruder); + set_directions(current_block->direction_bits); + } + + #if ENABLED(LASER_FEATURE) + if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { // Planner controls the laser + if (planner.laser_inline.status.isSyncPower) + // If the previous block was a M3 sync power then skip the trap power init otherwise it will 0 the sync power. + planner.laser_inline.status.isSyncPower = false; // Clear the flag to process subsequent trap calc's. + else if (current_block->laser.status.isEnabled) { + #if ENABLED(LASER_POWER_TRAP) + TERN_(DEBUG_LASER_TRAP, SERIAL_ECHO_MSG("InitTrapPwr:", current_block->laser.trap_ramp_active_pwr)); + cutter.apply_power(current_block->laser.status.isPowered ? current_block->laser.trap_ramp_active_pwr : 0); + #else + TERN_(DEBUG_CUTTER_POWER, SERIAL_ECHO_MSG("InlinePwr:", current_block->laser.power)); + cutter.apply_power(current_block->laser.status.isPowered ? current_block->laser.power : 0); + #endif + } + } + #endif // LASER_FEATURE + + // If the endstop is already pressed, endstop interrupts won't invoke + // endstop_triggered and the move will grind. So check here for a + // triggered endstop, which marks the block for discard on the next ISR. + endstops.update(); + + #if ENABLED(Z_LATE_ENABLE) + // If delayed Z enable, enable it now. This option will severely interfere with + // timing between pulses when chaining motion between blocks, and it could lead + // to lost steps in both X and Y axis, so avoid using it unless strictly necessary!! + if (current_block->steps.z) enable_axis(Z_AXIS); + #endif + + // Mark ticks_nominal as not-yet-calculated + ticks_nominal = 0; + + #if ENABLED(S_CURVE_ACCELERATION) + // Initialize the Bézier speed curve + _calc_bezier_curve_coeffs(current_block->initial_rate, current_block->cruise_rate, current_block->acceleration_time_inverse); + // We haven't started the 2nd half of the trapezoid + bezier_2nd_half = false; + #else + // Set as deceleration point the initial rate of the block + acc_step_rate = current_block->initial_rate; + #endif + + // Calculate Nonlinear Extrusion fixed-point quotients + #if NONLINEAR_EXTRUSION_Q24 + ne.edividend = advance_dividend.e; + const float scale = (float(ne.edividend) / advance_divisor) * planner.mm_per_step[E_AXIS_N(current_block->extruder)]; + ne.scale_q24 = _BV32(24) * scale; + if (ne.settings.enabled && current_block->direction_bits.e && ANY_AXIS_MOVES(current_block)) { + ne.q24.A = _BV32(24) * ne.settings.coeff.A; + ne.q24.B = _BV32(24) * ne.settings.coeff.B; + ne.q24.C = _BV32(24) * ne.settings.coeff.C; + } + else { + ne.q24.A = ne.q24.B = 0; + ne.q24.C = _BV32(24); + } + #endif + + // Calculate the initial timer interval + interval = calc_multistep_timer_interval(current_block->initial_rate << oversampling_factor); + // Initialize ac/deceleration time as if half the time passed. + acceleration_time = deceleration_time = interval / 2; + + // Apply Nonlinear Extrusion, if enabled + calc_nonlinear_e(current_block->initial_rate << oversampling_factor); + + #if ENABLED(LIN_ADVANCE) + #if ENABLED(SMOOTH_LIN_ADVANCE) + curr_timer_tick = 0; + #else + if (la_active) { + const uint32_t la_step_rate = la_advance_steps < current_block->max_adv_steps ? current_block->la_advance_rate : 0; + la_interval = calc_timer_interval((current_block->initial_rate + la_step_rate) >> current_block->la_scaling); + } + #endif + #endif + } + } // !current_block + + // Return the interval to wait + return interval; + } + +#endif // HAS_STANDARD_MOTION #if ENABLED(LIN_ADVANCE) @@ -3229,12 +3243,12 @@ void Stepper::init() { #define _WRITE_STEP(AXIS, HIGHLOW) AXIS ##_STEP_WRITE(HIGHLOW) #define _DISABLE_AXIS(AXIS) DISABLE_AXIS_## AXIS() - #define AXIS_INIT(AXIS, PIN) \ + #define AXIS_INIT(AXIS, PAXIS) \ _STEP_INIT(AXIS); \ - _WRITE_STEP(AXIS, !_STEP_STATE(PIN)); \ + _WRITE_STEP(AXIS, !_STEP_STATE(PAXIS)); \ _DISABLE_AXIS(AXIS) - #define E_AXIS_INIT(NUM) DEFER(AXIS_INIT)(E##NUM, E) + #define E_AXIS_INIT(NUM) AXIS_INIT(_CAT(E,NUM), E) // Init Step Pins #if HAS_X_STEP diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index b04980cb1e..6e6761f842 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -400,8 +400,11 @@ class Stepper { static block_t* current_block; // A pointer to the block currently being traced - static AxisBits last_direction_bits, // The next stepping-bits to be output - axis_did_move; // Last Movement in the given direction is not null, as computed when the last movement was fetched from planner + static AxisBits last_direction_bits; // The last set of directions applied to all axes + + #if HAS_STANDARD_MOTION + static AxisBits axis_did_move; // Last Movement in the given direction is not null, as computed when the last movement was fetched from planner + #endif static bool abort_current_block; // Signals to the stepper that current block should be aborted @@ -542,11 +545,13 @@ class Stepper { // The ISR scheduler static void isr(); - // The stepper pulse ISR phase - static void pulse_phase_isr(); + #if HAS_STANDARD_MOTION + // The stepper pulse ISR phase + static void pulse_phase_isr(); - // The stepper block processing ISR phase - static hal_timer_t block_phase_isr(); + // The stepper block processing ISR phase + static hal_timer_t block_phase_isr(); + #endif #if HAS_ZV_SHAPING static void shaping_isr(); @@ -618,7 +623,7 @@ class Stepper { if (current_block->is_page()) page_manager.free_page(current_block->page_idx); #endif current_block = nullptr; - axis_did_move.reset(); + TERN_(HAS_STANDARD_MOTION, axis_did_move.reset()); planner.release_current_block(); TERN_(HAS_ROUGH_LIN_ADVANCE, la_interval = nextAdvanceISR = LA_ADV_NEVER); } @@ -629,8 +634,10 @@ class Stepper { // The direction of a single motor. A true result indicates forward or positive motion. FORCE_INLINE static bool motor_direction(const AxisEnum axis) { return last_direction_bits[axis]; } - // The last movement direction was not null on the specified axis. Note that motor direction is not necessarily the same. - FORCE_INLINE static bool axis_is_moving(const AxisEnum axis) { return axis_did_move[axis]; } + #if HAS_STANDARD_MOTION + // The last movement direction was not null on the specified axis. Note that motor direction is not necessarily the same. + FORCE_INLINE static bool axis_is_moving(const AxisEnum axis) { return axis_did_move[axis]; } + #endif // Handle a triggered endstop static void endstop_triggered(const AxisEnum axis); @@ -761,14 +768,14 @@ class Stepper { // Set the current position in steps static void _set_position(const abce_long_t &spos); - // Calculate the timing interval for the given step rate - static hal_timer_t calc_timer_interval(uint32_t step_rate); - - // Calculate timing interval and steps-per-ISR for the given step rate - static hal_timer_t calc_multistep_timer_interval(uint32_t step_rate); - - // Evaluate axis motions and set bits in axis_did_move - static void set_axis_moved_for_current_block(); + #if HAS_STANDARD_MOTION + // Calculate the timing interval for the given step rate + static hal_timer_t calc_timer_interval(uint32_t step_rate); + // Calculate timing interval and steps-per-ISR for the given step rate + static hal_timer_t calc_multistep_timer_interval(uint32_t step_rate); + // Evaluate axis motions and set bits in axis_did_move + static void set_axis_moved_for_current_block(); + #endif #if NONLINEAR_EXTRUSION_Q24 static void calc_nonlinear_e(const uint32_t step_rate); diff --git a/buildroot/tests/rambo b/buildroot/tests/rambo index 6ebb26a177..fcbe648b1d 100755 --- a/buildroot/tests/rambo +++ b/buildroot/tests/rambo @@ -73,7 +73,8 @@ opt_set MOTHERBOARD BOARD_RAMBO EXTRUDERS 0 TEMP_SENSOR_BED 1 TEMP_SENSOR_PROBE DEFAULT_MAX_ACCELERATION '{ 3000, 3000, 100 }' \ MANUAL_FEEDRATE '{ 50*60, 50*60, 4*60 }' \ AXIS_RELATIVE_MODES '{ false, false, false }' -opt_enable REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER FIX_MOUNTED_PROBE Z_SAFE_HOMING FT_MOTION FTM_SMOOTHING FTM_HOME_AND_PROBE +opt_enable REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER FIX_MOUNTED_PROBE Z_SAFE_HOMING \ + FT_MOTION FTM_SMOOTHING FTM_HOME_AND_PROBE NO_STANDARD_MOTION exec_test $1 $2 "Rambo with ZERO EXTRUDERS, heated bed, FT_MOTION" "$3" # From a889336b257c5ff8faf3724c1fa1cb1ae1bfaf65 Mon Sep 17 00:00:00 2001 From: thinkyhead Date: Thu, 11 Dec 2025 00:34:17 +0000 Subject: [PATCH 05/17] [cron] Bump distribution date (2025-12-11) --- Marlin/Version.h | 2 +- Marlin/src/inc/Version.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Marlin/Version.h b/Marlin/Version.h index f23871ff04..09e5bf7260 100644 --- a/Marlin/Version.h +++ b/Marlin/Version.h @@ -41,7 +41,7 @@ * here we define this default string as the date where the latest release * version was tagged. */ -//#define STRING_DISTRIBUTION_DATE "2025-12-09" +//#define STRING_DISTRIBUTION_DATE "2025-12-11" /** * The protocol for communication to the host. Protocol indicates communication diff --git a/Marlin/src/inc/Version.h b/Marlin/src/inc/Version.h index 63f0634f0a..bf9b229598 100644 --- a/Marlin/src/inc/Version.h +++ b/Marlin/src/inc/Version.h @@ -42,7 +42,7 @@ * version was tagged. */ #ifndef STRING_DISTRIBUTION_DATE - #define STRING_DISTRIBUTION_DATE "2025-12-09" + #define STRING_DISTRIBUTION_DATE "2025-12-11" #endif /** From 9eb82d8894bcc24c898a806bef4a185fd526f3f5 Mon Sep 17 00:00:00 2001 From: David Buezas Date: Thu, 11 Dec 2025 22:30:17 +0100 Subject: [PATCH 06/17] =?UTF-8?q?=F0=9F=90=9B=20Limit=20FTM=20reset-on-thr?= =?UTF-8?q?eshold=20E=20pos=20to=20maintain=20precision=20(#28192)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/module/ft_motion.cpp | 34 ++++++++++++++++++++++++++ Marlin/src/module/ft_motion.h | 1 + Marlin/src/module/ft_motion/stepping.h | 2 ++ 3 files changed, 37 insertions(+) diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index d8cc2e5f45..c7a558d7d2 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -349,6 +349,7 @@ bool FTMotion::plan_next_block() { if (current_block->is_sync_pos()) stepper._set_position(current_block->position); continue; } + ensure_float_precision(); #if ENABLED(POWER_LOSS_RECOVERY) recovery.info.sdpos = current_block->sdpos; @@ -400,6 +401,39 @@ bool FTMotion::plan_next_block() { } } +/** + * Ensure extruder position stays within floating point precision bounds. + * Float32 numbers have 23 bits of precision, so the minimum increment ("resolution") around a value x is: + * resolution = 2^(floor(log2(|x|)) - 23) + * By resetting at ±1'000mm (1 meter), we get a minimum resolution of ~ 0.00006mm, enough for smoothing to work well. + */ +void FTMotion::ensure_float_precision() { + constexpr float FTM_POSITION_WRAP_THRESHOLD = 1'000.0f; // (mm) Reset when position exceeds this to prevent floating point precision loss + #if HAS_EXTRUDERS + if (fabs(endPos_prevBlock.E) >= FTM_POSITION_WRAP_THRESHOLD) { + const float offset = -endPos_prevBlock.E; + endPos_prevBlock.E += offset; + + // Offset extruder shaping buffer + #if ALL(HAS_FTM_SHAPING, FTM_SHAPER_E) + for (uint32_t i = 0; i < FTM_ZMAX; ++i) shaping.E.d_zi[i] += offset; + #endif + + // Offset extruder smoothing buffer + #if ENABLED(FTM_SMOOTHING) + for (uint8_t i = 0; i < FTM_SMOOTHING_ORDER; ++i) smoothing.E.smoothing_pass[i] += offset; + #endif + + // Offset linear advance previous position + prev_traj_e += offset; + + // Offset stepper current position + const int64_t delta_steps_q48_16 = offset * planner.settings.axis_steps_per_mm[block_extruder_axis] * (1ULL << 16); + stepping.curr_steps_q48_16.E += delta_steps_q48_16; + }; + #endif +} + xyze_float_t FTMotion::calc_traj_point(const float dist) { xyze_float_t traj_coords; #define _SET_TRAJ(q) traj_coords.q = startPos.q + ratio.q * dist; diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index 991d3efcef..7af440b163 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -280,6 +280,7 @@ class FTMotion { static void fill_stepper_plan_buffer(); static xyze_float_t calc_traj_point(const float dist); static bool plan_next_block(); + static void ensure_float_precision(); }; // class FTMotion diff --git a/Marlin/src/module/ft_motion/stepping.h b/Marlin/src/module/ft_motion/stepping.h index 9274cbf369..a845e78e3d 100644 --- a/Marlin/src/module/ft_motion/stepping.h +++ b/Marlin/src/module/ft_motion/stepping.h @@ -233,9 +233,11 @@ typedef struct Stepping { FORCE_INLINE bool is_busy() { return !(is_empty() && ticks_left_in_frame_fp == 0); } + FORCE_INLINE bool is_empty() { return stepper_plan_head == stepper_plan_tail; } + FORCE_INLINE bool is_full() { return ((stepper_plan_head + 1) & FTM_BUFFER_MASK) == stepper_plan_tail; } From 4cd972c3bb2b87319159b192a87b82979820f5d9 Mon Sep 17 00:00:00 2001 From: narno2202 <130909513+narno2202@users.noreply.github.com> Date: Thu, 11 Dec 2025 22:33:35 +0100 Subject: [PATCH 07/17] =?UTF-8?q?=F0=9F=90=9B=20FT=20Motion=20-=20Set=20mo?= =?UTF-8?q?ving=5Faxis=5Fflag=20for=20each=20block=20(#28214)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/module/ft_motion.cpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index c7a558d7d2..df9eb2bd14 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -185,10 +185,7 @@ void FTMotion::loop() { } // Set busy status for use by planner.busy() - const bool oldBusy = busy; busy = stepping.is_busy(); - if (oldBusy && !busy) moving_axis_flags.reset(); - } #if HAS_FTM_SHAPING @@ -383,12 +380,8 @@ bool FTMotion::plan_next_block() { TERN_(FTM_HAS_LIN_ADVANCE, use_advance_lead = current_block->use_advance_lead); - #define _SET_MOVE_END(A) do{ \ - if (moveDist.A) { \ - moving_axis_flags.A = true; \ - axis_move_dir.A = moveDist.A > 0; \ - } \ - }while(0); + axis_move_dir = current_block->direction_bits; + #define _SET_MOVE_END(A) moving_axis_flags.A = moveDist.A ? true : false; LOGICAL_AXIS_MAP(_SET_MOVE_END); From f2ac2e7cadcab6753a81e4a4d8372badab7ead7b Mon Sep 17 00:00:00 2001 From: narno2202 <130909513+narno2202@users.noreply.github.com> Date: Thu, 11 Dec 2025 23:15:49 +0100 Subject: [PATCH 08/17] =?UTF-8?q?=F0=9F=9A=B8=20Optimize=20FTM=20menu=20co?= =?UTF-8?q?de,=20use=20some=20setters=20(#28170)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Scott Lahteine --- Marlin/src/core/types.h | 1 + Marlin/src/gcode/feature/ft_motion/M493.cpp | 22 +- Marlin/src/gcode/feature/ft_motion/M494.cpp | 11 +- Marlin/src/inc/SanityCheck.h | 2 +- Marlin/src/lcd/language/language_en.h | 1 + Marlin/src/lcd/menu/menu.h | 4 +- Marlin/src/lcd/menu/menu_item.h | 2 + Marlin/src/lcd/menu/menu_motion.cpp | 250 +++++++------------- Marlin/src/module/ft_motion.cpp | 31 ++- Marlin/src/module/ft_motion.h | 49 +++- Marlin/src/module/ft_motion/shaping.h | 6 +- Marlin/src/module/ft_motion/smoothing.cpp | 2 +- Marlin/src/module/ft_motion/smoothing.h | 10 +- Marlin/src/module/ft_motion/stepping.h | 20 +- 14 files changed, 187 insertions(+), 224 deletions(-) diff --git a/Marlin/src/core/types.h b/Marlin/src/core/types.h index c72f02558f..7008d04c53 100644 --- a/Marlin/src/core/types.h +++ b/Marlin/src/core/types.h @@ -177,6 +177,7 @@ template struct IF { typedef L type; }; #define CARTES_CODE(x,y,z,e) XYZ_CODE(x,y,z) CODE_ITEM_E(e) #define CARTES_GANG(x,y,z,e) XYZ_GANG(x,y,z) GANG_ITEM_E(e) #define CARTES_AXIS_NAMES CARTES_LIST(X,Y,Z,E) +#define CARTES_AXIS_NAMES_LC CARTES_LIST(x,y,z,e) #define CARTES_MAP(F) MAP(F, CARTES_AXIS_NAMES) #if CARTES_COUNT #define CARTES_COMMA , diff --git a/Marlin/src/gcode/feature/ft_motion/M493.cpp b/Marlin/src/gcode/feature/ft_motion/M493.cpp index 528102c868..905bddf124 100644 --- a/Marlin/src/gcode/feature/ft_motion/M493.cpp +++ b/Marlin/src/gcode/feature/ft_motion/M493.cpp @@ -215,8 +215,7 @@ void GcodeSuite::M493() { if (parser.seen('S')) { const bool active = parser.value_bool(); if (active != c.active) { - stepper.ftMotion_syncPosition(); - c.active = active; + ftMotion.toggle(); flag.report = true; } } @@ -258,21 +257,10 @@ void GcodeSuite::M493() { // Dynamic frequency mode parameter. if (parser.seenval('D')) { if (AXIS_IS_SHAPING(X) || AXIS_IS_SHAPING(Y) || AXIS_IS_SHAPING(Z) || AXIS_IS_SHAPING(E)) { - const dynFreqMode_t val = dynFreqMode_t(parser.value_byte()); - switch (val) { - #if HAS_DYNAMIC_FREQ_MM - case dynFreqMode_Z_BASED: - #endif - #if HAS_DYNAMIC_FREQ_G - case dynFreqMode_MASS_BASED: - #endif - case dynFreqMode_DISABLED: - c.dynFreqMode = val; - flag.report = true; - break; - default: - SERIAL_ECHOLN(F("?Invalid "), F("(D)ynamic Frequency Mode value.")); - break; + switch (c.setDynFreqMode(parser.value_byte())) { + case 0: break; // Same value, no update + case 1: flag.report = true; break; // New value, updated + default: SERIAL_ECHOLN(F("?Invalid "), F("(D)ynamic Frequency Mode value.")); break; } } else diff --git a/Marlin/src/gcode/feature/ft_motion/M494.cpp b/Marlin/src/gcode/feature/ft_motion/M494.cpp index 26d2b7d2b8..6f90badb49 100644 --- a/Marlin/src/gcode/feature/ft_motion/M494.cpp +++ b/Marlin/src/gcode/feature/ft_motion/M494.cpp @@ -89,12 +89,8 @@ void GcodeSuite::M494() { // Parse trajectory type parameter. if (parser.seenval('T')) { - const int val = parser.value_int(); - if (WITHIN(val, 0, 2)) { - planner.synchronize(); - ftMotion.setTrajectoryType((TrajectoryType)val); + if (ftMotion.updateTrajectoryType(TrajectoryType(parser.value_int()))) report = true; - } else SERIAL_ECHOLN(F("?Invalid "), F("(T)rajectory type value. Use 0=TRAPEZOIDAL, 1=POLY5, 2=POLY6")); } @@ -116,11 +112,8 @@ void GcodeSuite::M494() { #define SMOOTH_SET(A,N) \ if (parser.seenval(CHARIFY(A))) { \ - const float val = parser.value_float(); \ - if (WITHIN(val, 0.0f, FTM_MAX_SMOOTHING_TIME)) { \ - ftMotion.set_smoothing_time(_AXIS(A), val); \ + if (ftMotion.set_smoothing_time(_AXIS(A), parser.value_float())) \ report = true; \ - } \ else \ SERIAL_ECHOLNPGM("?Invalid ", C(N), " smoothing time (", C(CHARIFY(A)), ") value."); \ } diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index 88d177cde3..5a55671fc1 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -4483,7 +4483,7 @@ static_assert(_PLUS_TEST(3), "DEFAULT_MAX_ACCELERATION values must be positive." #error "FT_MOTION does not currently support MIXING_EXTRUDER." #endif #if !HAS_X_AXIS - static_assert(FTM_DEFAULT_SHAPER_X != ftMotionShaper_NONE, "Without any linear axes FTM_DEFAULT_SHAPER_X must be ftMotionShaper_NONE."); + static_assert(FTM_DEFAULT_SHAPER_X == ftMotionShaper_NONE, "Without any linear axes FTM_DEFAULT_SHAPER_X must be ftMotionShaper_NONE."); #endif #if HAS_DYNAMIC_FREQ_MM static_assert(FTM_DEFAULT_DYNFREQ_MODE != dynFreqMode_Z_BASED, "dynFreqMode_Z_BASED requires a Z axis."); diff --git a/Marlin/src/lcd/language/language_en.h b/Marlin/src/lcd/language/language_en.h index 5e34a47723..559b3e339d 100644 --- a/Marlin/src/lcd/language/language_en.h +++ b/Marlin/src/lcd/language/language_en.h @@ -947,6 +947,7 @@ namespace LanguageNarrow_en { LSTR MSG_FTM_VTOL_N = _UxGT("@ Vib. Level"); LSTR MSG_FTM_SMOOTH_TIME_N = _UxGT("@ Smoothing Time"); LSTR MSG_FTM_POLY6_OVERSHOOT = _UxGT("@ Poly6 Overshoot"); + LSTR MSG_FTM_CONFIGURE_AXIS_N = _UxGT("Configure @ Axis"); LSTR MSG_FTM_RESONANCE_TEST = _UxGT("Resonance Test"); LSTR MSG_FTM_RT_RUNNING = _UxGT("Res. Test Running..."); diff --git a/Marlin/src/lcd/menu/menu.h b/Marlin/src/lcd/menu/menu.h index 869b06828c..7ef7894f7b 100644 --- a/Marlin/src/lcd/menu/menu.h +++ b/Marlin/src/lcd/menu/menu.h @@ -59,8 +59,8 @@ class MenuItemBase { static const char* itemStringC; // Store an index and string for later substitution - FORCE_INLINE static void init(const int8_t ind=0, FSTR_P const fstr=nullptr) { itemIndex = ind; itemStringF = fstr; itemStringC = nullptr; } - FORCE_INLINE static void init(const int8_t ind, const char * const cstr) { itemIndex = ind; itemStringC = cstr; itemStringF = nullptr; } + FORCE_INLINE static void init(const int8_t ind=-1, FSTR_P const fstr=nullptr) { itemStringF = fstr; itemStringC = nullptr; if (ind >= 0) itemIndex = ind; } + FORCE_INLINE static void init(const int8_t ind, const char * const cstr) { itemStringC = cstr; itemStringF = nullptr; if (ind >= 0) itemIndex = ind; } // Implementation-specific: // Draw an item either selected (pre_char) or not (space) with post_char diff --git a/Marlin/src/lcd/menu/menu_item.h b/Marlin/src/lcd/menu/menu_item.h index ec680665bf..8b23ab1b9c 100644 --- a/Marlin/src/lcd/menu/menu_item.h +++ b/Marlin/src/lcd/menu/menu_item.h @@ -462,9 +462,11 @@ class MenuItem_bool : public MenuEditItemBase { #elif ENABLED(GENERIC_BACK_MENU_ITEM) #define BACK_ITEM_F(V...) MENU_ITEM_F(back, GET_TEXT_F(MSG_BACK)) #define BACK_ITEM(V...) MENU_ITEM(back, MSG_BACK) + #define BACK_ITEM_N BACK_ITEM #else #define BACK_ITEM_F(FLABEL) MENU_ITEM_F(back, FLABEL) #define BACK_ITEM(LABEL) MENU_ITEM(back, LABEL) + #define BACK_ITEM_N(N, LABEL) MENU_ITEM_N(back, N, LABEL) #endif #define ACTION_ITEM_N_S_F(N, S, FLABEL, ACTION) MENU_ITEM_N_S_F(function, N, S, FLABEL, ACTION) diff --git a/Marlin/src/lcd/menu/menu_motion.cpp b/Marlin/src/lcd/menu/menu_motion.cpp index bbd6b5beff..5b3d72a6ee 100644 --- a/Marlin/src/lcd/menu/menu_motion.cpp +++ b/Marlin/src/lcd/menu/menu_motion.cpp @@ -331,47 +331,57 @@ void menu_move() { FSTR_P get_dyn_freq_mode_name() { switch (ftMotion.cfg.dynFreqMode) { default: - case dynFreqMode_DISABLED: return GET_TEXT_F(MSG_LCD_OFF); - case dynFreqMode_Z_BASED: return GET_TEXT_F(MSG_FTM_Z_BASED); - case dynFreqMode_MASS_BASED: return GET_TEXT_F(MSG_FTM_MASS_BASED); + case dynFreqMode_DISABLED: return GET_TEXT_F(MSG_LCD_OFF); + #if HAS_DYNAMIC_FREQ_MM + case dynFreqMode_Z_BASED: return GET_TEXT_F(MSG_FTM_Z_BASED); + #endif + #if HAS_DYNAMIC_FREQ_G + case dynFreqMode_MASS_BASED: return GET_TEXT_F(MSG_FTM_MASS_BASED); + #endif } } #endif - void ftm_menu_set_shaper(ftMotionShaper_t &outShaper, const ftMotionShaper_t s) { - outShaper = s; + void ftm_menu_set_shaper(const ftMotionShaper_t s) { + ftMotion.cfg.shaper[MenuItemBase::itemIndex] = s; ftMotion.update_shaping_params(); ui.go_back(); } - #define MENU_FTM_SHAPER(A) \ - inline void menu_ftm_shaper_##A() { \ - const ftMotionShaper_t shaper = ftMotion.cfg.shaper.A; \ - START_MENU(); \ - BACK_ITEM(MSG_FIXED_TIME_MOTION); \ - if (shaper != ftMotionShaper_NONE) ACTION_ITEM(MSG_LCD_OFF, []{ ftm_menu_set_shaper(ftMotion.cfg.shaper.A, ftMotionShaper_NONE ); }); \ - if (shaper != ftMotionShaper_ZV) ACTION_ITEM(MSG_FTM_ZV, []{ ftm_menu_set_shaper(ftMotion.cfg.shaper.A, ftMotionShaper_ZV ); }); \ - if (shaper != ftMotionShaper_ZVD) ACTION_ITEM(MSG_FTM_ZVD, []{ ftm_menu_set_shaper(ftMotion.cfg.shaper.A, ftMotionShaper_ZVD ); }); \ - if (shaper != ftMotionShaper_ZVDD) ACTION_ITEM(MSG_FTM_ZVDD, []{ ftm_menu_set_shaper(ftMotion.cfg.shaper.A, ftMotionShaper_ZVDD ); }); \ - if (shaper != ftMotionShaper_ZVDDD) ACTION_ITEM(MSG_FTM_ZVDDD,[]{ ftm_menu_set_shaper(ftMotion.cfg.shaper.A, ftMotionShaper_ZVDDD ); }); \ - if (shaper != ftMotionShaper_EI) ACTION_ITEM(MSG_FTM_EI, []{ ftm_menu_set_shaper(ftMotion.cfg.shaper.A, ftMotionShaper_EI ); }); \ - if (shaper != ftMotionShaper_2HEI) ACTION_ITEM(MSG_FTM_2HEI, []{ ftm_menu_set_shaper(ftMotion.cfg.shaper.A, ftMotionShaper_2HEI ); }); \ - if (shaper != ftMotionShaper_3HEI) ACTION_ITEM(MSG_FTM_3HEI, []{ ftm_menu_set_shaper(ftMotion.cfg.shaper.A, ftMotionShaper_3HEI ); }); \ - if (shaper != ftMotionShaper_MZV) ACTION_ITEM(MSG_FTM_MZV, []{ ftm_menu_set_shaper(ftMotion.cfg.shaper.A, ftMotionShaper_MZV ); }); \ - END_MENU(); \ - } + void menu_ftm_shaper() { + const int8_t axis = MenuItemBase::itemIndex; + const ftMotionShaper_t shaper = ftMotion.cfg.shaper[axis]; + + START_MENU(); + BACK_ITEM_N(axis, MSG_FTM_CONFIGURE_AXIS_N); + + if (shaper != ftMotionShaper_NONE) ACTION_ITEM_N(axis, MSG_LCD_OFF, []{ ftm_menu_set_shaper(ftMotionShaper_NONE) ; }); + if (shaper != ftMotionShaper_ZV) ACTION_ITEM_N(axis, MSG_FTM_ZV, []{ ftm_menu_set_shaper(ftMotionShaper_ZV) ; }); + if (shaper != ftMotionShaper_ZVD) ACTION_ITEM_N(axis, MSG_FTM_ZVD, []{ ftm_menu_set_shaper(ftMotionShaper_ZVD) ; }); + if (shaper != ftMotionShaper_ZVDD) ACTION_ITEM_N(axis, MSG_FTM_ZVDD, []{ ftm_menu_set_shaper(ftMotionShaper_ZVDD) ; }); + if (shaper != ftMotionShaper_ZVDDD) ACTION_ITEM_N(axis, MSG_FTM_ZVDDD,[]{ ftm_menu_set_shaper(ftMotionShaper_ZVDDD) ; }); + if (shaper != ftMotionShaper_EI) ACTION_ITEM_N(axis, MSG_FTM_EI, []{ ftm_menu_set_shaper(ftMotionShaper_EI) ; }); + if (shaper != ftMotionShaper_2HEI) ACTION_ITEM_N(axis, MSG_FTM_2HEI, []{ ftm_menu_set_shaper(ftMotionShaper_2HEI) ; }); + if (shaper != ftMotionShaper_3HEI) ACTION_ITEM_N(axis, MSG_FTM_3HEI, []{ ftm_menu_set_shaper(ftMotionShaper_3HEI) ; }); + if (shaper != ftMotionShaper_MZV) ACTION_ITEM_N(axis, MSG_FTM_MZV, []{ ftm_menu_set_shaper(ftMotionShaper_MZV) ; }); - SHAPED_MAP(MENU_FTM_SHAPER); - #if ENABLED(FTM_POLYS) - void menu_ftm_trajectory_generator() { - const TrajectoryType current_type = ftMotion.getTrajectoryType(); - START_MENU(); - BACK_ITEM(MSG_FIXED_TIME_MOTION); - if (current_type != TrajectoryType::TRAPEZOIDAL) ACTION_ITEM(MSG_FTM_TRAPEZOIDAL, []{ planner.synchronize(); ftMotion.setTrajectoryType(TrajectoryType::TRAPEZOIDAL); ui.go_back(); }); - if (current_type != TrajectoryType::POLY5) ACTION_ITEM(MSG_FTM_POLY5, []{ planner.synchronize(); ftMotion.setTrajectoryType(TrajectoryType::POLY5); ui.go_back(); }); - if (current_type != TrajectoryType::POLY6) ACTION_ITEM(MSG_FTM_POLY6, []{ planner.synchronize(); ftMotion.setTrajectoryType(TrajectoryType::POLY6); ui.go_back(); }); END_MENU(); } + + #if ENABLED(FTM_POLYS) + + void menu_ftm_trajectory_generator() { + const TrajectoryType traj_type = ftMotion.getTrajectoryType(); + START_MENU(); + BACK_ITEM(MSG_FIXED_TIME_MOTION); + + if (traj_type != TrajectoryType::TRAPEZOIDAL) ACTION_ITEM(MSG_FTM_TRAPEZOIDAL, []{ ftMotion.updateTrajectoryType(TrajectoryType::TRAPEZOIDAL); ui.go_back(); }); + if (traj_type != TrajectoryType::POLY5) ACTION_ITEM(MSG_FTM_POLY5, []{ ftMotion.updateTrajectoryType(TrajectoryType::POLY5); ui.go_back(); }); + if (traj_type != TrajectoryType::POLY6) ACTION_ITEM(MSG_FTM_POLY6, []{ ftMotion.updateTrajectoryType(TrajectoryType::POLY6); ui.go_back(); }); + + END_MENU(); + } + #endif // FTM_POLYS #if ENABLED(FTM_RESONANCE_TEST) @@ -401,6 +411,7 @@ void menu_move() { GCODES_ITEM_N(Z_AXIS, MSG_FTM_RT_START_N, F("M495 Z S")); SUBMENU(MSG_FTM_RETRIEVE_FREQ, menu_ftm_resonance_freq); } + END_MENU(); } @@ -412,14 +423,14 @@ void menu_move() { const dynFreqMode_t dmode = ftMotion.cfg.dynFreqMode; START_MENU(); - BACK_ITEM(MSG_FIXED_TIME_MOTION); + BACK_ITEM_N(MenuItemBase::itemIndex, MSG_FTM_CONFIGURE_AXIS_N); - if (dmode != dynFreqMode_DISABLED) ACTION_ITEM(MSG_LCD_OFF, []{ ftMotion.cfg.dynFreqMode = dynFreqMode_DISABLED; ui.go_back(); }); + if (dmode != dynFreqMode_DISABLED) ACTION_ITEM(MSG_LCD_OFF, []{ (void)ftMotion.cfg.setDynFreqMode(dynFreqMode_DISABLED); ui.go_back(); }); #if HAS_DYNAMIC_FREQ_MM - if (dmode != dynFreqMode_Z_BASED) ACTION_ITEM(MSG_FTM_Z_BASED, []{ ftMotion.cfg.dynFreqMode = dynFreqMode_Z_BASED; ui.go_back(); }); + if (dmode != dynFreqMode_Z_BASED) ACTION_ITEM(MSG_FTM_Z_BASED, []{ (void)ftMotion.cfg.setDynFreqMode(dynFreqMode_Z_BASED); ui.go_back(); }); #endif #if HAS_DYNAMIC_FREQ_G - if (dmode != dynFreqMode_MASS_BASED) ACTION_ITEM(MSG_FTM_MASS_BASED, []{ ftMotion.cfg.dynFreqMode = dynFreqMode_MASS_BASED; ui.go_back(); }); + if (dmode != dynFreqMode_MASS_BASED) ACTION_ITEM(MSG_FTM_MASS_BASED, []{ (void)ftMotion.cfg.setDynFreqMode(dynFreqMode_MASS_BASED); ui.go_back(); }); #endif END_MENU(); @@ -427,75 +438,43 @@ void menu_move() { #endif // HAS_DYNAMIC_FREQ - // Suppress warning about storing a stack address in a static string pointer - #pragma GCC diagnostic push - #pragma GCC diagnostic ignored "-Wdangling-pointer" - - #if ALL(__AVR__, HAS_MARLINUI_U8GLIB) && DISABLED(OPTIMIZE_FT_MOTION_FOR_SIZE) - #define CACHE_FOR_SPEED 1 - #endif - - #if ENABLED(FTM_SMOOTHING) - #define _SMOO_MENU_ITEM(A) do{ \ - editable.decimal = c.smoothingTime.A; \ - EDIT_ITEM_FAST_N(float43, _AXIS(A), MSG_FTM_SMOOTH_TIME_N, &editable.decimal, 0.0f, FTM_MAX_SMOOTHING_TIME, []{ ftMotion.set_smoothing_time(_AXIS(A), editable.decimal); }); \ - }while(0); - #endif - - void menu_ft_motion() { - // Define stuff ahead of the menu loop + void menu_ftm_axis(const AxisEnum axis) { ft_config_t &c = ftMotion.cfg; - #ifdef __AVR__ - // Copy Flash strings to RAM for C-string substitution - // For U8G paged rendering check and skip extra string copy - #if HAS_X_AXIS - MString<20> shaper_name; - #if CACHE_FOR_SPEED - int8_t prev_a = -1; - #endif - auto _shaper_name = [&](const AxisEnum a) { - if (TERN1(CACHE_FOR_SPEED, a != prev_a)) { - TERN_(CACHE_FOR_SPEED, prev_a = a); - shaper_name = get_shaper_name(a); - } - return shaper_name; - }; - #endif - #if HAS_DYNAMIC_FREQ - MString<20> dmode; - #if CACHE_FOR_SPEED - bool got_d = false; - #endif - auto _dmode = [&]{ - if (TERN1(CACHE_FOR_SPEED, !got_d)) { - TERN_(CACHE_FOR_SPEED, got_d = true); - dmode = get_dyn_freq_mode_name(); - } - return dmode; - }; - #endif - MString<20> traj_name; - #if CACHE_FOR_SPEED - bool got_t = false; - #endif - auto _traj_name = [&]{ - if (TERN1(CACHE_FOR_SPEED, !got_t)) { - TERN_(CACHE_FOR_SPEED, got_t = true); - traj_name = ftMotion.getTrajectoryName(); - } - return traj_name; - }; - #else - auto _shaper_name = [](const AxisEnum a) { return get_shaper_name(a); }; - #if HAS_DYNAMIC_FREQ - auto _dmode = []{ return get_dyn_freq_mode_name(); }; - #endif - #if ENABLED(FTM_POLYS) - auto _traj_name = []{ return ftMotion.getTrajectoryName(); }; - #endif + START_MENU(); + BACK_ITEM(MSG_FIXED_TIME_MOTION); + + if (axis == X_AXIS || axis == Y_AXIS || TERN0(FTM_SHAPER_Z, axis == Z_AXIS) || TERN0(FTM_SHAPER_E, axis == E_AXIS)) { + SUBMENU_N_S(axis, get_shaper_name(axis), MSG_FTM_CMPN_MODE, menu_ftm_shaper); + if (IS_SHAPING(c.shaper[axis])) { + EDIT_ITEM_FAST_N(float42_52, axis, MSG_FTM_BASE_FREQ_N, &c.baseFreq[axis], FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.update_shaping_params); + EDIT_ITEM_FAST_N(float42_52, axis, MSG_FTM_ZETA_N, &c.zeta[axis], 0.0f, 1.0f, ftMotion.update_shaping_params); + if (IS_EISHAPING(c.shaper[axis])) + EDIT_ITEM_FAST_N(float42_52, axis, MSG_FTM_VTOL_N, &c.vtol[axis], 0.0f, 1.0f, ftMotion.update_shaping_params); + } + } + + #if ENABLED(FTM_SMOOTHING) + editable.decimal = c.smoothingTime[axis]; + EDIT_ITEM_FAST_N(float43, axis, MSG_FTM_SMOOTH_TIME_N, &editable.decimal, 0.0f, FTM_MAX_SMOOTHING_TIME, []{ (void)ftMotion.set_smoothing_time(AxisEnum(MenuItemBase::itemIndex), editable.decimal); }); #endif + #if HAS_DYNAMIC_FREQ + if (axis == X_AXIS || axis == Y_AXIS) { + SUBMENU_N_S(axis, get_dyn_freq_mode_name(), MSG_FTM_DYN_MODE, menu_ftm_dyn_mode); + if (c.dynFreqMode != dynFreqMode_DISABLED) + EDIT_ITEM_FAST_N(float42_52, axis, MSG_FTM_DFREQ_K_N, &c.dynFreqK[axis], 0.0f, 20.0f); + } + #endif + + END_MENU(); + } // menu_ftm_axis + + #define _FTM_AXIS_SUBMENU(A) SUBMENU_N(_AXIS(A), MSG_FTM_CONFIGURE_AXIS_N, []{ menu_ftm_axis(_AXIS(A)); }); + + void menu_ft_motion() { + ft_config_t &c = ftMotion.cfg; + START_MENU(); BACK_ITEM(MSG_MOTION); @@ -505,41 +484,23 @@ void menu_move() { #endif // Show only when FT Motion is active (or optionally always show) - if (c.active || ENABLED(FT_MOTION_NO_MENU_TOGGLE)) { + if (TERN(FT_MOTION_NO_MENU_TOGGLE, true, c.active)) { + #if ENABLED(FTM_POLYS) - SUBMENU_S(_traj_name(), MSG_FTM_TRAJECTORY, menu_ftm_trajectory_generator); + SUBMENU_S(ftMotion.getTrajectoryName(), MSG_FTM_TRAJECTORY, menu_ftm_trajectory_generator); if (ftMotion.getTrajectoryType() == TrajectoryType::POLY6) EDIT_ITEM(float42_52, MSG_FTM_POLY6_OVERSHOOT, &c.poly6_acceleration_overshoot, 1.25f, 1.875f); #endif - #define SHAPER_MENU_ITEM(A) \ - SUBMENU_N_S(_AXIS(A), _shaper_name(_AXIS(A)), MSG_FTM_CMPN_MODE, menu_ftm_shaper_##A); \ - if (AXIS_IS_SHAPING(A)) { \ - EDIT_ITEM_FAST_N(float42_52, _AXIS(A), MSG_FTM_BASE_FREQ_N, &c.baseFreq.A, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.update_shaping_params); \ - EDIT_ITEM_FAST_N(float42_52, _AXIS(A), MSG_FTM_ZETA_N, &c.zeta.A, 0.0f, 1.0f, ftMotion.update_shaping_params); \ - if (AXIS_IS_EISHAPING(A)) \ - EDIT_ITEM_FAST_N(float42_52, _AXIS(A), MSG_FTM_VTOL_N, &c.vtol.A, 0.0f, 1.0f, ftMotion.update_shaping_params); \ - } - SHAPED_MAP(SHAPER_MENU_ITEM); - - #if HAS_DYNAMIC_FREQ - SUBMENU_S(_dmode(), MSG_FTM_DYN_MODE, menu_ftm_dyn_mode); - if (c.dynFreqMode != dynFreqMode_DISABLED) { - #define _DYN_MENU_ITEM(A) EDIT_ITEM_FAST_N(float42_52, _AXIS(A), MSG_FTM_DFREQ_K_N, &c.dynFreqK.A, 0.0f, 20.0f); - SHAPED_MAP(_DYN_MENU_ITEM); - } - #endif + CARTES_MAP(_FTM_AXIS_SUBMENU); EDIT_ITEM(bool, MSG_FTM_AXIS_SYNC, &c.axis_sync_enabled); - #if ENABLED(FTM_SMOOTHING) - CARTES_MAP(_SMOO_MENU_ITEM); - #endif - #if ENABLED(FTM_RESONANCE_TEST) SUBMENU(MSG_FTM_RESONANCE_TEST, menu_ftm_resonance_test); #endif } + END_MENU(); } // menu_ft_motion @@ -553,34 +514,6 @@ void menu_move() { // Copy Flash strings to RAM for C-string substitution // For U8G paged rendering check and skip extra string copy - #if HAS_X_AXIS - #if CACHE_FOR_SPEED - int8_t prev_a = -1; - #endif - MString<20> shaper_name; - auto _shaper_name = [&](const AxisEnum a) { - if (TERN1(CACHE_FOR_SPEED, a != prev_a)) { - TERN_(CACHE_FOR_SPEED, prev_a = a); - shaper_name = get_shaper_name(a); - } - return shaper_name; - }; - #endif - - #if HAS_DYNAMIC_FREQ - #if CACHE_FOR_SPEED - bool got_d = false; - #endif - MString<20> dmode; - auto _dmode = [&]{ - if (TERN1(CACHE_FOR_SPEED, !got_d)) { - TERN_(CACHE_FOR_SPEED, got_d = true); - dmode = get_dyn_freq_mode_name(); - } - return dmode; - }; - #endif - #if ENABLED(FTM_POLYS) #if CACHE_FOR_SPEED bool got_t = false; @@ -597,10 +530,6 @@ void menu_move() { #else // !__AVR__ - auto _shaper_name = [](const AxisEnum a) { return get_shaper_name(a); }; - #if HAS_DYNAMIC_FREQ - auto _dmode = []{ return get_dyn_freq_mode_name(); }; - #endif #if ENABLED(FTM_POLYS) auto _traj_name = []{ return ftMotion.getTrajectoryName(); }; #endif @@ -616,22 +545,11 @@ void menu_move() { EDIT_ITEM(float42_52, MSG_FTM_POLY6_OVERSHOOT, &c.poly6_acceleration_overshoot, 1.25f, 1.875f); #endif - #define _CMPM_MENU_ITEM(A) SUBMENU_N_S(_AXIS(A), _shaper_name(_AXIS(A)), MSG_FTM_CMPN_MODE, menu_ftm_shaper_##A); - SHAPED_MAP(_CMPM_MENU_ITEM); - - #if HAS_DYNAMIC_FREQ - SUBMENU_S(_dmode(), MSG_FTM_DYN_MODE, menu_ftm_dyn_mode); - #endif - - #if ENABLED(FTM_SMOOTHING) - CARTES_MAP(_SMOO_MENU_ITEM); - #endif + SHAPED_MAP(_FTM_AXIS_SUBMENU); END_MENU(); } // menu_tune_ft_motion - #pragma GCC diagnostic pop - #endif // FT_MOTION_MENU void menu_motion() { diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index df9eb2bd14..e35ccba95e 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -192,7 +192,7 @@ void FTMotion::loop() { void FTMotion::update_shaping_params() { #define UPDATE_SHAPER(A) \ - shaping.A.ena = ftMotion.cfg.shaper.A != ftMotionShaper_NONE; \ + shaping.A.ena = IS_SHAPING(ftMotion.cfg.shaper.A); \ shaping.A.set_axis_shaping_A(cfg.shaper.A, cfg.zeta.A, cfg.vtol.A); \ shaping.A.set_axis_shaping_N(cfg.shaper.A, cfg.baseFreq.A, cfg.zeta.A); @@ -204,19 +204,19 @@ void FTMotion::loop() { #if ENABLED(FTM_SMOOTHING) + #include "planner.h" + void FTMotion::update_smoothing_params() { - #define _SMOOTH_PARAM(A) smoothing.A.set_smoothing_time(cfg.smoothingTime.A); + #define _SMOOTH_PARAM(A) smoothing.A.set_time(cfg.smoothingTime.A); CARTES_MAP(_SMOOTH_PARAM); smoothing.refresh_largest_delay_samples(); } - void FTMotion::set_smoothing_time(uint8_t axis, const float s_time) { - #define _SMOOTH_CASE(A) case _AXIS(A): cfg.smoothingTime.A = s_time; break; - switch (axis) { - default: - CARTES_MAP(_SMOOTH_CASE); - } + bool FTMotion::set_smoothing_time(const AxisEnum axis, const float s_time) { + if (!WITHIN(s_time, 0.0f, FTM_MAX_SMOOTHING_TIME)) return false; + cfg.smoothingTime[axis] = s_time; update_smoothing_params(); + return true; } #endif // FTM_SMOOTHING @@ -304,6 +304,21 @@ void FTMotion::init() { } } + // Update trajectory generator type from G-code or UI + bool FTMotion::updateTrajectoryType(const TrajectoryType type) { + if (type == trajectoryType) return false; + switch (type) { + default: return false; + case TrajectoryType::TRAPEZOIDAL: + case TrajectoryType::POLY5: + case TrajectoryType::POLY6: + break; + } + planner.synchronize(); + setTrajectoryType(type); + return true; + } + #endif // FTM_POLYS FSTR_P FTMotion::getTrajectoryName() { diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index 7af440b163..79dce3c8e1 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -95,15 +95,43 @@ typedef struct FTConfig { static constexpr TrajectoryType trajectory_type = TrajectoryType::TRAPEZOIDAL; #endif + #if HAS_STANDARD_MOTION + bool setActive(const bool a) { + if (a == active) return false; + stepper.ftMotion_syncPosition(); + active = a; + return true; + } + #endif + #if HAS_FTM_SHAPING + constexpr bool goodZeta(const float z) { return WITHIN(z, 0.01f, 1.0f); } constexpr bool goodVtol(const float v) { return WITHIN(v, 0.00f, 1.0f); } + #if HAS_DYNAMIC_FREQ + + uint8_t setDynFreqMode(const uint8_t m) { + if (dynFreqMode_t(m) == dynFreqMode) return 0; + switch (dynFreqMode_t(m)) { + default: return 2; + TERN_(HAS_DYNAMIC_FREQ_MM, case dynFreqMode_Z_BASED:) + TERN_(HAS_DYNAMIC_FREQ_G, case dynFreqMode_MASS_BASED:) + case dynFreqMode_DISABLED: + planner.synchronize(); + dynFreqMode = dynFreqMode_t(m); + break; + } + return 1; + } + bool modeUsesDynFreq() const { return (TERN0(HAS_DYNAMIC_FREQ_MM, dynFreqMode == dynFreqMode_Z_BASED) || TERN0(HAS_DYNAMIC_FREQ_G, dynFreqMode == dynFreqMode_MASS_BASED)); } - #endif + + #endif // HAS_DYNAMIC_FREQ + #endif // HAS_FTM_SHAPING constexpr bool goodBaseFreq(const float f) { return WITHIN(f, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2); } @@ -158,9 +186,9 @@ class FTMotion { TERN_(HAS_FTM_SHAPING, update_shaping_params()); #if ENABLED(FTM_SMOOTHING) - #define _SET_SMOOTH(A) set_smoothing_time(_AXIS(A), FTM_SMOOTHING_TIME_##A); - CARTES_MAP(_SET_SMOOTH); - #undef _SET_SMOOTH + #define _RESET_SMOOTH(A) (void)set_smoothing_time(_AXIS(A), FTM_SMOOTHING_TIME_##A); + CARTES_MAP(_RESET_SMOOTH); + #undef _RESET_SMOOTH #endif TERN_(FTM_POLYS, setTrajectoryType(TrajectoryType::FTM_TRAJECTORY_TYPE)); @@ -188,7 +216,7 @@ class FTMotion { // Refresh alpha and delay samples used by smoothing functions. static void update_smoothing_params(); // Setters for smoothingTime that update alpha and delay - static void set_smoothing_time(uint8_t axis, const float s_time); + static bool set_smoothing_time(const AxisEnum axis, const float s_time); #endif static void reset(); // Reset all states of the fixed time conversion to defaults. @@ -197,14 +225,17 @@ class FTMotion { #if ALL(FT_MOTION, HAS_STANDARD_MOTION) static bool toggle() { stepper.ftMotion_syncPosition(); - FLIP(cfg.active); + cfg.setActive(!cfg.active); update_shaping_params(); return cfg.active; } #endif // Trajectory generator selection - static void setTrajectoryType(const TrajectoryType type); + #if ENABLED(FTM_POLYS) + static void setTrajectoryType(const TrajectoryType type); + static bool updateTrajectoryType(const TrajectoryType type); + #endif static TrajectoryType getTrajectoryType() { return TERN(FTM_POLYS, trajectoryType, TrajectoryType::TRAPEZOIDAL); } static FSTR_P getTrajectoryName(); @@ -295,10 +326,10 @@ extern FTMotion ftMotion; // Use ftMotion.thing, not FTMotion::thing. bool isactive; FTMotionDisableInScope() { isactive = ftMotion.cfg.active; - ftMotion.cfg.active = false; + ftMotion.cfg.setActive(false); } ~FTMotionDisableInScope() { - ftMotion.cfg.active = isactive; + ftMotion.cfg.setActive(isactive); if (isactive) ftMotion.init(); } } FTMotionDisableInScope_t; diff --git a/Marlin/src/module/ft_motion/shaping.h b/Marlin/src/module/ft_motion/shaping.h index dacd0454fb..671bdeea88 100644 --- a/Marlin/src/module/ft_motion/shaping.h +++ b/Marlin/src/module/ft_motion/shaping.h @@ -41,8 +41,10 @@ enum dynFreqMode_t : uint8_t { dynFreqMode_MASS_BASED = 2 }; -#define AXIS_IS_SHAPING(A) TERN0(FTM_SHAPER_##A, (ftMotion.cfg.shaper.A != ftMotionShaper_NONE)) -#define AXIS_IS_EISHAPING(A) TERN0(FTM_SHAPER_##A, WITHIN(ftMotion.cfg.shaper.A, ftMotionShaper_EI, ftMotionShaper_3HEI)) +#define IS_SHAPING(S) (S != ftMotionShaper_NONE) +#define IS_EISHAPING(S) WITHIN(S, ftMotionShaper_EI, ftMotionShaper_3HEI) +#define AXIS_IS_SHAPING(A) TERN0(FTM_SHAPER_##A, IS_SHAPING(ftMotion.cfg.shaper.A)) +#define AXIS_IS_EISHAPING(A) TERN0(FTM_SHAPER_##A, IS_EISHAPING(ftMotion.cfg.shaper.A)) // Emitters for code that only cares about shaped XYZE #if HAS_FTM_SHAPING diff --git a/Marlin/src/module/ft_motion/smoothing.cpp b/Marlin/src/module/ft_motion/smoothing.cpp index f0016391b3..f40bd828aa 100644 --- a/Marlin/src/module/ft_motion/smoothing.cpp +++ b/Marlin/src/module/ft_motion/smoothing.cpp @@ -27,7 +27,7 @@ #include "smoothing.h" // Set smoothing time and recalculate alpha and delay. -void AxisSmoothing::set_smoothing_time(const float s_time) { +void AxisSmoothing::set_time(const float s_time) { if (s_time > 0.001f) { alpha = 1.0f - expf(-(FTM_TS) * (FTM_SMOOTHING_ORDER) / s_time ); delay_samples = s_time * FTM_FS; diff --git a/Marlin/src/module/ft_motion/smoothing.h b/Marlin/src/module/ft_motion/smoothing.h index 9d4043c028..3e32b2b917 100644 --- a/Marlin/src/module/ft_motion/smoothing.h +++ b/Marlin/src/module/ft_motion/smoothing.h @@ -24,7 +24,13 @@ #include "../../inc/MarlinConfig.h" typedef struct FTSmoothedAxes { - float CARTES_AXIS_NAMES; + union { + struct { float CARTES_AXIS_NAMES; }; + struct { float CARTES_AXIS_NAMES_LC; }; + float data[CARTES_COUNT]; + }; + float& operator[](const int n) { return data[n]; } + const float& operator[](const int n) const { return data[n]; } } ft_smoothed_float_t; // Smoothing data for each axis @@ -34,7 +40,7 @@ typedef struct AxisSmoothing { float smoothing_pass[FTM_SMOOTHING_ORDER] = { 0.0f }; // Last value of each of the exponential smoothing passes float alpha = 0.0f; // Pre-calculated alpha for smoothing. uint32_t delay_samples = 0; // Pre-calculated delay in samples for smoothing. - void set_smoothing_time(const float s_time); // Set smoothing time, recalculate alpha and delay. + void set_time(const float s_time); // Set smoothing time, recalculate alpha and delay. } axis_smoothing_t; typedef struct Smoothing { diff --git a/Marlin/src/module/ft_motion/stepping.h b/Marlin/src/module/ft_motion/stepping.h index a845e78e3d..474918ec75 100644 --- a/Marlin/src/module/ft_motion/stepping.h +++ b/Marlin/src/module/ft_motion/stepping.h @@ -31,18 +31,24 @@ FORCE_INLINE constexpr uint32_t a_times_b_shift_16(const uint32_t a, const uint3 return (hi * b) + ((lo * b) >> 16); } #define FTM_NEVER uint32_t(UINT16_MAX) // Reserved number to indicate "no ticks in this frame" (FRAME_TICKS_FP+1 would work too) -constexpr uint32_t FRAME_TICKS = STEPPER_TIMER_RATE / FTM_FS; // Timer ticks in a frame -static_assert(FRAME_TICKS < FTM_NEVER, "(STEPPER_TIMER_RATE / FTM_FS) must be < 2^16 (otherwise fixed-point numbers exceed uint16 vars)."); -constexpr uint32_t FTM_Q_INT = 32u - __builtin_clz(FRAME_TICKS + 1); // Bits to represent the max value (duration of a frame, +1 one for FTM_NEVER). +constexpr uint32_t FRAME_TICKS = STEPPER_TIMER_RATE / FTM_FS; // Timer ticks per frame (by default, 1kHz) +constexpr uint32_t TICKS_BITS = __builtin_clzl(FRAME_TICKS + 1UL); // Bits to represent the max value (duration of a frame, +1 one for FTM_NEVER). +constexpr uint32_t FTM_Q_INT = 32u - TICKS_BITS; // Bits remaining // "clz" counts leading zeroes. -constexpr uint32_t FTM_Q = 16 - FTM_Q_INT; // uint16 interval fractional bits. +constexpr uint32_t FTM_Q = 16u - FTM_Q_INT; // uint16 interval fractional bits. // Intervals buffer has fixed point numbers with the point on this position +static_assert(FRAME_TICKS < FTM_NEVER, "(STEPPER_TIMER_RATE / FTM_FS) must be < " STRINGIFY(FTM_NEVER) " to fit 16-bit fixed-point numbers."); +static_assert(FRAME_TICKS != 2000 || FTM_Q_INT == 11, "FTM_Q_INT should be 11"); +static_assert(FRAME_TICKS != 2000 || FTM_Q == 5, "FTM_Q should be 5"); +static_assert(FRAME_TICKS != 25000 || FTM_Q_INT == 15, "FTM_Q_INT should be 15"); +static_assert(FRAME_TICKS != 25000 || FTM_Q == 1, "FTM_Q should be 1"); + // The _FP and _fp suffixes mean the number is in fixed point format with the point at the FTM_Q position. // See: https://en.wikipedia.org/wiki/Fixed-point_arithmetic -// E.g number_fp = number << FTM_Q -// number == (number_fp >> FTM_Q) -constexpr uint32_t ONE_FP = 1 << FTM_Q; // Number 1 in fixed point format +// e.g., number_fp = number << FTM_Q +// number == (number_fp >> FTM_Q) +constexpr uint32_t ONE_FP = 1UL << FTM_Q; // Number 1 in fixed point format constexpr uint32_t FP_FLOOR_MASK = ~(ONE_FP - 1); // Bit mask to do FLOOR in fixed point constexpr uint32_t FRAME_TICKS_FP = FRAME_TICKS << FTM_Q; // Ticks in a frame in fixed point From 7816f5dc46dc2b6a5cf480c14ae072921a25259e Mon Sep 17 00:00:00 2001 From: thinkyhead Date: Fri, 12 Dec 2025 00:34:09 +0000 Subject: [PATCH 09/17] [cron] Bump distribution date (2025-12-12) --- Marlin/Version.h | 2 +- Marlin/src/inc/Version.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Marlin/Version.h b/Marlin/Version.h index 09e5bf7260..e2886c1f43 100644 --- a/Marlin/Version.h +++ b/Marlin/Version.h @@ -41,7 +41,7 @@ * here we define this default string as the date where the latest release * version was tagged. */ -//#define STRING_DISTRIBUTION_DATE "2025-12-11" +//#define STRING_DISTRIBUTION_DATE "2025-12-12" /** * The protocol for communication to the host. Protocol indicates communication diff --git a/Marlin/src/inc/Version.h b/Marlin/src/inc/Version.h index bf9b229598..f8678dd2cb 100644 --- a/Marlin/src/inc/Version.h +++ b/Marlin/src/inc/Version.h @@ -42,7 +42,7 @@ * version was tagged. */ #ifndef STRING_DISTRIBUTION_DATE - #define STRING_DISTRIBUTION_DATE "2025-12-11" + #define STRING_DISTRIBUTION_DATE "2025-12-12" #endif /** From e434fd0aefa87e1027d40019b6a71d19b68d132b Mon Sep 17 00:00:00 2001 From: Harald Wagener Date: Fri, 12 Dec 2025 03:50:13 +0100 Subject: [PATCH 10/17] =?UTF-8?q?=E2=9C=A8=20FTM=5FSHAPER=5F*=20options=20?= =?UTF-8?q?(#28217)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Scott Lahteine --- Marlin/Configuration_adv.h | 10 ++ Marlin/src/gcode/feature/ft_motion/M493.cpp | 95 ++++++----- Marlin/src/gcode/feature/ft_motion/M494.cpp | 7 +- Marlin/src/inc/Conditionals-4-adv.h | 3 + Marlin/src/inc/Conditionals-5-post.h | 17 +- Marlin/src/inc/SanityCheck.h | 5 +- Marlin/src/lcd/menu/menu_motion.cpp | 15 +- Marlin/src/module/ft_motion.cpp | 2 +- Marlin/src/module/ft_motion.h | 9 +- Marlin/src/module/ft_motion/shaping.cpp | 166 +++++++++++--------- Marlin/src/module/ft_motion/shaping.h | 10 +- Marlin/src/module/stepper.cpp | 14 +- buildroot/tests/rambo | 1 + 13 files changed, 211 insertions(+), 143 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index f217923520..b8d7ed694d 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1171,6 +1171,16 @@ #define FTM_DEFAULT_DYNFREQ_MODE dynFreqMode_DISABLED // Default mode of dynamic frequency calculation. (DISABLED, Z_BASED, MASS_BASED) #endif + // Disable unused shapers if you need more free space + #define FTM_SHAPER_ZV + #define FTM_SHAPER_ZVD + #define FTM_SHAPER_ZVDD + #define FTM_SHAPER_ZVDDD + #define FTM_SHAPER_EI + #define FTM_SHAPER_2HEI + #define FTM_SHAPER_3HEI + #define FTM_SHAPER_MZV + #define FTM_DEFAULT_SHAPER_X ftMotionShaper_NONE // Default shaper mode on X axis (NONE, ZV, ZVD, ZVDD, ZVDDD, EI, 2HEI, 3HEI, MZV) #define FTM_SHAPING_DEFAULT_FREQ_X 37.0f // (Hz) Default peak frequency used by input shapers #define FTM_SHAPING_ZETA_X 0.1f // Zeta used by input shapers for X axis diff --git a/Marlin/src/gcode/feature/ft_motion/M493.cpp b/Marlin/src/gcode/feature/ft_motion/M493.cpp index 905bddf124..85e2fc9d3b 100644 --- a/Marlin/src/gcode/feature/ft_motion/M493.cpp +++ b/Marlin/src/gcode/feature/ft_motion/M493.cpp @@ -136,13 +136,18 @@ void GcodeSuite::M493_report(const bool forReplay/*=true*/) { #else #define F_REPORT(A) #endif + #if HAS_FTM_EI_SHAPING + #define Q_REPORT(A) , F(" Q"), c.vtol.A + #else + #define Q_REPORT(A) + #endif #define _REPORT_M493_AXIS(A) \ SERIAL_ECHOLN(F(" M493 "), C(AXIS_CHAR(_AXIS(A))) \ , F(" C"), c.shaper.A \ , F(" A"), c.baseFreq.A \ F_REPORT(A) \ , F(" I"), c.zeta.A \ - , F(" Q"), c.vtol.A \ + Q_REPORT(A) \ ); // Shaper type for each axis SHAPED_MAP(_REPORT_M493_AXIS); @@ -293,12 +298,14 @@ void GcodeSuite::M493() { if (seenI && !goodZeta) SERIAL_ECHOLN(F("?Invalid "), F("(I) Zeta value. (0.01-1.0)")); // Zeta out of range - // Vibration Tolerance parameter - const bool seenQ = parser.seenval('Q'); - const float vtolVal = seenQ ? parser.value_float() : 0.0f; - const bool goodVtol = seenQ && c.goodVtol(vtolVal); - if (seenQ && !goodVtol) - SERIAL_ECHOLN(F("?Invalid "), F("(Q) Vibration Tolerance value. (0.0-1.0)")); // VTol out of range + #if HAS_FTM_EI_SHAPING + // Vibration Tolerance parameter + const bool seenQ = parser.seenval('Q'); + const float vtolVal = seenQ ? parser.value_float() : 0.0f; + const bool goodVtol = seenQ && c.goodVtol(vtolVal); + if (seenQ && !goodVtol) + SERIAL_ECHOLN(F("?Invalid "), F("(Q) Vibration Tolerance value. (0.0-1.0)")); // VTol out of range + #endif const bool apply_xy = !parser.seen("XYZE"); @@ -339,17 +346,19 @@ void GcodeSuite::M493() { SERIAL_ECHOLNPGM("?Wrong mode for ", C(STEPPER_A_NAME), " (I) zeta parameter."); } - // Parse X vtol parameter - if (seenQ) { - if (AXIS_IS_EISHAPING(X)) { - if (goodVtol) { - c.vtol.x = vtolVal; - flag.update = true; + #if HAS_FTM_EI_SHAPING + // Parse X vtol parameter + if (seenQ) { + if (AXIS_IS_EISHAPING(X)) { + if (goodVtol) { + c.vtol.x = vtolVal; + flag.update = true; + } } + else + SERIAL_ECHOLNPGM("?Wrong mode for ", C(STEPPER_A_NAME), " (Q) vtol parameter."); } - else - SERIAL_ECHOLNPGM("?Wrong mode for ", C(STEPPER_A_NAME), " (Q) vtol parameter."); - } + #endif } #endif // HAS_X_AXIS @@ -391,16 +400,18 @@ void GcodeSuite::M493() { } // Parse Y vtol parameter - if (seenQ) { - if (AXIS_IS_EISHAPING(Y)) { - if (goodVtol) { - c.vtol.y = vtolVal; - flag.update = true; + #if HAS_FTM_EI_SHAPING + if (seenQ) { + if (AXIS_IS_EISHAPING(Y)) { + if (goodVtol) { + c.vtol.y = vtolVal; + flag.update = true; + } } + else + SERIAL_ECHOLNPGM("?Wrong mode for ", C(STEPPER_B_NAME), " (Q) vtol parameter."); } - else - SERIAL_ECHOLNPGM("?Wrong mode for ", C(STEPPER_B_NAME), " (Q) vtol parameter."); - } + #endif } #endif // HAS_Y_AXIS @@ -442,16 +453,18 @@ void GcodeSuite::M493() { } // Parse Z vtol parameter - if (seenQ) { - if (AXIS_IS_EISHAPING(Z)) { - if (goodVtol) { - c.vtol.z = vtolVal; - flag.update = true; + #if HAS_FTM_EI_SHAPING + if (seenQ) { + if (AXIS_IS_EISHAPING(Z)) { + if (goodVtol) { + c.vtol.z = vtolVal; + flag.update = true; + } } + else + SERIAL_ECHOLNPGM("?Wrong mode for ", C(STEPPER_C_NAME), " (Q) vtol parameter."); } - else - SERIAL_ECHOLNPGM("?Wrong mode for ", C(STEPPER_C_NAME), " (Q) vtol parameter."); - } + #endif } #endif // FTM_SHAPER_Z @@ -493,16 +506,18 @@ void GcodeSuite::M493() { } // Parse E vtol parameter - if (seenQ) { - if (AXIS_IS_EISHAPING(E)) { - if (goodVtol) { - c.vtol.e = vtolVal; - flag.update = true; + #if HAS_FTM_EI_SHAPING + if (seenQ) { + if (AXIS_IS_EISHAPING(E)) { + if (goodVtol) { + c.vtol.e = vtolVal; + flag.update = true; + } } + else + SERIAL_ECHOLNPGM("?Wrong mode for ", C('E'), " (Q) vtol parameter."); } - else - SERIAL_ECHOLNPGM("?Wrong mode for ", C('E'), " (Q) vtol parameter."); - } + #endif } #endif // FTM_SHAPER_E diff --git a/Marlin/src/gcode/feature/ft_motion/M494.cpp b/Marlin/src/gcode/feature/ft_motion/M494.cpp index 6f90badb49..212a69c0a2 100644 --- a/Marlin/src/gcode/feature/ft_motion/M494.cpp +++ b/Marlin/src/gcode/feature/ft_motion/M494.cpp @@ -29,13 +29,12 @@ #include "../../../module/planner.h" void say_ftm_settings() { - #if ENABLED(FTM_POLYS) - SERIAL_ECHOLN(F(" Trajectory: "), ftMotion.getTrajectoryName(), C('('), (uint8_t)ftMotion.getTrajectoryType(), C(')')); + #if ANY(FTM_POLYS, FTM_SMOOTHING) + const ft_config_t &c = ftMotion.cfg; #endif - const ft_config_t &c = ftMotion.cfg; - #if ENABLED(FTM_POLYS) + SERIAL_ECHOLN(F(" Trajectory: "), ftMotion.getTrajectoryName(), C('('), (uint8_t)ftMotion.getTrajectoryType(), C(')')); if (ftMotion.getTrajectoryType() == TrajectoryType::POLY6) SERIAL_ECHOLNPGM(" Poly6 Overshoot: ", p_float_t(c.poly6_acceleration_overshoot, 3)); #endif diff --git a/Marlin/src/inc/Conditionals-4-adv.h b/Marlin/src/inc/Conditionals-4-adv.h index f92ba2c079..b363ccde11 100644 --- a/Marlin/src/inc/Conditionals-4-adv.h +++ b/Marlin/src/inc/Conditionals-4-adv.h @@ -375,6 +375,9 @@ #undef MULTISTEPPING_LIMIT #define MULTISTEPPING_LIMIT 1 #endif + #if ANY(FTM_SHAPER_EI, FTM_SHAPER_2HEI, FTM_SHAPER_3HEI) + #define HAS_FTM_EI_SHAPING 1 + #endif #endif #if DISABLED(NO_STANDARD_MOTION) #define HAS_STANDARD_MOTION 1 diff --git a/Marlin/src/inc/Conditionals-5-post.h b/Marlin/src/inc/Conditionals-5-post.h index e2d2952e1b..0fa53dbf3e 100644 --- a/Marlin/src/inc/Conditionals-5-post.h +++ b/Marlin/src/inc/Conditionals-5-post.h @@ -3690,13 +3690,16 @@ #define FTM_TS (1.0f / FTM_FS) // (s) Time step for trajectory generation. (Reciprocal of FTM_FS) #define FTM_RATIO (FTM_FS / FTM_MIN_SHAPE_FREQ) // Factor for use in FTM_ZMAX. DON'T CHANGE. #define FTM_SMOOTH_MAX_I uint32_t(TERN0(FTM_SMOOTHING, CEIL(FTM_FS * FTM_MAX_SMOOTHING_TIME))) // Max delays for smoothing - #define FTM_ZMAX (FTM_RATIO * 2 + FTM_SMOOTH_MAX_I) // Maximum delays for shaping functions (even numbers only!) - // Calculate as: - // ZV : FTM_RATIO / 2 - // ZVD, MZV : FTM_RATIO - // 2HEI : FTM_RATIO * 3 / 2 - // 3HEI : FTM_RATIO * 2 - #define FTM_SMOOTHING_ORDER 5 // 3 to 5 is closest to gaussian + + // Maximum delays for shaping functions (even numbers only!) + #define FTM_ZMAX (TERN(HAS_FTM_EI_SHAPING, 2, 1) * FTM_RATIO + FTM_SMOOTH_MAX_I) + + #define FTM_SMOOTHING_ORDER 5 // 3 to 5 is closest to Gaussian + // Calculate as: + // ZV : FTM_RATIO / 2 + // ZVD, MZV : FTM_RATIO + // 2HEI : FTM_RATIO * 3 / 2 + // 3HEI : FTM_RATIO * 2 #ifndef FTM_BUFFER_SIZE #define FTM_BUFFER_SIZE 128 #endif diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index 5a55671fc1..694f88f3ee 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -4519,7 +4519,10 @@ static_assert(_PLUS_TEST(3), "DEFAULT_MAX_ACCELERATION values must be positive." #error "Z_LATE_ENABLE is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it." #endif #endif -#endif + #if HAS_FTM_SHAPING && NONE(FTM_SHAPER_ZV, FTM_SHAPER_ZVD, FTM_SHAPER_ZVDD, FTM_SHAPER_ZVDDD, FTM_SHAPER_EI, FTM_SHAPER_2HEI, FTM_SHAPER_3HEI, FTM_SHAPER_MZV) + #error "For FT_MOTION at least one FTM_SHAPER_* type must be enabled." + #endif +#endif // FT_MOTION // Multi-Stepping Limit static_assert(WITHIN(MULTISTEPPING_LIMIT, 1, 128) && IS_POWER_OF_2(MULTISTEPPING_LIMIT), "MULTISTEPPING_LIMIT must be 1, 2, 4, 8, 16, 32, 64, or 128."); diff --git a/Marlin/src/lcd/menu/menu_motion.cpp b/Marlin/src/lcd/menu/menu_motion.cpp index 5b3d72a6ee..106646430c 100644 --- a/Marlin/src/lcd/menu/menu_motion.cpp +++ b/Marlin/src/lcd/menu/menu_motion.cpp @@ -444,13 +444,20 @@ void menu_move() { START_MENU(); BACK_ITEM(MSG_FIXED_TIME_MOTION); - if (axis == X_AXIS || axis == Y_AXIS || TERN0(FTM_SHAPER_Z, axis == Z_AXIS) || TERN0(FTM_SHAPER_E, axis == E_AXIS)) { + #if HAS_FTM_EI_SHAPING + #define EISHAPER_MENU_ITEM(A) \ + if (AXIS_IS_EISHAPING(A)) \ + EDIT_ITEM_FAST_N(float42_52, axis, MSG_FTM_VTOL_N, &c.vtol[axis], 0.0f, 1.0f, ftMotion.update_shaping_params); + #else + #define EISHAPER_MENU_ITEM(A) NOOP + #endif + + if (false SHAPED_GANG(|| axis == X_AXIS, || axis == Y_AXIS, || axis == Z_AXIS, || axis == E_AXIS)) { SUBMENU_N_S(axis, get_shaper_name(axis), MSG_FTM_CMPN_MODE, menu_ftm_shaper); - if (IS_SHAPING(c.shaper[axis])) { + if (AXIS_IS_SHAPING(axis)) { EDIT_ITEM_FAST_N(float42_52, axis, MSG_FTM_BASE_FREQ_N, &c.baseFreq[axis], FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.update_shaping_params); EDIT_ITEM_FAST_N(float42_52, axis, MSG_FTM_ZETA_N, &c.zeta[axis], 0.0f, 1.0f, ftMotion.update_shaping_params); - if (IS_EISHAPING(c.shaper[axis])) - EDIT_ITEM_FAST_N(float42_52, axis, MSG_FTM_VTOL_N, &c.vtol[axis], 0.0f, 1.0f, ftMotion.update_shaping_params); + EISHAPER_MENU_ITEM(axis); } } diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index e35ccba95e..c4c2e5f302 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -193,7 +193,7 @@ void FTMotion::loop() { void FTMotion::update_shaping_params() { #define UPDATE_SHAPER(A) \ shaping.A.ena = IS_SHAPING(ftMotion.cfg.shaper.A); \ - shaping.A.set_axis_shaping_A(cfg.shaper.A, cfg.zeta.A, cfg.vtol.A); \ + shaping.A.set_axis_shaping_A(cfg.shaper.A, cfg.zeta.A OPTARG(HAS_FTM_EI_SHAPING, cfg.vtol.A)); \ shaping.A.set_axis_shaping_N(cfg.shaper.A, cfg.baseFreq.A, cfg.zeta.A); SHAPED_MAP(UPDATE_SHAPER); diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index 79dce3c8e1..bf6a38a88f 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -72,8 +72,11 @@ typedef struct FTConfig { SHAPED_ARRAY(FTM_SHAPING_DEFAULT_FREQ_X, FTM_SHAPING_DEFAULT_FREQ_Y, FTM_SHAPING_DEFAULT_FREQ_Z, FTM_SHAPING_DEFAULT_FREQ_E); ft_shaped_float_t zeta = // Damping factor SHAPED_ARRAY(FTM_SHAPING_ZETA_X, FTM_SHAPING_ZETA_Y, FTM_SHAPING_ZETA_Z, FTM_SHAPING_ZETA_E); - ft_shaped_float_t vtol = // Vibration Level - SHAPED_ARRAY(FTM_SHAPING_V_TOL_X, FTM_SHAPING_V_TOL_Y, FTM_SHAPING_V_TOL_Z, FTM_SHAPING_V_TOL_E); + + #if HAS_FTM_EI_SHAPING + ft_shaped_float_t vtol = // Vibration Level + SHAPED_ARRAY(FTM_SHAPING_V_TOL_X, FTM_SHAPING_V_TOL_Y, FTM_SHAPING_V_TOL_Z, FTM_SHAPING_V_TOL_E); + #endif #if HAS_DYNAMIC_FREQ dynFreqMode_t dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; // Dynamic frequency mode configuration. @@ -147,7 +150,7 @@ typedef struct FTConfig { shaper.A = FTM_DEFAULT_SHAPER_##A; \ baseFreq.A = FTM_SHAPING_DEFAULT_FREQ_##A; \ zeta.A = FTM_SHAPING_ZETA_##A; \ - vtol.A = FTM_SHAPING_V_TOL_##A; \ + TERN_(HAS_FTM_EI_SHAPING, vtol.A = FTM_SHAPING_V_TOL_##A); \ }while(0); SHAPED_MAP(_SET_CFG_DEFAULTS); diff --git a/Marlin/src/module/ft_motion/shaping.cpp b/Marlin/src/module/ft_motion/shaping.cpp index a077ce99b8..f4f00a8589 100644 --- a/Marlin/src/module/ft_motion/shaping.cpp +++ b/Marlin/src/module/ft_motion/shaping.cpp @@ -27,7 +27,11 @@ #include "shaping.h" // Refresh the gains used by shaping functions. -void AxisShaping::set_axis_shaping_A(const ftMotionShaper_t shaper, const float zeta, const float vtol) { +void AxisShaping::set_axis_shaping_A( + const ftMotionShaper_t shaper, + const float zeta + OPTARG(HAS_FTM_EI_SHAPING, const float vtol) +) { const float K = exp(-zeta * M_PI / sqrt(1.f - sq(zeta))), K2 = sq(K), @@ -36,79 +40,95 @@ void AxisShaping::set_axis_shaping_A(const ftMotionShaper_t shaper, const float switch (shaper) { - case ftMotionShaper_ZV: - max_i = 1U; - Ai[0] = 1.0f / (1.0f + K); - Ai[1] = Ai[0] * K; + #if ENABLED(FTM_SHAPER_ZV) + case ftMotionShaper_ZV: + max_i = 1U; + Ai[0] = 1.0f / (1.0f + K); + Ai[1] = Ai[0] * K; + break; + #endif + + #if ENABLED(FTM_SHAPER_ZVD) + case ftMotionShaper_ZVD: + max_i = 2U; + Ai[0] = 1.0f / (1.0f + 2.0f * K + K2); + Ai[1] = Ai[0] * 2.0f * K; + Ai[2] = Ai[0] * K2; + break; + #endif + + #if ENABLED(FTM_SHAPER_ZVDD) + case ftMotionShaper_ZVDD: + max_i = 3U; + Ai[0] = 1.0f / (1.0f + 3.0f * K + 3.0f * K2 + K3); + Ai[1] = Ai[0] * 3.0f * K; + Ai[2] = Ai[0] * 3.0f * K2; + Ai[3] = Ai[0] * K3; + break; + #endif + + #if ENABLED(FTM_SHAPER_ZVDDD) + case ftMotionShaper_ZVDDD: + max_i = 4U; + Ai[0] = 1.0f / (1.0f + 4.0f * K + 6.0f * K2 + 4.0f * K3 + K4); + Ai[1] = Ai[0] * 4.0f * K; + Ai[2] = Ai[0] * 6.0f * K2; + Ai[3] = Ai[0] * 4.0f * K3; + Ai[4] = Ai[0] * K4; + break; + #endif + + #if ENABLED(FTM_SHAPER_EI) + case ftMotionShaper_EI: { + max_i = 2U; + Ai[0] = 0.25f * (1.0f + vtol); + Ai[1] = 0.50f * (1.0f - vtol) * K; + Ai[2] = Ai[0] * K2; + + const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2]); + for (uint32_t i = 0; i < 3U; i++) Ai[i] *= adj; + } break; + #endif + + #if ENABLED(FTM_SHAPER_H2EI) + case ftMotionShaper_2HEI: { + max_i = 3U; + const float vtolx2 = sq(vtol); + const float X = POW(vtolx2 * (sqrt(1.0f - vtolx2) + 1.0f), 1.0f / 3.0f); + Ai[0] = (3.0f * sq(X) + 2.0f * X + 3.0f * vtolx2) / (16.0f * X); + Ai[1] = (0.5f - Ai[0]) * K; + Ai[2] = Ai[1] * K; + Ai[3] = Ai[0] * K3; + + const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2] + Ai[3]); + for (uint32_t i = 0; i < 4U; i++) Ai[i] *= adj; + } break; + #endif + + #if ENABLED(FTM_SHAPER_3HEI) + case ftMotionShaper_3HEI: { + max_i = 4U; + Ai[0] = 0.0625f * ( 1.0f + 3.0f * vtol + 2.0f * sqrt( 2.0f * ( vtol + 1.0f ) * vtol ) ); + Ai[1] = 0.25f * ( 1.0f - vtol ) * K; + Ai[2] = ( 0.5f * ( 1.0f + vtol ) - 2.0f * Ai[0] ) * K2; + Ai[3] = Ai[1] * K2; + Ai[4] = Ai[0] * K4; + + const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2] + Ai[3] + Ai[4]); + for (uint32_t i = 0; i < 5U; i++) Ai[i] *= adj; + } break; + #endif + + #if ENABLED(FTM_SHAPER_MZV) + case ftMotionShaper_MZV: { + max_i = 2U; + const float Bx = 1.4142135623730950488016887242097f * K; + Ai[0] = 1.0f / (1.0f + Bx + K2); + Ai[1] = Ai[0] * Bx; + Ai[2] = Ai[0] * K2; + } break; - - case ftMotionShaper_ZVD: - max_i = 2U; - Ai[0] = 1.0f / (1.0f + 2.0f * K + K2); - Ai[1] = Ai[0] * 2.0f * K; - Ai[2] = Ai[0] * K2; - break; - - case ftMotionShaper_ZVDD: - max_i = 3U; - Ai[0] = 1.0f / (1.0f + 3.0f * K + 3.0f * K2 + K3); - Ai[1] = Ai[0] * 3.0f * K; - Ai[2] = Ai[0] * 3.0f * K2; - Ai[3] = Ai[0] * K3; - break; - - case ftMotionShaper_ZVDDD: - max_i = 4U; - Ai[0] = 1.0f / (1.0f + 4.0f * K + 6.0f * K2 + 4.0f * K3 + K4); - Ai[1] = Ai[0] * 4.0f * K; - Ai[2] = Ai[0] * 6.0f * K2; - Ai[3] = Ai[0] * 4.0f * K3; - Ai[4] = Ai[0] * K4; - break; - - case ftMotionShaper_EI: { - max_i = 2U; - Ai[0] = 0.25f * (1.0f + vtol); - Ai[1] = 0.50f * (1.0f - vtol) * K; - Ai[2] = Ai[0] * K2; - - const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2]); - for (uint32_t i = 0; i < 3U; i++) Ai[i] *= adj; - } break; - - case ftMotionShaper_2HEI: { - max_i = 3U; - const float vtolx2 = sq(vtol); - const float X = POW(vtolx2 * (sqrt(1.0f - vtolx2) + 1.0f), 1.0f / 3.0f); - Ai[0] = (3.0f * sq(X) + 2.0f * X + 3.0f * vtolx2) / (16.0f * X); - Ai[1] = (0.5f - Ai[0]) * K; - Ai[2] = Ai[1] * K; - Ai[3] = Ai[0] * K3; - - const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2] + Ai[3]); - for (uint32_t i = 0; i < 4U; i++) Ai[i] *= adj; - } break; - - case ftMotionShaper_3HEI: { - max_i = 4U; - Ai[0] = 0.0625f * ( 1.0f + 3.0f * vtol + 2.0f * sqrt( 2.0f * ( vtol + 1.0f ) * vtol ) ); - Ai[1] = 0.25f * ( 1.0f - vtol ) * K; - Ai[2] = ( 0.5f * ( 1.0f + vtol ) - 2.0f * Ai[0] ) * K2; - Ai[3] = Ai[1] * K2; - Ai[4] = Ai[0] * K4; - - const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2] + Ai[3] + Ai[4]); - for (uint32_t i = 0; i < 5U; i++) Ai[i] *= adj; - } break; - - case ftMotionShaper_MZV: { - max_i = 2U; - const float Bx = 1.4142135623730950488016887242097f * K; - Ai[0] = 1.0f / (1.0f + Bx + K2); - Ai[1] = Ai[0] * Bx; - Ai[2] = Ai[0] * K2; - } - break; + #endif case ftMotionShaper_NONE: max_i = 0; diff --git a/Marlin/src/module/ft_motion/shaping.h b/Marlin/src/module/ft_motion/shaping.h index 671bdeea88..f207b3d2b7 100644 --- a/Marlin/src/module/ft_motion/shaping.h +++ b/Marlin/src/module/ft_motion/shaping.h @@ -41,8 +41,8 @@ enum dynFreqMode_t : uint8_t { dynFreqMode_MASS_BASED = 2 }; -#define IS_SHAPING(S) (S != ftMotionShaper_NONE) -#define IS_EISHAPING(S) WITHIN(S, ftMotionShaper_EI, ftMotionShaper_3HEI) +#define IS_SHAPING(S) ((S) != ftMotionShaper_NONE) +#define IS_EISHAPING(S) TERN0(HAS_FTM_EI_SHAPING, WITHIN(S, ftMotionShaper_EI, ftMotionShaper_3HEI)) #define AXIS_IS_SHAPING(A) TERN0(FTM_SHAPER_##A, IS_SHAPING(ftMotion.cfg.shaper.A)) #define AXIS_IS_EISHAPING(A) TERN0(FTM_SHAPER_##A, IS_EISHAPING(ftMotion.cfg.shaper.A)) @@ -104,7 +104,11 @@ typedef struct AxisShaping { void set_axis_shaping_N(const ftMotionShaper_t shaper, const float f, const float zeta); // Set the indices (per pulse delays) used by shaping functions - void set_axis_shaping_A(const ftMotionShaper_t shaper, const float zeta, const float vtol); + void set_axis_shaping_A( + const ftMotionShaper_t shaper, + const float zeta + OPTARG(HAS_FTM_EI_SHAPING, const float vtol) + ); } axis_shaping_t; diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 947a74f96a..86ad562e8c 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1569,10 +1569,14 @@ HAL_STEP_TIMER_ISR() { void Stepper::isr() { - static hal_timer_t nextMainISR = 0; // Interval until the next main Stepper Pulse phase (0 = Now) - + #if HAS_STANDARD_MOTION + static hal_timer_t nextMainISR = 0; // Interval until the next main Stepper Pulse phase (0 = Now) + #endif #if ENABLED(SMOOTH_LIN_ADVANCE) - static hal_timer_t smoothLinAdvISR = 0; + static hal_timer_t smoothLinAdvISR = 0; // Interval until the next Smooth Linear Advance phase (0 = Now) + #endif + #if ENABLED(FT_MOTION) + static uint32_t ftMotion_nextStepperISR = 0; // Interval until the next FT Motion phase (0 = Now) #endif // Program timer compare for the maximum period, so it does NOT @@ -1586,10 +1590,6 @@ void Stepper::isr() { // Limit the amount of iterations uint8_t max_loops = 10; - #if ENABLED(FT_MOTION) - static uint32_t ftMotion_nextStepperISR = 0U; // Storage for the next ISR for stepping. - #endif - // FT Motion can be toggled if Standard Motion is also active const bool using_ftMotion = ENABLED(NO_STANDARD_MOTION) || TERN0(FT_MOTION, ftMotion.cfg.active); diff --git a/buildroot/tests/rambo b/buildroot/tests/rambo index fcbe648b1d..2730f9f8f8 100755 --- a/buildroot/tests/rambo +++ b/buildroot/tests/rambo @@ -75,6 +75,7 @@ opt_set MOTHERBOARD BOARD_RAMBO EXTRUDERS 0 TEMP_SENSOR_BED 1 TEMP_SENSOR_PROBE AXIS_RELATIVE_MODES '{ false, false, false }' opt_enable REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER FIX_MOUNTED_PROBE Z_SAFE_HOMING \ FT_MOTION FTM_SMOOTHING FTM_HOME_AND_PROBE NO_STANDARD_MOTION +opt_disable FTM_SHAPER_EI FTM_SHAPER_2HEI FTM_SHAPER_3HEI exec_test $1 $2 "Rambo with ZERO EXTRUDERS, heated bed, FT_MOTION" "$3" # From b99f5c3618e0c5e20733da5f151cab2c91474868 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 12 Dec 2025 17:08:39 -0600 Subject: [PATCH 11/17] =?UTF-8?q?=F0=9F=94=A7=20Group=20motion=20condition?= =?UTF-8?q?als?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/inc/Conditionals-4-adv.h | 60 ++++++++++++++++------------- 1 file changed, 33 insertions(+), 27 deletions(-) diff --git a/Marlin/src/inc/Conditionals-4-adv.h b/Marlin/src/inc/Conditionals-4-adv.h index b363ccde11..7e94c617c1 100644 --- a/Marlin/src/inc/Conditionals-4-adv.h +++ b/Marlin/src/inc/Conditionals-4-adv.h @@ -336,16 +336,6 @@ #endif #endif -// Use Junction Deviation for motion if Jerk is disabled -#if DISABLED(CLASSIC_JERK) - #define HAS_JUNCTION_DEVIATION 1 -#endif - -// E jerk exists with JD disabled (of course) but also when Linear Advance is disabled on Delta/SCARA -#if HAS_EXTRUDERS && (ENABLED(CLASSIC_JERK) || (IS_KINEMATIC && DISABLED(LIN_ADVANCE))) - #define HAS_CLASSIC_E_JERK 1 -#endif - // Fixed-Time Motion #if ENABLED(FT_MOTION) #if HAS_X_AXIS @@ -365,40 +355,56 @@ #endif #if ENABLED(NO_STANDARD_MOTION) #define FTM_HOME_AND_PROBE - #undef LIN_ADVANCE - #undef SMOOTH_LIN_ADVANCE - #undef S_CURVE_ACCELERATION - #undef ADAPTIVE_STEP_SMOOTHING - #undef INPUT_SHAPING_X - #undef INPUT_SHAPING_Y - #undef INPUT_SHAPING_E_SYNC - #undef MULTISTEPPING_LIMIT - #define MULTISTEPPING_LIMIT 1 #endif #if ANY(FTM_SHAPER_EI, FTM_SHAPER_2HEI, FTM_SHAPER_3HEI) #define HAS_FTM_EI_SHAPING 1 #endif #endif + +// Standard Motion #if DISABLED(NO_STANDARD_MOTION) #define HAS_STANDARD_MOTION 1 -#endif - -// ZV Input shaping -#if ANY(INPUT_SHAPING_X, INPUT_SHAPING_Y, INPUT_SHAPING_Z) - #define HAS_ZV_SHAPING 1 +#else + #undef LIN_ADVANCE + #undef SMOOTH_LIN_ADVANCE + #undef S_CURVE_ACCELERATION + #undef ADAPTIVE_STEP_SMOOTHING + #undef INPUT_SHAPING_X + #undef INPUT_SHAPING_Y + #undef INPUT_SHAPING_Z + #undef INPUT_SHAPING_E_SYNC + #undef MULTISTEPPING_LIMIT + #define MULTISTEPPING_LIMIT 1 #endif // Linear advance uses Jerk since E is an isolated axis #if ANY(FTM_HAS_LIN_ADVANCE, LIN_ADVANCE) #define HAS_LIN_ADVANCE_K 1 #endif -#if HAS_JUNCTION_DEVIATION && ENABLED(LIN_ADVANCE) - #define HAS_LINEAR_E_JERK 1 -#endif +// Linear Advance without smoothing #if ENABLED(LIN_ADVANCE) && DISABLED(SMOOTH_LIN_ADVANCE) #define HAS_ROUGH_LIN_ADVANCE 1 #endif +// ZV Input Shaping for Standard Motion +#if ANY(INPUT_SHAPING_X, INPUT_SHAPING_Y, INPUT_SHAPING_Z) + #define HAS_ZV_SHAPING 1 +#endif + +// Use Junction Deviation for motion if Jerk is disabled +#if DISABLED(CLASSIC_JERK) + #define HAS_JUNCTION_DEVIATION 1 +#endif + +// E jerk exists with JD disabled (of course) but also when Linear Advance is disabled on Delta/SCARA +#if HAS_EXTRUDERS && (ENABLED(CLASSIC_JERK) || (IS_KINEMATIC && DISABLED(LIN_ADVANCE))) + #define HAS_CLASSIC_E_JERK 1 +#endif +// E jerk is derived from JD factors +#if ALL(HAS_JUNCTION_DEVIATION, LIN_ADVANCE) + #define HAS_LINEAR_E_JERK 1 +#endif + // Some displays can toggle Adaptive Step Smoothing. // The state is saved to EEPROM. // In future this may be added to a G-code such as M205 A. From 89379cd373b2597d0c8cadd6a005de8d640a8273 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 12 Dec 2025 17:09:13 -0600 Subject: [PATCH 12/17] =?UTF-8?q?=F0=9F=94=A8=20Single=20precision=20float?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/HAL/ESP32/HAL.cpp | 2 +- Marlin/src/HAL/ESP32/HAL.h | 1 - Marlin/src/HAL/GD32_MFL/HAL.h | 1 - Marlin/src/HAL/STM32/HAL.h | 2 -- buildroot/share/PlatformIO/scripts/STM32F1_build_flags.py | 1 - buildroot/share/PlatformIO/scripts/common-cxxflags.py | 2 +- ini/esp32.ini | 2 +- ini/gd32.ini | 1 - platformio.ini | 2 +- 9 files changed, 4 insertions(+), 10 deletions(-) diff --git a/Marlin/src/HAL/ESP32/HAL.cpp b/Marlin/src/HAL/ESP32/HAL.cpp index c48aee0e21..acfec29b2f 100644 --- a/Marlin/src/HAL/ESP32/HAL.cpp +++ b/Marlin/src/HAL/ESP32/HAL.cpp @@ -271,7 +271,7 @@ void MarlinHAL::adc_start(const pin_t pin) { uint32_t mv; esp_adc_cal_get_voltage((adc_channel_t)chan, &characteristics[attenuations[chan]], &mv); - adc_result = mv * isr_float_t(1023) / isr_float_t(ADC_REFERENCE_VOLTAGE) / isr_float_t(1000); + adc_result = (mv * 1023) * (1000.f / (ADC_REFERENCE_VOLTAGE)); // Change the attenuation level based on the new reading adc_atten_t atten; diff --git a/Marlin/src/HAL/ESP32/HAL.h b/Marlin/src/HAL/ESP32/HAL.h index cd9e738be3..f48eabea0d 100644 --- a/Marlin/src/HAL/ESP32/HAL.h +++ b/Marlin/src/HAL/ESP32/HAL.h @@ -76,7 +76,6 @@ // Types // ------------------------ -typedef double isr_float_t; // FPU ops are used for single-precision, so use double for ISRs. typedef int16_t pin_t; typedef struct pwm_pin { diff --git a/Marlin/src/HAL/GD32_MFL/HAL.h b/Marlin/src/HAL/GD32_MFL/HAL.h index 56e52b53b5..6195212028 100644 --- a/Marlin/src/HAL/GD32_MFL/HAL.h +++ b/Marlin/src/HAL/GD32_MFL/HAL.h @@ -57,7 +57,6 @@ #define __bss_end __bss_end__ // Types -typedef double isr_float_t; // FPU ops are used for single-precision, so use double for ISRs. typedef uint8_t pin_t; // Parity with mfl platform // Servo diff --git a/Marlin/src/HAL/STM32/HAL.h b/Marlin/src/HAL/STM32/HAL.h index 8f9b56704c..24889c872a 100644 --- a/Marlin/src/HAL/STM32/HAL.h +++ b/Marlin/src/HAL/STM32/HAL.h @@ -63,8 +63,6 @@ // Types // ------------------------ -typedef double isr_float_t; // FPU ops are used for single-precision, so use double for ISRs. - typedef int32_t pin_t; // Parity with platform/ststm32 class libServo; diff --git a/buildroot/share/PlatformIO/scripts/STM32F1_build_flags.py b/buildroot/share/PlatformIO/scripts/STM32F1_build_flags.py index dfce215348..58104c0b45 100755 --- a/buildroot/share/PlatformIO/scripts/STM32F1_build_flags.py +++ b/buildroot/share/PlatformIO/scripts/STM32F1_build_flags.py @@ -19,7 +19,6 @@ if __name__ == "__main__": "-fsigned-char", "-fno-move-loop-invariants", "-fno-strict-aliasing", - "-fsingle-precision-constant", "--specs=nano.specs", "--specs=nosys.specs", diff --git a/buildroot/share/PlatformIO/scripts/common-cxxflags.py b/buildroot/share/PlatformIO/scripts/common-cxxflags.py index 1ec733e59b..00d8abfa35 100644 --- a/buildroot/share/PlatformIO/scripts/common-cxxflags.py +++ b/buildroot/share/PlatformIO/scripts/common-cxxflags.py @@ -14,7 +14,7 @@ if pioutil.is_pio_build(): # "-Wno-sign-compare", "-fno-sized-deallocation" ] - if "teensy" not in env["PIOENV"]: + if "teensy" not in env["PIOENV"] and "esp32" not in env["PIOENV"]: cxxflags += ["-Wno-register"] env.Append(CXXFLAGS=cxxflags) env.Append(CFLAGS=["-Wno-implicit-function-declaration"]) diff --git a/ini/esp32.ini b/ini/esp32.ini index f40b37ef14..1718dc0b87 100644 --- a/ini/esp32.ini +++ b/ini/esp32.ini @@ -17,7 +17,7 @@ platform = espressif32@2.1.0 platform_packages = espressif/toolchain-xtensa-esp32s3 board = esp32dev build_flags = ${common.build_flags} -DCORE_DEBUG_LEVEL=0 -std=gnu++17 -build_unflags = -std=gnu11 -std=gnu++11 +build_unflags = -std=gnu11 -std=gnu++11 -fsingle-precision-constant -Wno-register build_src_filter = ${common.default_src_filter} + lib_ignore = NativeEthernet, AsyncTCP_RP2040W upload_speed = 500000 diff --git a/ini/gd32.ini b/ini/gd32.ini index d9cc3f7082..e0c34ad04f 100644 --- a/ini/gd32.ini +++ b/ini/gd32.ini @@ -39,7 +39,6 @@ build_flags = ${gd32_base.build_flags} -DSS_TIMER=3 -DTIMER_SERVO=4 -DTRANSFER_CLOCK_DIV=8 - -fsingle-precision-constant extra_scripts = ${gd32_base.extra_scripts} buildroot/share/PlatformIO/scripts/offset_and_rename.py monitor_speed = 115200 diff --git a/platformio.ini b/platformio.ini index 995c29ca17..9f562c0be9 100644 --- a/platformio.ini +++ b/platformio.ini @@ -47,7 +47,7 @@ extra_configs = # [common] build_flags = -g3 -D__MARLIN_FIRMWARE__ -DNDEBUG - -fmax-errors=5 + -fmax-errors=5 -fsingle-precision-constant extra_scripts = pre:buildroot/share/PlatformIO/scripts/configuration.py pre:buildroot/share/PlatformIO/scripts/common-dependencies.py From 6f653f5993720d80b4d1ed899ce2a11fa4d60fc4 Mon Sep 17 00:00:00 2001 From: thinkyhead Date: Sat, 13 Dec 2025 00:32:35 +0000 Subject: [PATCH 13/17] [cron] Bump distribution date (2025-12-13) --- Marlin/Version.h | 2 +- Marlin/src/inc/Version.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Marlin/Version.h b/Marlin/Version.h index e2886c1f43..ddd5ea9300 100644 --- a/Marlin/Version.h +++ b/Marlin/Version.h @@ -41,7 +41,7 @@ * here we define this default string as the date where the latest release * version was tagged. */ -//#define STRING_DISTRIBUTION_DATE "2025-12-12" +//#define STRING_DISTRIBUTION_DATE "2025-12-13" /** * The protocol for communication to the host. Protocol indicates communication diff --git a/Marlin/src/inc/Version.h b/Marlin/src/inc/Version.h index f8678dd2cb..6545c4d8fa 100644 --- a/Marlin/src/inc/Version.h +++ b/Marlin/src/inc/Version.h @@ -42,7 +42,7 @@ * version was tagged. */ #ifndef STRING_DISTRIBUTION_DATE - #define STRING_DISTRIBUTION_DATE "2025-12-12" + #define STRING_DISTRIBUTION_DATE "2025-12-13" #endif /** From 02e1199d514ad683e85885fb6720e3e17184655d Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 12 Dec 2025 19:45:15 -0600 Subject: [PATCH 14/17] =?UTF-8?q?=F0=9F=94=A8=20Single=20precision=20float?= =?UTF-8?q?=20(2)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ini/native.ini | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ini/native.ini b/ini/native.ini index 9f241efb2b..67938852ec 100644 --- a/ini/native.ini +++ b/ini/native.ini @@ -17,7 +17,7 @@ platform = native framework = build_flags = ${common.build_flags} -D__PLAT_LINUX__ -std=gnu++17 -ggdb -g -lrt -lpthread -D__MARLIN_FIRMWARE__ -Wno-expansion-to-defined build_src_flags = -Wall -IMarlin/src/HAL/LINUX/include -build_unflags = -Wall +build_unflags = -Wall -fsingle-precision-constant lib_ldf_mode = off build_src_filter = ${common.default_src_filter} + @@ -53,6 +53,7 @@ build_flags = ${common.build_flags} -std=gnu++17 -I/usr/include/SDL2 -IMarlin -IMarlin/src/HAL/NATIVE_SIM/u8g -D__PLAT_NATIVE_SIM__ -DU8G_HAL_LINKS -DGLM_ENABLE_EXPERIMENTAL build_src_flags = -Wall -Wno-expansion-to-defined -Wno-deprecated-declarations -Wcast-align +build_unflags = -fsingle-precision-constant release_flags = -g0 -O3 -flto debug_build_flags = -fstack-protector-strong -g -g3 -ggdb lib_compat_mode = off From bcece89ad037baf77e0340e244478d6de44a7b48 Mon Sep 17 00:00:00 2001 From: thinkyhead Date: Sun, 14 Dec 2025 00:37:23 +0000 Subject: [PATCH 15/17] [cron] Bump distribution date (2025-12-14) --- Marlin/Version.h | 2 +- Marlin/src/inc/Version.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Marlin/Version.h b/Marlin/Version.h index ddd5ea9300..926bddf13f 100644 --- a/Marlin/Version.h +++ b/Marlin/Version.h @@ -41,7 +41,7 @@ * here we define this default string as the date where the latest release * version was tagged. */ -//#define STRING_DISTRIBUTION_DATE "2025-12-13" +//#define STRING_DISTRIBUTION_DATE "2025-12-14" /** * The protocol for communication to the host. Protocol indicates communication diff --git a/Marlin/src/inc/Version.h b/Marlin/src/inc/Version.h index 6545c4d8fa..7584030113 100644 --- a/Marlin/src/inc/Version.h +++ b/Marlin/src/inc/Version.h @@ -42,7 +42,7 @@ * version was tagged. */ #ifndef STRING_DISTRIBUTION_DATE - #define STRING_DISTRIBUTION_DATE "2025-12-13" + #define STRING_DISTRIBUTION_DATE "2025-12-14" #endif /** From 575136a82aca9e8a63454f2f256414128baaba7d Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 13 Dec 2025 20:02:26 -0600 Subject: [PATCH 16/17] =?UTF-8?q?=F0=9F=8E=A8=20Clean=20up=20HAL/timers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/HAL/AVR/timers.h | 3 +-- Marlin/src/HAL/DUE/timers.h | 1 - Marlin/src/HAL/ESP32/timers.h | 1 - Marlin/src/HAL/GD32_MFL/timers.cpp | 7 ++++--- Marlin/src/HAL/GD32_MFL/timers.h | 4 ++-- Marlin/src/HAL/HC32/timers.h | 6 +++--- Marlin/src/HAL/LINUX/timers.h | 1 - Marlin/src/HAL/LPC1768/timers.h | 7 +++---- Marlin/src/HAL/NATIVE_SIM/timers.h | 7 +++---- Marlin/src/HAL/RP2040/timers.h | 1 - Marlin/src/HAL/SAMD21/timers.h | 1 - Marlin/src/HAL/SAMD51/timers.h | 1 - Marlin/src/HAL/STM32/timers.cpp | 2 +- Marlin/src/HAL/STM32/timers.h | 4 ++-- Marlin/src/HAL/STM32F1/timers.cpp | 4 ++-- Marlin/src/HAL/STM32F1/timers.h | 1 - Marlin/src/HAL/TEENSY31_32/timers.h | 3 +-- Marlin/src/HAL/TEENSY35_36/timers.h | 3 +-- Marlin/src/HAL/TEENSY40_41/timers.h | 9 ++++----- 19 files changed, 27 insertions(+), 39 deletions(-) diff --git a/Marlin/src/HAL/AVR/timers.h b/Marlin/src/HAL/AVR/timers.h index 46993ead33..1800da0985 100644 --- a/Marlin/src/HAL/AVR/timers.h +++ b/Marlin/src/HAL/AVR/timers.h @@ -50,10 +50,9 @@ typedef uint16_t hal_timer_t; #define STEPPER_TIMER_RATE HAL_TIMER_RATE #define STEPPER_TIMER_PRESCALE 8 -#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000UL) +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000UL) // (MHz) Stepper Timer ticks per µs #define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define ENABLE_STEPPER_DRIVER_INTERRUPT() SBI(TIMSK1, OCIE1A) diff --git a/Marlin/src/HAL/DUE/timers.h b/Marlin/src/HAL/DUE/timers.h index 6d02033ed5..f892eac8f5 100644 --- a/Marlin/src/HAL/DUE/timers.h +++ b/Marlin/src/HAL/DUE/timers.h @@ -59,7 +59,6 @@ typedef uint32_t hal_timer_t; #define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) #define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) diff --git a/Marlin/src/HAL/ESP32/timers.h b/Marlin/src/HAL/ESP32/timers.h index 2ba034d9b6..d656e92224 100644 --- a/Marlin/src/HAL/ESP32/timers.h +++ b/Marlin/src/HAL/ESP32/timers.h @@ -66,7 +66,6 @@ typedef uint64_t hal_timer_t; #define STEP_TIMER_MIN_INTERVAL 8 // minimum time in µs between stepper interrupts #define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define TONE_TIMER_PRESCALE 1000 // Arbitrary value, no idea what i'm doing here diff --git a/Marlin/src/HAL/GD32_MFL/timers.cpp b/Marlin/src/HAL/GD32_MFL/timers.cpp index 632499742c..13a5c166ef 100644 --- a/Marlin/src/HAL/GD32_MFL/timers.cpp +++ b/Marlin/src/HAL/GD32_MFL/timers.cpp @@ -121,9 +121,10 @@ void HAL_timer_start(const uint8_t timer_number, const uint32_t frequency) { if (is_step) { timer.setPrescaler(STEPPER_TIMER_PRESCALE); - timer.setRolloverValue(_MIN(static_cast(HAL_TIMER_TYPE_MAX), - (HAL_TIMER_RATE) / (STEPPER_TIMER_PRESCALE)), - TimerFormat::TICK); + timer.setRolloverValue( + _MIN(HAL_TIMER_TYPE_MAX, hal_timer_t((HAL_TIMER_RATE) / (STEPPER_TIMER_PRESCALE))), + TimerFormat::TICK + ); is_step_timer_initialized = true; } else { diff --git a/Marlin/src/HAL/GD32_MFL/timers.h b/Marlin/src/HAL/GD32_MFL/timers.h index 00a757d4e0..1c69c0b468 100644 --- a/Marlin/src/HAL/GD32_MFL/timers.h +++ b/Marlin/src/HAL/GD32_MFL/timers.h @@ -47,8 +47,8 @@ extern uint32_t GetStepperTimerClkFreq(); #define STEPPER_TIMER_PRESCALE (GetStepperTimerClkFreq() / STEPPER_TIMER_RATE) // Prescaler = 30 #define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000UL) // (MHz) Stepper Timer ticks per µs -#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US +// Pulse Timer (counter) calculations +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE // Timer interrupt priorities diff --git a/Marlin/src/HAL/HC32/timers.h b/Marlin/src/HAL/HC32/timers.h index 9992033e0e..8a4465e2d8 100644 --- a/Marlin/src/HAL/HC32/timers.h +++ b/Marlin/src/HAL/HC32/timers.h @@ -69,9 +69,9 @@ extern Timer0 step_timer; #define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000UL) // Integer 3 // Pulse timer (== stepper timer) -#define MF_TIMER_PULSE MF_TIMER_STEP -#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US +#define MF_TIMER_PULSE MF_TIMER_STEP +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer +#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE // // HAL functions diff --git a/Marlin/src/HAL/LINUX/timers.h b/Marlin/src/HAL/LINUX/timers.h index ea64f1517d..39c76f02b5 100644 --- a/Marlin/src/HAL/LINUX/timers.h +++ b/Marlin/src/HAL/LINUX/timers.h @@ -56,7 +56,6 @@ typedef uint32_t hal_timer_t; #define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) #define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) diff --git a/Marlin/src/HAL/LPC1768/timers.h b/Marlin/src/HAL/LPC1768/timers.h index 6e158f9290..3aec2418d2 100644 --- a/Marlin/src/HAL/LPC1768/timers.h +++ b/Marlin/src/HAL/LPC1768/timers.h @@ -77,12 +77,11 @@ typedef uint32_t hal_timer_t; #define TEMP_TIMER_RATE 1000000 // 1MHz #define TEMP_TIMER_FREQUENCY 1000 // (Hz) Temperature ISR frequency -#define STEPPER_TIMER_RATE HAL_TIMER_RATE // (Hz) Frequency of stepper timer (HAL_TIMER_RATE / STEPPER_TIMER_PRESCALE) -#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000UL) // (MHz) Stepper Timer ticks per µs -#define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) +#define STEPPER_TIMER_RATE HAL_TIMER_RATE // (Hz) Frequency of stepper timer (HAL_TIMER_RATE / STEPPER_TIMER_PRESCALE) +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000UL) // (MHz) Stepper Timer ticks per µs +#define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) #define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) diff --git a/Marlin/src/HAL/NATIVE_SIM/timers.h b/Marlin/src/HAL/NATIVE_SIM/timers.h index af3e5ff992..be59bb8676 100644 --- a/Marlin/src/HAL/NATIVE_SIM/timers.h +++ b/Marlin/src/HAL/NATIVE_SIM/timers.h @@ -55,12 +55,11 @@ typedef uint64_t hal_timer_t; #define TEMP_TIMER_RATE 1'000'000 // (Hz) Temperature Timer count rate #define TEMP_TIMER_FREQUENCY 1000 // (Hz) Temperature ISR call frequency -#define STEPPER_TIMER_RATE HAL_TIMER_RATE // (Hz) Frequency of Stepper Timer (HAL_TIMER_RATE / STEPPER_TIMER_PRESCALE) -#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1'000'000) // (MHz) Stepper Timer ticks per µs -#define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) +#define STEPPER_TIMER_RATE HAL_TIMER_RATE // (Hz) Frequency of Stepper Timer (HAL_TIMER_RATE / STEPPER_TIMER_PRESCALE) +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1'000'000) // (MHz) Stepper Timer ticks per µs +#define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) #define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) diff --git a/Marlin/src/HAL/RP2040/timers.h b/Marlin/src/HAL/RP2040/timers.h index fd078ae4a9..8d9fde57a8 100644 --- a/Marlin/src/HAL/RP2040/timers.h +++ b/Marlin/src/HAL/RP2040/timers.h @@ -65,7 +65,6 @@ typedef uint64_t hal_timer_t; #define STEPPER_TIMER_PRESCALE (10) #define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) diff --git a/Marlin/src/HAL/SAMD21/timers.h b/Marlin/src/HAL/SAMD21/timers.h index 55034459c3..dfae605595 100644 --- a/Marlin/src/HAL/SAMD21/timers.h +++ b/Marlin/src/HAL/SAMD21/timers.h @@ -56,7 +56,6 @@ typedef uint32_t hal_timer_t; #define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) #define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) diff --git a/Marlin/src/HAL/SAMD51/timers.h b/Marlin/src/HAL/SAMD51/timers.h index b69c131406..2682891cdc 100644 --- a/Marlin/src/HAL/SAMD51/timers.h +++ b/Marlin/src/HAL/SAMD51/timers.h @@ -55,7 +55,6 @@ typedef uint32_t hal_timer_t; #define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) #define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) diff --git a/Marlin/src/HAL/STM32/timers.cpp b/Marlin/src/HAL/STM32/timers.cpp index 10e0dc43a4..b4271d3a58 100644 --- a/Marlin/src/HAL/STM32/timers.cpp +++ b/Marlin/src/HAL/STM32/timers.cpp @@ -141,7 +141,7 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { */ timer_instance[timer_num]->setPrescaleFactor(STEPPER_TIMER_PRESCALE); //the -1 is done internally - timer_instance[timer_num]->setOverflow(_MIN(hal_timer_t(HAL_TIMER_TYPE_MAX), (HAL_TIMER_RATE) / (STEPPER_TIMER_PRESCALE) /* /frequency */), TICK_FORMAT); + timer_instance[timer_num]->setOverflow(_MIN(HAL_TIMER_TYPE_MAX, hal_timer_t((HAL_TIMER_RATE) / (STEPPER_TIMER_PRESCALE) /* / frequency */)), TICK_FORMAT); break; case MF_TIMER_TEMP: // TEMP TIMER - any available 16bit timer timer_instance[timer_num] = new HardwareTimer(TEMP_TIMER_DEV); diff --git a/Marlin/src/HAL/STM32/timers.h b/Marlin/src/HAL/STM32/timers.h index e931daa72e..7b55f8a52d 100644 --- a/Marlin/src/HAL/STM32/timers.h +++ b/Marlin/src/HAL/STM32/timers.h @@ -56,8 +56,8 @@ extern uint32_t GetStepperTimerClkFreq(); #define STEPPER_TIMER_PRESCALE (GetStepperTimerClkFreq() / (STEPPER_TIMER_RATE)) #define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000UL) // (MHz) Stepper Timer ticks per µs -#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US +// Pulse Timer (counter) calculations +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) diff --git a/Marlin/src/HAL/STM32F1/timers.cpp b/Marlin/src/HAL/STM32F1/timers.cpp index 12477a4583..141dc80b80 100644 --- a/Marlin/src/HAL/STM32F1/timers.cpp +++ b/Marlin/src/HAL/STM32F1/timers.cpp @@ -84,7 +84,7 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { timer_set_prescaler(STEP_TIMER_DEV, (uint16_t)(STEPPER_TIMER_PRESCALE - 1)); timer_set_reload(STEP_TIMER_DEV, 0xFFFF); timer_oc_set_mode(STEP_TIMER_DEV, STEP_TIMER_CHAN, TIMER_OC_MODE_FROZEN, TIMER_OC_NO_PRELOAD); // no output pin change - timer_set_compare(STEP_TIMER_DEV, STEP_TIMER_CHAN, _MIN(hal_timer_t(HAL_TIMER_TYPE_MAX), (STEPPER_TIMER_RATE) / frequency)); + timer_set_compare(STEP_TIMER_DEV, STEP_TIMER_CHAN, _MIN(HAL_TIMER_TYPE_MAX, hal_timer_t((STEPPER_TIMER_RATE) / frequency))); timer_no_ARR_preload_ARPE(STEP_TIMER_DEV); // Need to be sure no preload on ARR register timer_attach_interrupt(STEP_TIMER_DEV, STEP_TIMER_CHAN, stepTC_Handler); HAL_timer_set_interrupt_priority(MF_TIMER_STEP, STEP_TIMER_IRQ_PRIO); @@ -97,7 +97,7 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { timer_set_count(TEMP_TIMER_DEV, 0); timer_set_prescaler(TEMP_TIMER_DEV, (uint16_t)(TEMP_TIMER_PRESCALE - 1)); timer_set_reload(TEMP_TIMER_DEV, 0xFFFF); - timer_set_compare(TEMP_TIMER_DEV, TEMP_TIMER_CHAN, _MIN(hal_timer_t(HAL_TIMER_TYPE_MAX), (F_CPU) / (TEMP_TIMER_PRESCALE) / frequency)); + timer_set_compare(TEMP_TIMER_DEV, TEMP_TIMER_CHAN, _MIN(HAL_TIMER_TYPE_MAX, hal_timer_t((F_CPU) / (TEMP_TIMER_PRESCALE) / frequency))); timer_attach_interrupt(TEMP_TIMER_DEV, TEMP_TIMER_CHAN, tempTC_Handler); HAL_timer_set_interrupt_priority(MF_TIMER_TEMP, TEMP_TIMER_IRQ_PRIO); timer_generate_update(TEMP_TIMER_DEV); diff --git a/Marlin/src/HAL/STM32F1/timers.h b/Marlin/src/HAL/STM32F1/timers.h index 2aee309c38..3d6650a1a2 100644 --- a/Marlin/src/HAL/STM32F1/timers.h +++ b/Marlin/src/HAL/STM32F1/timers.h @@ -103,7 +103,6 @@ typedef uint16_t hal_timer_t; #define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000UL) // (MHz) Stepper Timer ticks per µs #define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE timer_dev* HAL_get_timer_dev(int number); diff --git a/Marlin/src/HAL/TEENSY31_32/timers.h b/Marlin/src/HAL/TEENSY31_32/timers.h index c22ffc21f9..cdca84ec1c 100644 --- a/Marlin/src/HAL/TEENSY31_32/timers.h +++ b/Marlin/src/HAL/TEENSY31_32/timers.h @@ -59,11 +59,10 @@ typedef uint32_t hal_timer_t; #define TEMP_TIMER_FREQUENCY 1000 #define STEPPER_TIMER_RATE HAL_TIMER_RATE -#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000UL) +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000UL) // (MHz) Stepper Timer ticks per µs #define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) #define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) diff --git a/Marlin/src/HAL/TEENSY35_36/timers.h b/Marlin/src/HAL/TEENSY35_36/timers.h index 9c55af6228..1a5a8460c6 100644 --- a/Marlin/src/HAL/TEENSY35_36/timers.h +++ b/Marlin/src/HAL/TEENSY35_36/timers.h @@ -59,11 +59,10 @@ typedef uint32_t hal_timer_t; #define TEMP_TIMER_FREQUENCY 1000 #define STEPPER_TIMER_RATE HAL_TIMER_RATE -#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000UL) +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000UL) // (MHz) Stepper Timer ticks per µs #define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) #define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) diff --git a/Marlin/src/HAL/TEENSY40_41/timers.h b/Marlin/src/HAL/TEENSY40_41/timers.h index 14a9587486..66d00b1788 100644 --- a/Marlin/src/HAL/TEENSY40_41/timers.h +++ b/Marlin/src/HAL/TEENSY40_41/timers.h @@ -57,13 +57,12 @@ typedef uint32_t hal_timer_t; #define TEMP_TIMER_RATE 1000000 #define TEMP_TIMER_FREQUENCY 1000 -#define HAL_TIMER_RATE GPT1_TIMER_RATE -#define STEPPER_TIMER_RATE HAL_TIMER_RATE -#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000UL) -#define STEPPER_TIMER_PRESCALE (GPT_TIMER_RATE / STEPPER_TIMER_RATE) +#define HAL_TIMER_RATE GPT1_TIMER_RATE +#define STEPPER_TIMER_RATE HAL_TIMER_RATE +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000UL) +#define STEPPER_TIMER_PRESCALE (GPT_TIMER_RATE / STEPPER_TIMER_RATE) #define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) From f74713da0cd6ee13de43062b7211d0271859754e Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 13 Dec 2025 20:02:38 -0600 Subject: [PATCH 17/17] =?UTF-8?q?=F0=9F=93=9D=20Specify=20mm=20for=20NOZZL?= =?UTF-8?q?E=5FTO=5FPROBE=5FOFFSET?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/Configuration.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h index ea307f126d..bd07add891 100644 --- a/Marlin/Configuration.h +++ b/Marlin/Configuration.h @@ -1661,7 +1661,7 @@ * Nozzle-to-Probe offsets { X, Y, Z } * * X and Y offset - * Use a caliper or ruler to measure the distance from the tip of + * Use a caliper or ruler to measure the distance (in mm) from the tip of * the Nozzle to the center-point of the Probe in the X and Y axes. * * Z offset @@ -1697,7 +1697,7 @@ * | [-] | * O-- FRONT --+ */ -#define NOZZLE_TO_PROBE_OFFSET { 10, 10, 0 } +#define NOZZLE_TO_PROBE_OFFSET { 10, 10, 0 } // (mm) X, Y, Z distance from Nozzle tip to Probe trigger-point // Enable and set to use a specific tool for probing. Disable to allow any tool. #define PROBING_TOOL 0